Modified Least Square Method Fit Circle from Data

template<typename _tp>
struct Circle_
{
    _tp x;
    _tp y;
    _tp r;
};

typedef Circle_<float> Circle3f;

// This function is based on Modified Least Square Methods from Paper
// "A Few Methods for Fitting Circles to Data".
void FitCircle(const std::vector<cv::Point2f> &vecPoints, Circle3f &circle)
{
    double Sx = 0., Sy = 0., Sxx = 0., Sx2 = 0., Sy2 = 0., Sxy = 0., Syy = 0., Sx3 = 0., Sy3 = 0., Sxy2 = 0., Syx2 = 0.;
    for ( const auto &point : vecPoints )   {
        Sx   += point.x;
        Sy   += point.y;
        Sx2  += point.x * point.x;
        Sy2  += point.y * point.y;
        Sxy  += point.x * point.y;
        Sx3  += point.x * point.x * point.x;
        Sy3  += point.y * point.y * point.y;
        Sxy2 += point.x * point.y * point.y;
        Syx2 += point.y * point.x * point.x;
    }

    double A, B, C, D, E;
    int n = vecPoints.size();
    A = n * Sx2 - Sx * Sx;
    B = n * Sxy - Sx * Sy;
    C = n * Sy2 - Sy * Sy;
    D = 0.5 * ( n * Sxy2 - Sx * Sy2 + n * Sx3 - Sx * Sx2 );
    E = 0.5 * ( n * Syx2 - Sy * Sx2 + n * Sy3 - Sy * Sy2 );

    auto AC_B2 = ( A * C - B * B);  // The variable name is from AC - B^2
    auto am = ( D * C - B * E ) / AC_B2;
    auto bm = ( A * E - B * D ) / AC_B2;

    double rSqureSum = 0.f;
    for ( const auto &point : vecPoints )
    {
        rSqureSum += sqrt ( ( point.x - am ) * ( point.x - am ) + ( point.y - bm) * ( point.y - bm) );
    }
    auto r = rSqureSum / n;
    circle.x = static_cast<float>( am );
    circle.y = static_cast<float>( bm );
    circle.r = static_cast<float>( r );
}

void TestFitCircle()
{
    std::vector<cv::Point2f> vecPoints;
    vecPoints.push_back(cv::Point2f(0, 10));
    vecPoints.push_back(cv::Point2f(0.1f, 10.1f));
    vecPoints.push_back(cv::Point2f(10, 0));
    vecPoints.push_back(cv::Point2f(10, 20));
    vecPoints.push_back(cv::Point2f(20, 10));

    Circle3f circle; 
    FitCircle(vecPoints, circle);

    cout << "X, Y " <<circle.x << ", " << circle.y << "    r " << circle.r << endl;
}