Modified Least Square Method and Ransan Method to Fit Circle from Data

In OpenCv, it only provide the function fitEllipse to fit Ellipse, but doesn't provide function to fit circle, so i read some paper, and write a function to do it. The paper it refer to is "A Few Methods for Fitting Circles to Data".

Also when there is a lot of noise in the data, need to find a way to exclude the noise data and get a more accuracy result.

There are two methods, one is iterate method, first use all the data to fit a model, then find the points exceed the error tolerance to the model, excude them and fit again. Until reach iteration times limit or all the data is in error tolerance.

Another method is ransac method, it has detailed introduction in paper "Random Sample Consensus: A Paradigm for Model Fitting with Apphcatlons to Image Analysis and Automated Cartography".

// This function is based on Modified Least Square Methods from Paper
// "A Few Methods for Fitting Circles to Data".
cv::RotatedRect FitCircle(const std::vector<cv::Point2f> &vecPoints)
{
    cv::RotatedRect rotatedRect;
    if ( vecPoints.size() < 3 )
        return rotatedRect;

    double Sx = 0., Sy = 0., Sx2 = 0., Sy2 = 0., Sxy = 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) );
    }
    float r = static_cast<float>( rSqureSum / n );
    rotatedRect.center.x = static_cast<float>( am );
    rotatedRect.center.y = static_cast<float>( bm );
    rotatedRect.size = cv::Size2f( 2.f * r,  2.f * r  );

    return rotatedRect;
}

std::vector<size_t> findPointOverTol( const std::vector<cv::Point2f> &vecPoints, const cv::RotatedRect &rotatedRect, int method, float tolerance )
{
    std::vector<size_t> vecResult;
    for ( size_t i = 0;i < vecPoints.size(); ++ i )  {
        cv::Point2f point = vecPoints[i];
        auto disToCtr = sqrt( ( point.x - rotatedRect.center.x) * (point.x - rotatedRect.center.x)  + ( point.y - rotatedRect.center.y) * ( point.y - rotatedRect.center.y ) );
        auto err = disToCtr - rotatedRect.size.width / 2;
        if ( method == 1 && fabs ( err ) > tolerance )  {
            vecResult.push_back(i);
        }else if ( method == 2 && err > tolerance ) {
            vecResult.push_back(i);
        }else if ( method == 3 && err < -tolerance ) {
            vecResult.push_back(i);
        }
    }
    return vecResult;
}

//method 1 : Exclude all the points out of positive error tolerance and inside the negative error tolerance.
//method 2 : Exclude all the points out of positive error tolerance.
//method 3 : Exclude all the points inside the negative error tolerance.
cv::RotatedRect FitCircleIterate(const std::vector<cv::Point2f> &vecPoints, int method, float tolerance)
{
    cv::RotatedRect rotatedRect;
    if (vecPoints.size() < 3)
        return rotatedRect;

    std::vector<cv::Point2f> vecLocalPoints;
    for ( const auto &point : vecPoints )   {
        vecLocalPoints.push_back ( point );
    }
    rotatedRect = FitCircle ( vecLocalPoints );
    
    std::vector<size_t> overTolPoints = findPointOverTol ( vecLocalPoints, rotatedRect, method, tolerance );
    int nIteratorNum = 1;
    while ( ! overTolPoints.empty() && nIteratorNum < 20 )   {
        for (auto it = overTolPoints.rbegin(); it != overTolPoints.rend(); ++ it)
            vecLocalPoints.erase(vecLocalPoints.begin() + *it);
        rotatedRect = FitCircle ( vecLocalPoints );
        overTolPoints = findPointOverTol ( vecLocalPoints, rotatedRect, method, tolerance );
        ++ nIteratorNum;
    }
    return rotatedRect;
}

std::vector<cv::Point2f> randomSelectPoints(const std::vector<cv::Point2f> &vecPoints, int needToSelect)
{    
    std::set<int> setResult;
    std::vector<cv::Point2f> vecResultPoints;

    if ( (int)vecPoints.size() < needToSelect )
        return vecResultPoints;

    while ((int)setResult.size() < needToSelect)   {
        int nValue = std::rand() % vecPoints.size();
        setResult.insert(nValue);        
    }
    for ( auto index : setResult )
        vecResultPoints.push_back ( vecPoints[index] );
    return vecResultPoints;
}

std::vector<cv::Point2f> findPointsInTol( const std::vector<cv::Point2f> &vecPoints, const cv::RotatedRect &rotatedRect, float tolerance )
{
    std::vector<cv::Point2f> vecResult;
    for ( const auto &point : vecPoints )  {
        auto disToCtr = sqrt( ( point.x - rotatedRect.center.x) * (point.x - rotatedRect.center.x)  + ( point.y - rotatedRect.center.y) * ( point.y - rotatedRect.center.y ) );
        auto err = disToCtr - rotatedRect.size.width / 2;
        if ( fabs ( err ) < tolerance )  {
            vecResult.push_back(point);
        }
    }
    return vecResult;
}

/* The ransac algorithm is from paper "Random Sample Consensus: A Paradigm for Model Fitting with Apphcatlons to Image Analysis and Automated Cartography".
*  Given a model that requires a minimum of n data points to instantiate
* its free parameters, and a set of data points P such that the number of
* points in P is greater than n, randomly select a subset S1 of
* n data points from P and instantiate the model. Use the instantiated
* model M1 to determine the subset S1* of points in P that are within
* some error tolerance of M1. The set S1* is called the consensus set of
* S1.
* If g (S1*) is greater than some threshold t, which is a function of the
* estimate of the number of gross errors in P, use S1* to compute
* (possibly using least squares) a new model M1* and return.
* If g (S1*) is less than t, randomly select a new subset S2 and repeat the
* above process. If, after some predetermined number of trials, no
* consensus set with t or more members has been found, either solve the
* model with the largest consensus set found, or terminate in failure. */
cv::RotatedRect FitCircleRansac(const std::vector<cv::Point2f> &vecPoints, float tolerance, int maxRansacTime, int nFinishThreshold)
{   
    cv::RotatedRect fitResult;
    if (vecPoints.size() < 3)
        return fitResult;

    int nRansacTime = 0;
    const int RANSAC_CIRCLE_POINT = 3;
    size_t nMaxConsentNum = 0;

    while ( nRansacTime < maxRansacTime )   {
        std::vector<cv::Point2f> vecSelectedPoints = randomSelectPoints ( vecPoints, RANSAC_CIRCLE_POINT );
        cv::RotatedRect rectReult = FitCircle ( vecSelectedPoints );
        vecSelectedPoints = findPointsInTol ( vecPoints, rectReult, tolerance );

        if ( vecSelectedPoints.size() >= (size_t)nFinishThreshold ) {
           return FitCircle ( vecSelectedPoints );
        }
        else if ( vecSelectedPoints.size() > nMaxConsentNum )
        {
            fitResult = FitCircle ( vecSelectedPoints );
            nMaxConsentNum = vecSelectedPoints.size();
        }
        ++ nRansacTime;
    }

    return fitResult;
}

void TestFitCircle()
{
    std::vector<cv::Point2f> vecPoints;
    vecPoints.push_back(cv::Point2f(0, 10));
    vecPoints.push_back(cv::Point2f(2.9f, 2.9f));
    vecPoints.push_back(cv::Point2f(17.07f, 17.07f));
    vecPoints.push_back(cv::Point2f(3.f, 8.f));
    vecPoints.push_back(cv::Point2f(10, 0));
    vecPoints.push_back(cv::Point2f(10, 20));
    vecPoints.push_back(cv::Point2f(11, 18));
    vecPoints.push_back(cv::Point2f(20, 10));
    vecPoints.push_back(cv::Point2f(28, 18));
    vecPoints.push_back(cv::Point2f(17, 9));

    cv::RotatedRect rectResult;
    rectResult = FitCircle(vecPoints);

    cout << "                X, Y " << rectResult.center.x << ", " << rectResult.center.y << "    r " << rectResult.size.width / 2. << endl;

    rectResult = FitCircleIterate ( vecPoints, 2, 2 );
    cout << "Iterator Result X, Y " << rectResult.center.x << ", " << rectResult.center.y << "    r " << rectResult.size.width / 2. << endl;

    rectResult = FitCircleRansac ( vecPoints, 1, 20, 7 );
    cout << "Ransac   Result X, Y " << rectResult.center.x << ", " << rectResult.center.y << "    r " << rectResult.size.width / 2. << endl;
}