椭圆拟合算法实现

椭圆的目标函数：

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

得到线性方程组：

|A1B1C1D1E1||A|=|resul1|

|A2B2C2D2E2||B|=|resul2|

|A3B3C3D3E3||C|=|resul3|

|A4B4C4D4E4||D|=|resul4|

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

vectorgetEllipsepar(vectorvec_point)

{

vectorvec_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

{

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;

}