OpenCV最小二成拟合椭圆算法

OpenCV最小二成拟合椭圆算法

//传入样本点，返回椭圆的5个参数

vector<double> getEllipsepar(vector<CvPoint> vec_point)
{
    vector<double> vec_result;
    double  x3y1 = 0,x1y3= 0,x2y2= 0,yyy4= 0, xxx3= 0,xxx2= 0,x2y1= 0,yyy3= 0,x1y2= 0 ,yyy2= 0,x1y1= 0,xxx1= 0,yyy1= 0;

    int N = vec_point.size();
    cout << N << endl;
    for (int m_i = 0;m_i < N ;++m_i )
    {
        double xi = vec_point[m_i].x ;
        double yi = vec_point[m_i].y;
        x3y1   += xi*xi*xi*yi ;
        x1y3   += xi*yi*yi*yi;
        x2y2   += xi*xi*yi*yi; ;
        yyy4   +=yi*yi*yi*yi;
        xxx3   += xi*xi*xi ;
        xxx2   += xi*xi ;
        x2y1 += xi*xi*yi;

        x1y2 += xi*yi*yi;
        yyy2 += yi*yi;
        x1y1 += xi*yi;
        xxx1 += xi;
        yyy1 += yi;
        yyy3 += yi*yi*yi;
    }

    long double resul1 = -(x3y1);
    long double resul2 = -(x2y2);
    long double resul3 = -(xxx3);
    long double resul4 = -(x2y1);
    long double resul5 = -(xxx2);
    long double B1 = x1y3,     C1 = x2y1,  D1 = x1y2,  E1 = x1y1,  A1 = x2y2;
    long double B2 = yyy4,     C2 = x1y2,  D2 = yyy3,  E2 = yyy2,  A2 = x1y3;
    long double B3 = x1y2,     C3 = xxx2,  D3 = x1y1,  E3 = xxx1,  A3 = x2y1;
    long double B4 = yyy3,     C4 = x1y1,  D4 = yyy2,  E4 = yyy1,  A4 = x1y2;
    long double B5 = yyy2,     C5 = xxx1,  D5 = yyy1,  E5 = N,     A5 = x1y1;

//
    CvMat* Ma    = cvCreateMat(5,5,CV_64FC1);
    CvMat* Md    = cvCreateMat(5,1,CV_64FC1);
    CvMat* Mb    = cvCreateMat(5,1,CV_64FC1);
//

    cvmSet(Mb,0,0,resul1);
    cvmSet(Mb,1,0,resul2);
    cvmSet(Mb,2,0,resul3);
    cvmSet(Mb,3,0,resul4);
    cvmSet(Mb,4,0,resul5);

    cvmSet(Ma,0,0,A1);
    cvmSet(Ma,0,1,B1);
    cvmSet(Ma,0,2,C1);
    cvmSet(Ma,0,3,D1);
    cvmSet(Ma,0,4,E1);

    cvmSet(Ma,1,0,A2);
    cvmSet(Ma,1,1,B2);
    cvmSet(Ma,1,2,C2);
    cvmSet(Ma,1,3,D2);
    cvmSet(Ma,1,4,E2);

    cvmSet(Ma,2,0,A3);
    cvmSet(Ma,2,1,B3);
    cvmSet(Ma,2,2,C3);
    cvmSet(Ma,2,3,D3);
    cvmSet(Ma,2,4,E3);

    cvmSet(Ma,3,0,A4);
    cvmSet(Ma,3,1,B4);
    cvmSet(Ma,3,2,C4);
    cvmSet(Ma,3,3,D4);
    cvmSet(Ma,3,4,E4);

    cvmSet(Ma,4,0,A5);
    cvmSet(Ma,4,1,B5);
    cvmSet(Ma,4,2,C5);
    cvmSet(Ma,4,3,D5);
    cvmSet(Ma,4,4,E5);

    cvSolve(Ma, Mb, Md);
    long double A = cvmGet(Md,0,0);
    long double B = cvmGet(Md,1,0);
    long double C = cvmGet(Md,2,0);
    long double D = cvmGet(Md,3,0);
    long double E = cvmGet(Md,4,0);

    double XC = (2*B*C - A*D) /(A*A-4*B);
    double YC =(2*D-A*D)/(A*A-4*B);

    long double fenzi = 2*(A*C*D-B*C*C-D*D+4*E*B-A*A*E);

    long double fenmu =(A*A-4*B)   * (B- sqrt(A*A+ (1-B) * (1-B) )  +1);
    long double femmu2 =(A*A-4*B)  * (B+ sqrt(A*A+ (1-B) * (1-B) )  +1);
    double XA =sqrt(fabs(fenzi/fenmu));
    double XB =sqrt(fabs(fenzi/femmu2));
    double Xtheta =atan(sqrt((XA*XA-XB*XB*B)/(XA*XA*B-XB*XB))+0.0001)*180/3.1415926;

    vec_result.push_back(XC);
    vec_result.push_back(YC);
    vec_result.push_back(XA);
    vec_result.push_back(XB);
    vec_result.push_back(Xtheta);
    return vec_result;
}