最小二乘圆拟合

/***********************************************************************
int fit_circle(CvPoint* points, int num, double * A, double * B, double *R)
Function:polyfit circle
input parameter:
CvPoint* points
int num
double * A
double * B
double *R
output parameter:
0：    success
other：error
Date：2013.04.23
Author：LiuYaqiang
***********************************************************************/
int fit_circle(CvPoint* points, int num, double * A, double * B, double *R)
{
    int i;
    double X1, X2, X3, Y1, Y2, Y3, X1Y1, X1Y2, X2Y1;
    double C, D, E, G, H, N;
    double a, b,c;
    //拟合数据数量判断
    if (num < 3){
        printf("Error: fit data number is less than 3!\n");
        return -1;
    }


    X1 = X2 = X3 = Y1 = Y2 = Y3 = X1Y1 = X1Y2 = X2Y1 = 0;


    for (i = 0; i < num; i++){
        X1 = X1 + points[i].x;
        Y1 = Y1 + points[i].y;
        X2 = X2 + points[i].x * points[i].x;
        Y2 = Y2 + points[i].y * points[i].y;
        X3 = X3 + points[i].x * points[i].x*points[i].x;
        Y3 = Y3 + points[i].y * points[i].y*points[i].y;
        X1Y1 = X1Y1 + points[i].x * points[i].y;
        X1Y2 = X1Y2 + points[i].x * points[i].y*points[i].y;
        X2Y1 = X2Y1 + points[i].x * points[i].x*points[i].y;
    }


    N = num;
    C = N * X2 - X1 * X1;
    D = N * X1Y1 - X1 * Y1;
    E = N * X3 + N * X1Y2 - (X2 + Y2) * X1;
    G = N * Y2 - Y1 * Y1;
    H = N * X2Y1 + N * Y3 - (X2 + Y2) * Y1;


    a = (H * D-E * G) / (C * G - D * D);
    b = (H * C - E * D) / (D * D - G * C);
    c = -(a * X1 + b * Y1 + X2 + Y2)/N;


    *A = a / (-2);
    *B = b / (-2);
    *R = sqrt(a * a + b * b - 4 * c) / 2;
//printf("End:a=%f\tb=%f\tc=%f\n",a,b,c);
    return 0;
}