最小二乘椭圆拟合算法实现 opencv

椭圆的目标函数：
F(A,B,C,D,E) = XiGeMa(xi^2 + A*xiyi+B*yi^2+C*xi+D*yi+E)^2
分别对A,B,C,D,E 求一阶偏导 并令其等于0
得到线性方程组：

|A1 B1 C1 D1 E1 | |A| = |resul1|
|A2 B2 C2 D2 E2 | |B| = |resul2|
|A3 B3 C3 D3 E3 | |C| = |resul3|
|A4 B4 C4 D4 E4 | |D| = |resul4|
|A5 B5 C5 D5 E5 | |E| = |resul5|

求得A,B,C,D,E.
椭圆的五个参数：
center.x = (2*B*C - A*D) /(A*A-4*B);
center.y = (2*D-A*D)/(A*A-4*B);
fenzi = 2*(A*C*D-B*C*C-D*D+4*E*B-A*A*E);
fenmu =(A*A-4*B) * (B- sqrt(A*A+ (1-B) * (1-B) ) +1);
femmu2 =(A*A-4*B) * (B+ sqrt(A*A+ (1-B) * (1-B) ) +1);
long =sqrt(fabs(fenzi/fenmu));
short =sqrt(fabs(fenzi/femmu2));
theta = atan(sqrt((center.x *center.x -center.y*center.y*B)/(center.x *center.x *B-center.y*center.y))+0.0001)*180/cv_pi;;



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();

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;
}