最小二乘椭圆拟合

感谢原作者的分享

直接上代码：

.h文件为：

[cpp] view plain copy

print ?

1. //在这里，我实现了两种算法，一种是  
2. //http://wenku.baidu.com/link?url=7kIrC8LoOMCtlmAH8yqkpUQfiKwWnVe4EoUJekkQSgQ1qTWfLAuEXTYvYTv7SATGIJYX4IxcTIB94-iO0SpUgztWgx661O2VEOwm_dvoSqO  
3. //这篇文章给出的，核心也是最小二乘法，利用gauss消去法解方程组，不过他给出的代码有些小bug，所以我改了一下，也去掉了opencv的东西。  
4. //  还有一个就是利用奇异值分解法来求超定方程的最小二乘法的思想来求出椭圆的五个参数，关于奇异值分解法可以参考  
5. //http://blog.youkuaiyun.com/wangzhiqing3/article/details/7446444  
6.   
7.   
8. /************************************************************************* 
9.     版本：     2014-12-31 
10.     功能说明： 对平面上的一些列点给出最小二乘的椭圆拟合，利用奇异值分解法 
11.     解得最小二乘解作为椭圆参数。 
12.     调用形式： cvFitEllipse2f（arrayx,arrayy,box）；     
13.     参数说明： arrayx: arrayx[n],每个值为x轴一个点 
14. arrayx: arrayy[n],每个值为y轴一个点 
15. n     : 点的个数 
16. box   : box[5],椭圆的五个参数，分别为center.x,center.y,2a,2b,xtheta 
17. esp: 解精度，通常取1e-6,这个是解方程用的说 
18.      ***************************************************************************/  
19.   
20.   
21.   
22.   
23. #include"stdafx.h"  
24. #include<cstdlib>  
25. #include<float.h>  
26. #include<vector>  
27.      using namespace std;  
28.   
29. class LSEllipse  
30. {  
31. public:  
32.     LSEllipse(void);  
33.     ~LSEllipse(void);  
34.     vector<double> getEllipseparGauss(vector<CPoint> vec_point);  
35.     void cvFitEllipse2f( int *arrayx, int *arrayy,int n,float *box );  
36. private:  
37.     int SVD(float *a,int m,int n,float b[],float x[],float esp);  
38.     int gmiv(float a[],int m,int n,float b[],float x[],float aa[],float eps,float u[],float v[],int ka);  
39.     int ginv(float a[],int m,int n,float aa[],float eps,float u[],float v[],int ka);  
40.     int muav(float a[],int m,int n,float u[],float v[],float eps,int ka);  
41. };  

//在这里，我实现了两种算法，一种是
//http://wenku.baidu.com/link?url=7kIrC8LoOMCtlmAH8yqkpUQfiKwWnVe4EoUJekkQSgQ1qTWfLAuEXTYvYTv7SATGIJYX4IxcTIB94-iO0SpUgztWgx661O2VEOwm_dvoSqO
//这篇文章给出的，核心也是最小二乘法，利用gauss消去法解方程组，不过他给出的代码有些小bug，所以我改了一下，也去掉了opencv的东西。
//	还有一个就是利用奇异值分解法来求超定方程的最小二乘法的思想来求出椭圆的五个参数，关于奇异值分解法可以参考
//http://blog.youkuaiyun.com/wangzhiqing3/article/details/7446444


/*************************************************************************
	版本：     2014-12-31
	功能说明： 对平面上的一些列点给出最小二乘的椭圆拟合，利用奇异值分解法
	解得最小二乘解作为椭圆参数。
	调用形式： cvFitEllipse2f（arrayx,arrayy,box）；    
	参数说明： arrayx: arrayx[n],每个值为x轴一个点
arrayx: arrayy[n],每个值为y轴一个点
n     : 点的个数
box   : box[5],椭圆的五个参数，分别为center.x,center.y,2a,2b,xtheta
esp: 解精度，通常取1e-6,这个是解方程用的说
	 ***************************************************************************/




#include"stdafx.h"
#include<cstdlib>
#include<float.h>
#include<vector>
	 using namespace std;

class LSEllipse
{
public:
	LSEllipse(void);
	~LSEllipse(void);
	vector<double> getEllipseparGauss(vector<CPoint> vec_point);
	void cvFitEllipse2f( int *arrayx, int *arrayy,int n,float *box );
private:
	int SVD(float *a,int m,int n,float b[],float x[],float esp);
	int gmiv(float a[],int m,int n,float b[],float x[],float aa[],float eps,float u[],float v[],int ka);
	int ginv(float a[],int m,int n,float aa[],float eps,float u[],float v[],int ka);
	int muav(float a[],int m,int n,float u[],float v[],float eps,int ka);
};

.cpp文件为：

[cpp] view plain copy

print ?

1. //点A (a,b)为椭圆上的一点，θ为OA与X轴的夹角，则参数方程为  
2. //x=acosθ  
3. //y=bsinθ  
4. //x^2/a^2+y^2/b^2=1为标准方程  
5.   
6.   
7. #include"stdafx.h"  
8. #include "LSEllipse.h"  
9. #include <cmath>  
10.   
11. LSEllipse::LSEllipse(void)  
12. {  
13. }  
14.   
15.   
16. LSEllipse::~LSEllipse(void)  
17. {  
18. }  
19. //列主元高斯消去法  
20. //A为系数矩阵，x为解向量，若成功，返回true，否则返回false，并将x清空。  
21.   
22. bool RGauss(const vector<vector<double> > & A, vector<double> & x)  
23. {  
24.     x.clear();  
25.     int n = A.size();  
26.     int m = A[0].size();  
27.     x.resize(n);  
28.     //复制系数矩阵，防止修改原矩阵  
29.     vector<vector<double> > Atemp(n);  
30.     for (int i = 0; i < n; i++)  
31.     {  
32.         vector<double> temp(m);  
33.         for (int j = 0; j < m; j++)  
34.         {  
35.             temp[j] = A[i][j];  
36.         }  
37.         Atemp[i] = temp;  
38.         temp.clear();  
39.     }  
40.     for (int k = 0; k < n; k++)  
41.     {  
42.         //选主元  
43.         double max = -1;  
44.         int l = -1;  
45.         for (int i = k; i < n; i++)  
46.         {  
47.             if (abs(Atemp[i][k]) > max)  
48.             {  
49.                 max = abs(Atemp[i][k]);  
50.                 l = i;  
51.             }  
52.         }  
53.         if (l != k)  
54.         {  
55.             //交换系数矩阵的l行和k行  
56.             for (int i = 0; i < m; i++)  
57.             {  
58.                 double temp = Atemp[l][i];  
59.                 Atemp[l][i] = Atemp[k][i];  
60.                 Atemp[k][i] = temp;  
61.             }  
62.         }  
63.         //消元  
64.         for (int i = k+1; i < n; i++)  
65.         {  
66.             double l = Atemp[i][k]/Atemp[k][k];  
67.             for (int j = k; j < m; j++)  
68.             {  
69.                 Atemp[i][j] = Atemp[i][j] - l*Atemp[k][j];  
70.             }  
71.         }  
72.     }  
73.     //回代  
74.     x[n-1] = Atemp[n-1][m-1]/Atemp[n-1][m-2];  
75.     for (int k = n-2; k >= 0; k--)  
76.     {  
77.         double s = 0.0;  
78.         for (int j = k+1; j < n; j++)  
79.         {  
80.             s += Atemp[k][j]*x[j];  
81.         }  
82.         x[k] = (Atemp[k][m-1] - s)/Atemp[k][k];  
83.     }  
84.     return true;  
85. }  
86.   
87. vector<double>  LSEllipse::getEllipseparGauss(vector<CPoint> vec_point)  
88. {  
89.     vector<double> vec_result;  
90.     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;  
91.     int N = vec_point.size();  
92.     for (int m_i = 0;m_i < N ;++m_i )  
93.     {  
94.         double xi = vec_point[m_i].x ;  
95.         double yi = vec_point[m_i].y;  
96.         x3y1 += xi*xi*xi*yi ;  
97.         x1y3 += xi*yi*yi*yi;  
98.         x2y2 += xi*xi*yi*yi; ;  
99.         yyy4 +=yi*yi*yi*yi;  
100.         xxx3 += xi*xi*xi ;  
101.         xxx2 += xi*xi ;  
102.         x2y1 += xi*xi*yi;  
103.   
104.         x1y2 += xi*yi*yi;  
105.         yyy2 += yi*yi;  
106.         x1y1 += xi*yi;  
107.         xxx1 += xi;  
108.         yyy1 += yi;  
109.         yyy3 += yi*yi*yi;  
110.     }  
111.     double resul[5];  
112.     resul[0] = -(x3y1);  
113.     resul[1] = -(x2y2);  
114.     resul[2] = -(xxx3);  
115.     resul[3] = -(x2y1);  
116.     resul[4] = -(xxx2);  
117.     long double Bb[5],Cc[5],Dd[5],Ee[5],Aa[5];  
118.     Bb[0] = x1y3, Cc[0] = x2y1, Dd[0] = x1y2, Ee[0] = x1y1, Aa[0] = x2y2;  
119.     Bb[1] = yyy4, Cc[1] = x1y2, Dd[1] = yyy3, Ee[1] = yyy2, Aa[1] = x1y3;  
120.     Bb[2] = x1y2, Cc[2] = xxx2, Dd[2] = x1y1, Ee[2] = xxx1, Aa[2] = x2y1;  
121.     Bb[3] = yyy3, Cc[3]= x1y1, Dd[3] = yyy2, Ee[3] = yyy1, Aa[3] = x1y2;  
122.     Bb[4]= yyy2, Cc[4]= xxx1, Dd[4] = yyy1, Ee[4] = N, Aa[4]= x1y1;  
123.   
124.     vector<vector<double>>Ma(5);  
125.     vector<double>Md(5);  
126.     for(int i=0;i<5;i++)  
127.     {  
128.         Ma[i].push_back(Aa[i]);  
129.         Ma[i].push_back(Bb[i]);  
130.         Ma[i].push_back(Cc[i]);  
131.         Ma[i].push_back(Dd[i]);  
132.         Ma[i].push_back(Ee[i]);  
133.         Ma[i].push_back(resul[i]);  
134.     }  
135.   
136.     RGauss(Ma,Md);  
137.     long double A=Md[0];  
138.     long double B=Md[1];  
139.     long double C=Md[2];  
140.     long double D=Md[3];  
141.     long double E=Md[4];  
142.     double XC=(2*B*C-A*D)/(A*A-4*B);  
143.     double YC=(2*D-A*C)/(A*A-4*B);  
144.     long double fenzi=2*(A*C*D-B*C*C-D*D+4*E*B-A*A*E);  
145.     long double fenmu=(A*A-4*B)*(B-sqrt(A*A+(1-B)*(1-B))+1);  
146.     long double fenmu2=(A*A-4*B)*(B+sqrt(A*A+(1-B)*(1-B))+1);  
147.     double XA=sqrt(fabs(fenzi/fenmu));  
148.     double XB=sqrt(fabs(fenzi/fenmu2));  
149.     double Xtheta=0.5*atan(A/(1-B))*180/3.1415926;  
150.     if(B<1)  
151.         Xtheta+=90;  
152.     vec_result.push_back(XC);  
153.     vec_result.push_back(YC);  
154.     vec_result.push_back(XA);  
155.     vec_result.push_back(XB);  
156.     vec_result.push_back(Xtheta);  
157.     return vec_result;  
158. }  
159.   
160. void  LSEllipse::cvFitEllipse2f(  int *arrayx, int *arrayy,int n,float *box )  
161. {     
162.     float cx=0,cy=0;  
163.     double rp[5], t;  
164.     float *A1=new float[n*5];  
165.     float *A2=new float[2*2];  
166.     float *A3=new float[n*3];  
167.     float *B1=new float[n],*B2=new float[2],*B3=new float[n];  
168.     const double min_eps = 1e-6;  
169.     int i;  
170.     for( i = 0; i < n; i++ )  
171.     {  
172.   
173.         cx += arrayx[i]*1.0;  
174.         cy += arrayy[i]*1.0;  
175.   
176.     }  
177.     cx /= n;  
178.     cy /= n;  
179.     for( i = 0; i < n; i++ )  
180.     {  
181.         int step=i*5;  
182.         float px,py;  
183.         px = arrayx[i]*1.0;  
184.         py = arrayy[i]*1.0;  
185.         px -= cx;  
186.         py -= cy;  
187.         B1[i] = 10000.0;  
188.         A1[step] = -px * px;  
189.         A1[step + 1] = -py * py;  
190.         A1[step + 2] = -px * py;  
191.         A1[step + 3] = px;  
192.         A1[step + 4] = py;  
193.     }  
194.     float *x1=new float[5];  
195.     //解出Ax^2+By^2+Cxy+Dx+Ey=10000的最小二乘解！  
196.     SVD(A1,n,5,B1,x1,min_eps);  
197.     A2[0]=2*x1[0],A2[1]=A2[2]=x1[2],A2[3]=2*x1[1];  
198.     B2[0]=x1[3],B2[1]=x1[4];  
199.     float *x2=new float[2];  
200.     //标准化，将一次项消掉，求出center.x和center.y;  
201.     SVD(A2,2,2,B2,x2,min_eps);  
202.     rp[0]=x2[0],rp[1]=x2[1];  
203.     for( i = 0; i < n; i++ )  
204.     {  
205.         float px,py;  
206.         px = arrayx[i]*1.0;  
207.         py = arrayy[i]*1.0;  
208.         px -= cx;  
209.         py -= cy;  
210.         B3[i] = 1.0;  
211.         int step=i*3;  
212.         A3[step] = (px - rp[0]) * (px - rp[0]);  
213.         A3[step+1] = (py - rp[1]) * (py - rp[1]);  
214.         A3[step+2] = (px - rp[0]) * (py - rp[1]);  
215.   
216.     }  
217.     //求出A(x-center.x)^2+B(y-center.y)^2+C(x-center.x)(y-center.y)的最小二乘解  
218.     SVD(A3,n,3,B3,x1,min_eps);  
219.   
220.     rp[4] = -0.5 * atan2(x1[2], x1[1] - x1[0]);  
221.     t = sin(-2.0 * rp[4]);  
222.     if( fabs(t) > fabs(x1[2])*min_eps )  
223.         t = x1[2]/t;  
224.     else  
225.         t = x1[1] - x1[0];  
226.     rp[2] = fabs(x1[0] + x1[1] - t);  
227.     if( rp[2] > min_eps )  
228.         rp[2] = sqrt(2.0 / rp[2]);  
229.     rp[3] = fabs(x1[0] + x1[1] + t);  
230.     if( rp[3] > min_eps )  
231.         rp[3] = sqrt(2.0 / rp[3]);  
232.   
233.     box[0] = (float)rp[0] + cx;  
234.     box[1]= (float)rp[1] + cy;  
235.     box[2]= (float)(rp[2]*2);  
236.     box[3] = (float)(rp[3]*2);  
237.     if( box[2] > box[3] )  
238.     {  
239.         double tmp=box[2];  
240.         box[2]=box[3];  
241.         box[3]=tmp;  
242.     }  
243.     box[4] = (float)(90 + rp[4]*180/3.1415926);  
244.     if( box[4] < -180 )  
245.         box[4] += 360;  
246.     if( box[4] > 360 )  
247.         box[4] -= 360;  
248.     delete []A1;  
249.     delete []A2;  
250.     delete []A3;  
251.     delete []B1;  
252.     delete []B2;  
253.     delete []B3;  
254.     delete []x1;  
255.     delete []x2;  
256.   
257. }  
258.   
259. int LSEllipse::SVD(float *a,int m,int n,float b[],float x[],float esp) //奇异值分解  
260. {    
261.     float *aa;  
262.     float *u;  
263.     float *v;  
264.     aa=new float[n*m];  
265.     u=new  float[m*m];  
266.     v=new  float[n*n];  
267.   
268.     int ka;  
269.     int  flag;  
270.     if(m>n)  
271.     {   
272.         ka=m+1;  
273.     }else  
274.     {  
275.         ka=n+1;  
276.     }  
277.   
278.     flag=gmiv(a,m,n,b,x,aa,esp,u,v,ka);  
279.   
280.   
281.   
282.     delete []aa;  
283.     delete []u;  
284.     delete []v;  
285.   
286.     return(flag);  
287. }  
288.   
289.   
290.   
291.   
292.   
293. int LSEllipse::gmiv( float a[],int m,int n,float b[],float x[],float aa[],float eps,float u[],float v[],int ka)    
294. {   
295.     int i,j;  
296.     i=ginv(a,m,n,aa,eps,u,v,ka);  
297.   
298.     if (i<0) return(-1);  
299.     for (i=0; i<=n-1; i++)  
300.     { x[i]=0.0;  
301.     for (j=0; j<=m-1; j++)  
302.         x[i]=x[i]+aa[i*m+j]*b[j];  
303.     }  
304.     return(1);  
305. }  
306.   
307.   
308. int LSEllipse::ginv(float a[],int m,int n,float aa[],float eps,float u[],float v[],int ka)  
309. {   
310.   
311.     //  int muav(float a[],int m,int n,float u[],float v[],float eps,int ka);  
312.   
313.     int i,j,k,l,t,p,q,f;  
314.     i=muav(a,m,n,u,v,eps,ka);  
315.     if (i<0) return(-1);  
316.     j=n;  
317.     if (m<n) j=m;  
318.     j=j-1;  
319.     k=0;  
320.     while ((k<=j)&&(a[k*n+k]!=0.0)) k=k+1;  
321.     k=k-1;  
322.     for (i=0; i<=n-1; i++)  
323.         for (j=0; j<=m-1; j++)  
324.         { t=i*m+j; aa[t]=0.0;  
325.     for (l=0; l<=k; l++)  
326.     { f=l*n+i; p=j*m+l; q=l*n+l;  
327.     aa[t]=aa[t]+v[f]*u[p]/a[q];  
328.     }  
329.     }  
330.     return(1);  
331. }  
332.   
333.   
334.   
335.   
336.   
337.   
338. int LSEllipse::muav(float a[],int m,int n,float u[],float v[],float eps,int ka)  
339. { int i,j,k,l,it,ll,kk,ix,iy,mm,nn,iz,m1,ks;  
340. float d,dd,t,sm,sm1,em1,sk,ek,b,c,shh,fg[2],cs[2];  
341. float *s,*e,*w;  
342. //void ppp();  
343. // void sss();  
344. void ppp(float a[],float e[],float s[],float v[],int m,int n);  
345. void sss(float fg[],float cs[]);  
346.   
347. s=(float *) malloc(ka*sizeof(float));  
348. e=(float *) malloc(ka*sizeof(float));  
349. w=(float *) malloc(ka*sizeof(float));  
350. it=60; k=n;  
351. if (m-1<n) k=m-1;  
352. l=m;  
353. if (n-2<m) l=n-2;  
354. if (l<0) l=0;  
355. ll=k;  
356. if (l>k) ll=l;  
357. if (ll>=1)  
358. { for (kk=1; kk<=ll; kk++)  
359. { if (kk<=k)  
360. { d=0.0;  
361. for (i=kk; i<=m; i++)  
362. { ix=(i-1)*n+kk-1; d=d+a[ix]*a[ix];}  
363. s[kk-1]=(float)sqrt(d);  
364. if (s[kk-1]!=0.0)  
365. { ix=(kk-1)*n+kk-1;  
366. if (a[ix]!=0.0)  
367. { s[kk-1]=(float)fabs(s[kk-1]);  
368. if (a[ix]<0.0) s[kk-1]=-s[kk-1];  
369. }  
370. for (i=kk; i<=m; i++)  
371. { iy=(i-1)*n+kk-1;  
372. a[iy]=a[iy]/s[kk-1];  
373. }  
374. a[ix]=1.0f+a[ix];  
375. }  
376. s[kk-1]=-s[kk-1];  
377. }  
378. if (n>=kk+1)  
379. { for (j=kk+1; j<=n; j++)  
380. { if ((kk<=k)&&(s[kk-1]!=0.0))  
381. { d=0.0;  
382. for (i=kk; i<=m; i++)  
383. { ix=(i-1)*n+kk-1;  
384. iy=(i-1)*n+j-1;  
385. d=d+a[ix]*a[iy];  
386. }  
387. d=-d/a[(kk-1)*n+kk-1];  
388. for (i=kk; i<=m; i++)  
389. { ix=(i-1)*n+j-1;  
390. iy=(i-1)*n+kk-1;  
391. a[ix]=a[ix]+d*a[iy];  
392. }  
393. }  
394. e[j-1]=a[(kk-1)*n+j-1];  
395. }  
396. }  
397. if (kk<=k)  
398. { for (i=kk; i<=m; i++)  
399. { ix=(i-1)*m+kk-1; iy=(i-1)*n+kk-1;  
400. u[ix]=a[iy];  
401. }  
402. }  
403. if (kk<=l)  
404. { d=0.0;  
405. for (i=kk+1; i<=n; i++)  
406.     d=d+e[i-1]*e[i-1];  
407. e[kk-1]=(float)sqrt(d);  
408. if (e[kk-1]!=0.0)  
409. { if (e[kk]!=0.0)  
410. { e[kk-1]=(float)fabs(e[kk-1]);  
411. if (e[kk]<0.0) e[kk-1]=-e[kk-1];  
412. }  
413. for (i=kk+1; i<=n; i++)  
414.     e[i-1]=e[i-1]/e[kk-1];  
415. e[kk]=1.0f+e[kk];  
416. }  
417. e[kk-1]=-e[kk-1];  
418. if ((kk+1<=m)&&(e[kk-1]!=0.0))  
419. { for (i=kk+1; i<=m; i++) w[i-1]=0.0;  
420. for (j=kk+1; j<=n; j++)  
421.     for (i=kk+1; i<=m; i++)  
422.         w[i-1]=w[i-1]+e[j-1]*a[(i-1)*n+j-1];  
423. for (j=kk+1; j<=n; j++)  
424.     for (i=kk+1; i<=m; i++)  
425.     { ix=(i-1)*n+j-1;  
426. a[ix]=a[ix]-w[i-1]*e[j-1]/e[kk];  
427. }  
428. }  
429. for (i=kk+1; i<=n; i++)  
430.     v[(i-1)*n+kk-1]=e[i-1];  
431. }  
432. }  
433. }  
434. mm=n;  
435. if (m+1<n) mm=m+1;  
436. if (k<n) s[k]=a[k*n+k];  
437. if (m<mm) s[mm-1]=0.0;  
438. if (l+1<mm) e[l]=a[l*n+mm-1];  
439. e[mm-1]=0.0;  
440. nn=m;  
441. if (m>n) nn=n;  
442. if (nn>=k+1)  
443. { for (j=k+1; j<=nn; j++)  
444. { for (i=1; i<=m; i++)  
445. u[(i-1)*m+j-1]=0.0;  
446. u[(j-1)*m+j-1]=1.0;  
447. }  
448. }  
449. if (k>=1)  
450. { for (ll=1; ll<=k; ll++)  
451. { kk=k-ll+1; iz=(kk-1)*m+kk-1;  
452. if (s[kk-1]!=0.0)  
453. { if (nn>=kk+1)  
454. for (j=kk+1; j<=nn; j++)  
455. { d=0.0;  
456. for (i=kk; i<=m; i++)  
457. { ix=(i-1)*m+kk-1;  
458. iy=(i-1)*m+j-1;  
459. d=d+u[ix]*u[iy]/u[iz];  
460. }  
461. d=-d;  
462. for (i=kk; i<=m; i++)  
463. { ix=(i-1)*m+j-1;  
464. iy=(i-1)*m+kk-1;  
465. u[ix]=u[ix]+d*u[iy];  
466. }  
467. }  
468. for (i=kk; i<=m; i++)  
469. { ix=(i-1)*m+kk-1; u[ix]=-u[ix];}  
470. u[iz]=1.0f+u[iz];  
471. if (kk-1>=1)  
472.     for (i=1; i<=kk-1; i++)  
473.         u[(i-1)*m+kk-1]=0.0;  
474. }  
475. else  
476. { for (i=1; i<=m; i++)  
477. u[(i-1)*m+kk-1]=0.0;  
478. u[(kk-1)*m+kk-1]=1.0;  
479. }  
480. }  
481. }  
482. for (ll=1; ll<=n; ll++)  
483. { kk=n-ll+1; iz=kk*n+kk-1;  
484. if ((kk<=l)&&(e[kk-1]!=0.0))  
485. { for (j=kk+1; j<=n; j++)  
486. { d=0.0;  
487. for (i=kk+1; i<=n; i++)  
488. { ix=(i-1)*n+kk-1; iy=(i-1)*n+j-1;  
489. d=d+v[ix]*v[iy]/v[iz];  
490. }  
491. d=-d;  
492. for (i=kk+1; i<=n; i++)  
493. { ix=(i-1)*n+j-1; iy=(i-1)*n+kk-1;  
494. v[ix]=v[ix]+d*v[iy];  
495. }  
496. }  
497. }  
498. for (i=1; i<=n; i++)  
499.     v[(i-1)*n+kk-1]=0.0;  
500. v[iz-n]=1.0;  
501. }  
502. for (i=1; i<=m; i++)  
503.     for (j=1; j<=n; j++)  
504.         a[(i-1)*n+j-1]=0.0;  
505. m1=mm; it=60;  
506. while (1==1)  
507. { if (mm==0)  
508. { ppp(a,e,s,v,m,n);  
509. free(s); free(e); free(w); return(1);  
510. }  
511. if (it==0)  
512. { ppp(a,e,s,v,m,n);  
513. free(s); free(e); free(w); return(-1);  
514. }  
515. kk=mm-1;  
516. while ((kk!=0)&&(fabs(e[kk-1])!=0.0))  
517. { d=(float)(fabs(s[kk-1])+fabs(s[kk]));  
518. dd=(float)fabs(e[kk-1]);  
519. if (dd>eps*d) kk=kk-1;  
520. else e[kk-1]=0.0;  
521. }  
522. if (kk==mm-1)  
523. { kk=kk+1;  
524. if (s[kk-1]<0.0)  
525. { s[kk-1]=-s[kk-1];  
526. for (i=1; i<=n; i++)  
527. { ix=(i-1)*n+kk-1; v[ix]=-v[ix];}  
528. }  
529. while ((kk!=m1)&&(s[kk-1]<s[kk]))  
530. { d=s[kk-1]; s[kk-1]=s[kk]; s[kk]=d;  
531. if (kk<n)  
532.     for (i=1; i<=n; i++)  
533.     { ix=(i-1)*n+kk-1; iy=(i-1)*n+kk;  
534. d=v[ix]; v[ix]=v[iy]; v[iy]=d;  
535. }  
536. if (kk<m)  
537.     for (i=1; i<=m; i++)  
538.     { ix=(i-1)*m+kk-1; iy=(i-1)*m+kk;  
539. d=u[ix]; u[ix]=u[iy]; u[iy]=d;  
540. }  
541. kk=kk+1;  
542. }  
543. it=60;  
544. mm=mm-1;  
545. }  
546. else  
547. { ks=mm;  
548. while ((ks>kk)&&(fabs(s[ks-1])!=0.0))  
549. { d=0.0;  
550. if (ks!=mm) d=d+(float)fabs(e[ks-1]);  
551. if (ks!=kk+1) d=d+(float)fabs(e[ks-2]);  
552. dd=(float)fabs(s[ks-1]);  
553. if (dd>eps*d) ks=ks-1;  
554. else s[ks-1]=0.0;  
555. }  
556. if (ks==kk)  
557. { kk=kk+1;  
558. d=(float)fabs(s[mm-1]);  
559. t=(float)fabs(s[mm-2]);  
560. if (t>d) d=t;  
561. t=(float)fabs(e[mm-2]);  
562. if (t>d) d=t;  
563. t=(float)fabs(s[kk-1]);  
564. if (t>d) d=t;  
565. t=(float)fabs(e[kk-1]);  
566. if (t>d) d=t;  
567. sm=s[mm-1]/d; sm1=s[mm-2]/d;  
568. em1=e[mm-2]/d;  
569. sk=s[kk-1]/d; ek=e[kk-1]/d;  
570. b=((sm1+sm)*(sm1-sm)+em1*em1)/2.0f;  
571. c=sm*em1; c=c*c; shh=0.0;  
572. if ((b!=0.0)||(c!=0.0))  
573. { shh=(float)sqrt(b*b+c);  
574. if (b<0.0) shh=-shh;  
575. shh=c/(b+shh);  
576. }  
577. fg[0]=(sk+sm)*(sk-sm)-shh;  
578. fg[1]=sk*ek;  
579. for (i=kk; i<=mm-1; i++)  
580. { sss(fg,cs);  
581. if (i!=kk) e[i-2]=fg[0];  
582. fg[0]=cs[0]*s[i-1]+cs[1]*e[i-1];  
583. e[i-1]=cs[0]*e[i-1]-cs[1]*s[i-1];  
584. fg[1]=cs[1]*s[i];  
585. s[i]=cs[0]*s[i];  
586. if ((cs[0]!=1.0)||(cs[1]!=0.0))  
587.     for (j=1; j<=n; j++)  
588.     { ix=(j-1)*n+i-1;  
589. iy=(j-1)*n+i;  
590. d=cs[0]*v[ix]+cs[1]*v[iy];  
591. v[iy]=-cs[1]*v[ix]+cs[0]*v[iy];  
592. v[ix]=d;  
593. }  
594. sss(fg,cs);  
595. s[i-1]=fg[0];  
596. fg[0]=cs[0]*e[i-1]+cs[1]*s[i];  
597. s[i]=-cs[1]*e[i-1]+cs[0]*s[i];  
598. fg[1]=cs[1]*e[i];  
599. e[i]=cs[0]*e[i];  
600. if (i<m)  
601.     if ((cs[0]!=1.0)||(cs[1]!=0.0))  
602.         for (j=1; j<=m; j++)  
603.         { ix=(j-1)*m+i-1;  
604. iy=(j-1)*m+i;  
605. d=cs[0]*u[ix]+cs[1]*u[iy];  
606. u[iy]=-cs[1]*u[ix]+cs[0]*u[iy];  
607. u[ix]=d;  
608. }  
609. }  
610. e[mm-2]=fg[0];  
611. it=it-1;  
612. }  
613. else  
614. { if (ks==mm)  
615. { kk=kk+1;  
616. fg[1]=e[mm-2]; e[mm-2]=0.0;  
617. for (ll=kk; ll<=mm-1; ll++)  
618. { i=mm+kk-ll-1;  
619. fg[0]=s[i-1];  
620. sss(fg,cs);  
621. s[i-1]=fg[0];  
622. if (i!=kk)  
623. { fg[1]=-cs[1]*e[i-2];  
624. e[i-2]=cs[0]*e[i-2];  
625. }  
626. if ((cs[0]!=1.0)||(cs[1]!=0.0))  
627.     for (j=1; j<=n; j++)  
628.     { ix=(j-1)*n+i-1;  
629. iy=(j-1)*n+mm-1;  
630. d=cs[0]*v[ix]+cs[1]*v[iy];  
631. v[iy]=-cs[1]*v[ix]+cs[0]*v[iy];  
632. v[ix]=d;  
633. }  
634. }  
635. }  
636. else  
637. { kk=ks+1;  
638. fg[1]=e[kk-2];  
639. e[kk-2]=0.0;  
640. for (i=kk; i<=mm; i++)  
641. { fg[0]=s[i-1];  
642. sss(fg,cs);  
643. s[i-1]=fg[0];  
644. fg[1]=-cs[1]*e[i-1];  
645. e[i-1]=cs[0]*e[i-1];  
646. if ((cs[0]!=1.0)||(cs[1]!=0.0))  
647.     for (j=1; j<=m; j++)  
648.     { ix=(j-1)*m+i-1;  
649. iy=(j-1)*m+kk-2;  
650. d=cs[0]*u[ix]+cs[1]*u[iy];  
651. u[iy]=-cs[1]*u[ix]+cs[0]*u[iy];  
652. u[ix]=d;  
653. }  
654. }  
655. }  
656. }  
657. }  
658. }  
659.   
660. free(s);free(e);free(w);   
661. return(1);  
662.   
663.   
664. }  
665.   
666.   
667. void ppp(float a[],float e[],float s[],float v[],int m,int n)   
668. { int i,j,p,q;  
669. float d;  
670. if (m>=n) i=n;  
671. else i=m;  
672. for (j=1; j<=i-1; j++)  
673. { a[(j-1)*n+j-1]=s[j-1];  
674. a[(j-1)*n+j]=e[j-1];  
675. }  
676. a[(i-1)*n+i-1]=s[i-1];  
677. if (m<n) a[(i-1)*n+i]=e[i-1];  
678. for (i=1; i<=n-1; i++)  
679.     for (j=i+1; j<=n; j++)  
680.     { p=(i-1)*n+j-1; q=(j-1)*n+i-1;  
681. d=v[p]; v[p]=v[q]; v[q]=d;  
682. }  
683. return;  
684. }  
685.   
686.   
687. void sss(float fg[],float cs[])  
688. { float r,d;  
689. if ((fabs(fg[0])+fabs(fg[1]))==0.0)  
690. { cs[0]=1.0; cs[1]=0.0; d=0.0;}  
691. else   
692. { d=(float)sqrt(fg[0]*fg[0]+fg[1]*fg[1]);  
693. if (fabs(fg[0])>fabs(fg[1]))  
694. { d=(float)fabs(d);  
695. if (fg[0]<0.0) d=-d;  
696. }  
697. if (fabs(fg[1])>=fabs(fg[0]))  
698. { d=(float)fabs(d);  
699. if (fg[1]<0.0) d=-d;  
700. }  
701. cs[0]=fg[0]/d; cs[1]=fg[1]/d;  
702. }  
703. r=1.0;  
704. if (fabs(fg[0])>fabs(fg[1])) r=cs[1];  
705. else  
706.     if (cs[0]!=0.0) r=1.0f/cs[0];  
707. fg[0]=d; fg[1]=r;  
708. return;  
709. }  
710.   
711.   
712. //传入样本点，返回椭圆的5个参数  
713.   
714. vector<double> getEllipsepar(vector<CvPoint> vec_point)  
715. {  
716.     vector<double> vec_result;  
717.     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;  
718.   
719.   
720.   
721.     int N = vec_point.size();  
722.     cout << N << endl;  
723.     for (int m_i = 0;m_i < N ;++m_i )  
724.     {  
725.         double xi = vec_point[m_i].x ;  
726.         double yi = vec_point[m_i].y;  
727.         x3y1   += xi*xi*xi*yi ;  
728.         x1y3   += xi*yi*yi*yi;  
729.         x2y2   += xi*xi*yi*yi; ;  
730.         yyy4   +=yi*yi*yi*yi;  
731.         xxx3   += xi*xi*xi ;  
732.         xxx2   += xi*xi ;  
733.         x2y1 += xi*xi*yi;  
734.   
735.         x1y2 += xi*yi*yi;  
736.         yyy2 += yi*yi;  
737.         x1y1 += xi*yi;  
738.         xxx1 += xi;  
739.         yyy1 += yi;  
740.         yyy3 += yi*yi*yi;  
741.     }  
742.   
743.     long double resul1 = -(x3y1);  
744.     long double resul2 = -(x2y2);  
745.     long double resul3 = -(xxx3);  
746.     long double resul4 = -(x2y1);  
747.     long double resul5 = -(xxx2);  
748.     long double B1 = x1y3,     C1 = x2y1,  D1 = x1y2,  E1 = x1y1,  A1 = x2y2;  
749.     long double B2 = yyy4,     C2 = x1y2,  D2 = yyy3,  E2 = yyy2,  A2 = x1y3;  
750.     long double B3 = x1y2,     C3 = xxx2,  D3 = x1y1,  E3 = xxx1,  A3 = x2y1;  
751.     long double B4 = yyy3,     C4 = x1y1,  D4 = yyy2,  E4 = yyy1,  A4 = x1y2;  
752.     long double B5 = yyy2,     C5 = xxx1,  D5 = yyy1,  E5 = N,     A5 = x1y1;  
753.   
754.     //  
755.     CvMat* Ma    = cvCreateMat(5,5,CV_64FC1);  
756.     CvMat* Md    = cvCreateMat(5,1,CV_64FC1);  
757.     CvMat* Mb    = cvCreateMat(5,1,CV_64FC1);  
758.     //  
759.   
760.     cvmSet(Mb,0,0,resul1);  
761.     cvmSet(Mb,1,0,resul2);  
762.     cvmSet(Mb,2,0,resul3);  
763.     cvmSet(Mb,3,0,resul4);  
764.     cvmSet(Mb,4,0,resul5);  
765.   
766.   
767.   
768.     cvmSet(Ma,0,0,A1);  
769.     cvmSet(Ma,0,1,B1);  
770.     cvmSet(Ma,0,2,C1);  
771.     cvmSet(Ma,0,3,D1);  
772.     cvmSet(Ma,0,4,E1);  
773.   
774.   
775.     cvmSet(Ma,1,0,A2);  
776.     cvmSet(Ma,1,1,B2);  
777.     cvmSet(Ma,1,2,C2);  
778.     cvmSet(Ma,1,3,D2);  
779.     cvmSet(Ma,1,4,E2);  
780.   
781.   
782.     cvmSet(Ma,2,0,A3);  
783.     cvmSet(Ma,2,1,B3);  
784.     cvmSet(Ma,2,2,C3);  
785.     cvmSet(Ma,2,3,D3);  
786.     cvmSet(Ma,2,4,E3);  
787.   
788.     cvmSet(Ma,3,0,A4);  
789.     cvmSet(Ma,3,1,B4);  
790.     cvmSet(Ma,3,2,C4);  
791.     cvmSet(Ma,3,3,D4);  
792.     cvmSet(Ma,3,4,E4);  
793.   
794.     cvmSet(Ma,4,0,A5);  
795.     cvmSet(Ma,4,1,B5);  
796.     cvmSet(Ma,4,2,C5);  
797.     cvmSet(Ma,4,3,D5);  
798.     cvmSet(Ma,4,4,E5);  
799.   
800.   
801.   
802.   
803.     cvSolve(Ma, Mb, Md);  
804.     long double A = cvmGet(Md,0,0);  
805.     long double B = cvmGet(Md,1,0);  
806.     long double C = cvmGet(Md,2,0);  
807.     long double D = cvmGet(Md,3,0);  
808.     long double E = cvmGet(Md,4,0);  
809.   
810.     double XC = (2*B*C - A*D) /(A*A-4*B);  
811.     double YC =(2*D-A*D)/(A*A-4*B);  
812.   
813.     long double fenzi = 2*(A*C*D-B*C*C-D*D+4*E*B-A*A*E);  
814.   
815.     long double fenmu =(A*A-4*B)   * (B- sqrt(A*A+ (1-B) * (1-B) )  +1);  
816.     long double femmu2 =(A*A-4*B)  * (B+ sqrt(A*A+ (1-B) * (1-B) )  +1);  
817.     double XA =sqrt(fabs(fenzi/fenmu));  
818.     double XB =sqrt(fabs(fenzi/femmu2));  
819.     double Xtheta =atan(sqrt((XA*XA-XB*XB*B)/(XA*XA*B-XB*XB))+0.0001)*180/3.1415926;  
820.   
821.     vec_result.push_back(XC);  
822.     vec_result.push_back(YC);  
823.     vec_result.push_back(XA);  
824.     vec_result.push_back(XB);  
825.     vec_result.push_back(Xtheta);  
826.     return vec_result;  
827. }  

//点A (a,b)为椭圆上的一点，θ为OA与X轴的夹角，则参数方程为
//x=acosθ
//y=bsinθ
//x^2/a^2+y^2/b^2=1为标准方程


#include"stdafx.h"
#include "LSEllipse.h"
#include <cmath>

LSEllipse::LSEllipse(void)
{
}


LSEllipse::~LSEllipse(void)
{
}
//列主元高斯消去法
//A为系数矩阵，x为解向量，若成功，返回true，否则返回false，并将x清空。

bool RGauss(const vector<vector<double> > & A, vector<double> & x)
{
	x.clear();
	int n = A.size();
	int m = A[0].size();
	x.resize(n);
	//复制系数矩阵，防止修改原矩阵
	vector<vector<double> > Atemp(n);
	for (int i = 0; i < n; i++)
	{
		vector<double> temp(m);
		for (int j = 0; j < m; j++)
		{
			temp[j] = A[i][j];
		}
		Atemp[i] = temp;
		temp.clear();
	}
	for (int k = 0; k < n; k++)
	{
		//选主元
		double max = -1;
		int l = -1;
		for (int i = k; i < n; i++)
		{
			if (abs(Atemp[i][k]) > max)
			{
				max = abs(Atemp[i][k]);
				l = i;
			}
		}
		if (l != k)
		{
			//交换系数矩阵的l行和k行
			for (int i = 0; i < m; i++)
			{
				double temp = Atemp[l][i];
				Atemp[l][i] = Atemp[k][i];
				Atemp[k][i] = temp;
			}
		}
		//消元
		for (int i = k+1; i < n; i++)
		{
			double l = Atemp[i][k]/Atemp[k][k];
			for (int j = k; j < m; j++)
			{
				Atemp[i][j] = Atemp[i][j] - l*Atemp[k][j];
			}
		}
	}
	//回代
	x[n-1] = Atemp[n-1][m-1]/Atemp[n-1][m-2];
	for (int k = n-2; k >= 0; k--)
	{
		double s = 0.0;
		for (int j = k+1; j < n; j++)
		{
			s += Atemp[k][j]*x[j];
		}
		x[k] = (Atemp[k][m-1] - s)/Atemp[k][k];
	}
	return true;
}

vector<double>  LSEllipse::getEllipseparGauss(vector<CPoint> 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;
	}
	double resul[5];
	resul[0] = -(x3y1);
	resul[1] = -(x2y2);
	resul[2] = -(xxx3);
	resul[3] = -(x2y1);
	resul[4] = -(xxx2);
	long double Bb[5],Cc[5],Dd[5],Ee[5],Aa[5];
	Bb[0] = x1y3, Cc[0] = x2y1, Dd[0] = x1y2, Ee[0] = x1y1, Aa[0] = x2y2;
	Bb[1] = yyy4, Cc[1] = x1y2, Dd[1] = yyy3, Ee[1] = yyy2, Aa[1] = x1y3;
	Bb[2] = x1y2, Cc[2] = xxx2, Dd[2] = x1y1, Ee[2] = xxx1, Aa[2] = x2y1;
	Bb[3] = yyy3, Cc[3]= x1y1, Dd[3] = yyy2, Ee[3] = yyy1, Aa[3] = x1y2;
	Bb[4]= yyy2, Cc[4]= xxx1, Dd[4] = yyy1, Ee[4] = N, Aa[4]= x1y1;

	vector<vector<double>>Ma(5);
	vector<double>Md(5);
	for(int i=0;i<5;i++)
	{
		Ma[i].push_back(Aa[i]);
		Ma[i].push_back(Bb[i]);
		Ma[i].push_back(Cc[i]);
		Ma[i].push_back(Dd[i]);
		Ma[i].push_back(Ee[i]);
		Ma[i].push_back(resul[i]);
	}

	RGauss(Ma,Md);
	long double A=Md[0];
	long double B=Md[1];
	long double C=Md[2];
	long double D=Md[3];
	long double E=Md[4];
	double XC=(2*B*C-A*D)/(A*A-4*B);
	double YC=(2*D-A*C)/(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 fenmu2=(A*A-4*B)*(B+sqrt(A*A+(1-B)*(1-B))+1);
	double XA=sqrt(fabs(fenzi/fenmu));
	double XB=sqrt(fabs(fenzi/fenmu2));
	double Xtheta=0.5*atan(A/(1-B))*180/3.1415926;
	if(B<1)
		Xtheta+=90;
	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;
}

void  LSEllipse::cvFitEllipse2f(  int *arrayx, int *arrayy,int n,float *box )
{   
	float cx=0,cy=0;
	double rp[5], t;
	float *A1=new float[n*5];
	float *A2=new float[2*2];
	float *A3=new float[n*3];
	float *B1=new float[n],*B2=new float[2],*B3=new float[n];
	const double min_eps = 1e-6;
	int i;
	for( i = 0; i < n; i++ )
	{

		cx += arrayx[i]*1.0;
		cy += arrayy[i]*1.0;

	}
	cx /= n;
	cy /= n;
	for( i = 0; i < n; i++ )
	{
		int step=i*5;
		float px,py;
		px = arrayx[i]*1.0;
		py = arrayy[i]*1.0;
		px -= cx;
		py -= cy;
		B1[i] = 10000.0;
		A1[step] = -px * px;
		A1[step + 1] = -py * py;
		A1[step + 2] = -px * py;
		A1[step + 3] = px;
		A1[step + 4] = py;
	}
	float *x1=new float[5];
	//解出Ax^2+By^2+Cxy+Dx+Ey=10000的最小二乘解！
	SVD(A1,n,5,B1,x1,min_eps);
	A2[0]=2*x1[0],A2[1]=A2[2]=x1[2],A2[3]=2*x1[1];
	B2[0]=x1[3],B2[1]=x1[4];
	float *x2=new float[2];
	//标准化，将一次项消掉，求出center.x和center.y;
	SVD(A2,2,2,B2,x2,min_eps);
	rp[0]=x2[0],rp[1]=x2[1];
	for( i = 0; i < n; i++ )
	{
		float px,py;
		px = arrayx[i]*1.0;
		py = arrayy[i]*1.0;
		px -= cx;
		py -= cy;
		B3[i] = 1.0;
		int step=i*3;
		A3[step] = (px - rp[0]) * (px - rp[0]);
		A3[step+1] = (py - rp[1]) * (py - rp[1]);
		A3[step+2] = (px - rp[0]) * (py - rp[1]);

	}
	//求出A(x-center.x)^2+B(y-center.y)^2+C(x-center.x)(y-center.y)的最小二乘解
	SVD(A3,n,3,B3,x1,min_eps);

	rp[4] = -0.5 * atan2(x1[2], x1[1] - x1[0]);
	t = sin(-2.0 * rp[4]);
	if( fabs(t) > fabs(x1[2])*min_eps )
		t = x1[2]/t;
	else
		t = x1[1] - x1[0];
	rp[2] = fabs(x1[0] + x1[1] - t);
	if( rp[2] > min_eps )
		rp[2] = sqrt(2.0 / rp[2]);
	rp[3] = fabs(x1[0] + x1[1] + t);
	if( rp[3] > min_eps )
		rp[3] = sqrt(2.0 / rp[3]);

	box[0] = (float)rp[0] + cx;
	box[1]= (float)rp[1] + cy;
	box[2]= (float)(rp[2]*2);
	box[3] = (float)(rp[3]*2);
	if( box[2] > box[3] )
	{
		double tmp=box[2];
		box[2]=box[3];
		box[3]=tmp;
	}
	box[4] = (float)(90 + rp[4]*180/3.1415926);
	if( box[4] < -180 )
		box[4] += 360;
	if( box[4] > 360 )
		box[4] -= 360;
	delete []A1;
	delete []A2;
	delete []A3;
	delete []B1;
	delete []B2;
	delete []B3;
	delete []x1;
	delete []x2;

}

int LSEllipse::SVD(float *a,int m,int n,float b[],float x[],float esp) //奇异值分解
{  
	float *aa;
	float *u;
	float *v;
	aa=new float[n*m];
	u=new  float[m*m];
	v=new  float[n*n];

	int ka;
	int  flag;
	if(m>n)
	{ 
		ka=m+1;
	}else
	{
		ka=n+1;
	}

	flag=gmiv(a,m,n,b,x,aa,esp,u,v,ka);



	delete []aa;
	delete []u;
	delete []v;

	return(flag);
}





int LSEllipse::gmiv( float a[],int m,int n,float b[],float x[],float aa[],float eps,float u[],float v[],int ka)  
{ 
	int i,j;
	i=ginv(a,m,n,aa,eps,u,v,ka);

	if (i<0) return(-1);
	for (i=0; i<=n-1; i++)
	{ x[i]=0.0;
	for (j=0; j<=m-1; j++)
		x[i]=x[i]+aa[i*m+j]*b[j];
	}
	return(1);
}


int LSEllipse::ginv(float a[],int m,int n,float aa[],float eps,float u[],float v[],int ka)
{ 

	//  int muav(float a[],int m,int n,float u[],float v[],float eps,int ka);

	int i,j,k,l,t,p,q,f;
	i=muav(a,m,n,u,v,eps,ka);
	if (i<0) return(-1);
	j=n;
	if (m<n) j=m;
	j=j-1;
	k=0;
	while ((k<=j)&&(a[k*n+k]!=0.0)) k=k+1;
	k=k-1;
	for (i=0; i<=n-1; i++)
		for (j=0; j<=m-1; j++)
		{ t=i*m+j; aa[t]=0.0;
	for (l=0; l<=k; l++)
	{ f=l*n+i; p=j*m+l; q=l*n+l;
	aa[t]=aa[t]+v[f]*u[p]/a[q];
	}
	}
	return(1);
}






int LSEllipse::muav(float a[],int m,int n,float u[],float v[],float eps,int ka)
{ int i,j,k,l,it,ll,kk,ix,iy,mm,nn,iz,m1,ks;
float d,dd,t,sm,sm1,em1,sk,ek,b,c,shh,fg[2],cs[2];
float *s,*e,*w;
//void ppp();
// void sss();
void ppp(float a[],float e[],float s[],float v[],int m,int n);
void sss(float fg[],float cs[]);

s=(float *) malloc(ka*sizeof(float));
e=(float *) malloc(ka*sizeof(float));
w=(float *) malloc(ka*sizeof(float));
it=60; k=n;
if (m-1<n) k=m-1;
l=m;
if (n-2<m) l=n-2;
if (l<0) l=0;
ll=k;
if (l>k) ll=l;
if (ll>=1)
{ for (kk=1; kk<=ll; kk++)
{ if (kk<=k)
{ d=0.0;
for (i=kk; i<=m; i++)
{ ix=(i-1)*n+kk-1; d=d+a[ix]*a[ix];}
s[kk-1]=(float)sqrt(d);
if (s[kk-1]!=0.0)
{ ix=(kk-1)*n+kk-1;
if (a[ix]!=0.0)
{ s[kk-1]=(float)fabs(s[kk-1]);
if (a[ix]<0.0) s[kk-1]=-s[kk-1];
}
for (i=kk; i<=m; i++)
{ iy=(i-1)*n+kk-1;
a[iy]=a[iy]/s[kk-1];
}
a[ix]=1.0f+a[ix];
}
s[kk-1]=-s[kk-1];
}
if (n>=kk+1)
{ for (j=kk+1; j<=n; j++)
{ if ((kk<=k)&&(s[kk-1]!=0.0))
{ d=0.0;
for (i=kk; i<=m; i++)
{ ix=(i-1)*n+kk-1;
iy=(i-1)*n+j-1;
d=d+a[ix]*a[iy];
}
d=-d/a[(kk-1)*n+kk-1];
for (i=kk; i<=m; i++)
{ ix=(i-1)*n+j-1;
iy=(i-1)*n+kk-1;
a[ix]=a[ix]+d*a[iy];
}
}
e[j-1]=a[(kk-1)*n+j-1];
}
}
if (kk<=k)
{ for (i=kk; i<=m; i++)
{ ix=(i-1)*m+kk-1; iy=(i-1)*n+kk-1;
u[ix]=a[iy];
}
}
if (kk<=l)
{ d=0.0;
for (i=kk+1; i<=n; i++)
	d=d+e[i-1]*e[i-1];
e[kk-1]=(float)sqrt(d);
if (e[kk-1]!=0.0)
{ if (e[kk]!=0.0)
{ e[kk-1]=(float)fabs(e[kk-1]);
if (e[kk]<0.0) e[kk-1]=-e[kk-1];
}
for (i=kk+1; i<=n; i++)
	e[i-1]=e[i-1]/e[kk-1];
e[kk]=1.0f+e[kk];
}
e[kk-1]=-e[kk-1];
if ((kk+1<=m)&&(e[kk-1]!=0.0))
{ for (i=kk+1; i<=m; i++) w[i-1]=0.0;
for (j=kk+1; j<=n; j++)
	for (i=kk+1; i<=m; i++)
		w[i-1]=w[i-1]+e[j-1]*a[(i-1)*n+j-1];
for (j=kk+1; j<=n; j++)
	for (i=kk+1; i<=m; i++)
	{ ix=(i-1)*n+j-1;
a[ix]=a[ix]-w[i-1]*e[j-1]/e[kk];
}
}
for (i=kk+1; i<=n; i++)
	v[(i-1)*n+kk-1]=e[i-1];
}
}
}
mm=n;
if (m+1<n) mm=m+1;
if (k<n) s[k]=a[k*n+k];
if (m<mm) s[mm-1]=0.0;
if (l+1<mm) e[l]=a[l*n+mm-1];
e[mm-1]=0.0;
nn=m;
if (m>n) nn=n;
if (nn>=k+1)
{ for (j=k+1; j<=nn; j++)
{ for (i=1; i<=m; i++)
u[(i-1)*m+j-1]=0.0;
u[(j-1)*m+j-1]=1.0;
}
}
if (k>=1)
{ for (ll=1; ll<=k; ll++)
{ kk=k-ll+1; iz=(kk-1)*m+kk-1;
if (s[kk-1]!=0.0)
{ if (nn>=kk+1)
for (j=kk+1; j<=nn; j++)
{ d=0.0;
for (i=kk; i<=m; i++)
{ ix=(i-1)*m+kk-1;
iy=(i-1)*m+j-1;
d=d+u[ix]*u[iy]/u[iz];
}
d=-d;
for (i=kk; i<=m; i++)
{ ix=(i-1)*m+j-1;
iy=(i-1)*m+kk-1;
u[ix]=u[ix]+d*u[iy];
}
}
for (i=kk; i<=m; i++)
{ ix=(i-1)*m+kk-1; u[ix]=-u[ix];}
u[iz]=1.0f+u[iz];
if (kk-1>=1)
	for (i=1; i<=kk-1; i++)
		u[(i-1)*m+kk-1]=0.0;
}
else
{ for (i=1; i<=m; i++)
u[(i-1)*m+kk-1]=0.0;
u[(kk-1)*m+kk-1]=1.0;
}
}
}
for (ll=1; ll<=n; ll++)
{ kk=n-ll+1; iz=kk*n+kk-1;
if ((kk<=l)&&(e[kk-1]!=0.0))
{ for (j=kk+1; j<=n; j++)
{ d=0.0;
for (i=kk+1; i<=n; i++)
{ ix=(i-1)*n+kk-1; iy=(i-1)*n+j-1;
d=d+v[ix]*v[iy]/v[iz];
}
d=-d;
for (i=kk+1; i<=n; i++)
{ ix=(i-1)*n+j-1; iy=(i-1)*n+kk-1;
v[ix]=v[ix]+d*v[iy];
}
}
}
for (i=1; i<=n; i++)
	v[(i-1)*n+kk-1]=0.0;
v[iz-n]=1.0;
}
for (i=1; i<=m; i++)
	for (j=1; j<=n; j++)
		a[(i-1)*n+j-1]=0.0;
m1=mm; it=60;
while (1==1)
{ if (mm==0)
{ ppp(a,e,s,v,m,n);
free(s); free(e); free(w); return(1);
}
if (it==0)
{ ppp(a,e,s,v,m,n);
free(s); free(e); free(w); return(-1);
}
kk=mm-1;
while ((kk!=0)&&(fabs(e[kk-1])!=0.0))
{ d=(float)(fabs(s[kk-1])+fabs(s[kk]));
dd=(float)fabs(e[kk-1]);
if (dd>eps*d) kk=kk-1;
else e[kk-1]=0.0;
}
if (kk==mm-1)
{ kk=kk+1;
if (s[kk-1]<0.0)
{ s[kk-1]=-s[kk-1];
for (i=1; i<=n; i++)
{ ix=(i-1)*n+kk-1; v[ix]=-v[ix];}
}
while ((kk!=m1)&&(s[kk-1]<s[kk]))
{ d=s[kk-1]; s[kk-1]=s[kk]; s[kk]=d;
if (kk<n)
	for (i=1; i<=n; i++)
	{ ix=(i-1)*n+kk-1; iy=(i-1)*n+kk;
d=v[ix]; v[ix]=v[iy]; v[iy]=d;
}
if (kk<m)
	for (i=1; i<=m; i++)
	{ ix=(i-1)*m+kk-1; iy=(i-1)*m+kk;
d=u[ix]; u[ix]=u[iy]; u[iy]=d;
}
kk=kk+1;
}
it=60;
mm=mm-1;
}
else
{ ks=mm;
while ((ks>kk)&&(fabs(s[ks-1])!=0.0))
{ d=0.0;
if (ks!=mm) d=d+(float)fabs(e[ks-1]);
if (ks!=kk+1) d=d+(float)fabs(e[ks-2]);
dd=(float)fabs(s[ks-1]);
if (dd>eps*d) ks=ks-1;
else s[ks-1]=0.0;
}
if (ks==kk)
{ kk=kk+1;
d=(float)fabs(s[mm-1]);
t=(float)fabs(s[mm-2]);
if (t>d) d=t;
t=(float)fabs(e[mm-2]);
if (t>d) d=t;
t=(float)fabs(s[kk-1]);
if (t>d) d=t;
t=(float)fabs(e[kk-1]);
if (t>d) d=t;
sm=s[mm-1]/d; sm1=s[mm-2]/d;
em1=e[mm-2]/d;
sk=s[kk-1]/d; ek=e[kk-1]/d;
b=((sm1+sm)*(sm1-sm)+em1*em1)/2.0f;
c=sm*em1; c=c*c; shh=0.0;
if ((b!=0.0)||(c!=0.0))
{ shh=(float)sqrt(b*b+c);
if (b<0.0) shh=-shh;
shh=c/(b+shh);
}
fg[0]=(sk+sm)*(sk-sm)-shh;
fg[1]=sk*ek;
for (i=kk; i<=mm-1; i++)
{ sss(fg,cs);
if (i!=kk) e[i-2]=fg[0];
fg[0]=cs[0]*s[i-1]+cs[1]*e[i-1];
e[i-1]=cs[0]*e[i-1]-cs[1]*s[i-1];
fg[1]=cs[1]*s[i];
s[i]=cs[0]*s[i];
if ((cs[0]!=1.0)||(cs[1]!=0.0))
	for (j=1; j<=n; j++)
	{ ix=(j-1)*n+i-1;
iy=(j-1)*n+i;
d=cs[0]*v[ix]+cs[1]*v[iy];
v[iy]=-cs[1]*v[ix]+cs[0]*v[iy];
v[ix]=d;
}
sss(fg,cs);
s[i-1]=fg[0];
fg[0]=cs[0]*e[i-1]+cs[1]*s[i];
s[i]=-cs[1]*e[i-1]+cs[0]*s[i];
fg[1]=cs[1]*e[i];
e[i]=cs[0]*e[i];
if (i<m)
	if ((cs[0]!=1.0)||(cs[1]!=0.0))
		for (j=1; j<=m; j++)
		{ ix=(j-1)*m+i-1;
iy=(j-1)*m+i;
d=cs[0]*u[ix]+cs[1]*u[iy];
u[iy]=-cs[1]*u[ix]+cs[0]*u[iy];
u[ix]=d;
}
}
e[mm-2]=fg[0];
it=it-1;
}
else
{ if (ks==mm)
{ kk=kk+1;
fg[1]=e[mm-2]; e[mm-2]=0.0;
for (ll=kk; ll<=mm-1; ll++)
{ i=mm+kk-ll-1;
fg[0]=s[i-1];
sss(fg,cs);
s[i-1]=fg[0];
if (i!=kk)
{ fg[1]=-cs[1]*e[i-2];
e[i-2]=cs[0]*e[i-2];
}
if ((cs[0]!=1.0)||(cs[1]!=0.0))
	for (j=1; j<=n; j++)
	{ ix=(j-1)*n+i-1;
iy=(j-1)*n+mm-1;
d=cs[0]*v[ix]+cs[1]*v[iy];
v[iy]=-cs[1]*v[ix]+cs[0]*v[iy];
v[ix]=d;
}
}
}
else
{ kk=ks+1;
fg[1]=e[kk-2];
e[kk-2]=0.0;
for (i=kk; i<=mm; i++)
{ fg[0]=s[i-1];
sss(fg,cs);
s[i-1]=fg[0];
fg[1]=-cs[1]*e[i-1];
e[i-1]=cs[0]*e[i-1];
if ((cs[0]!=1.0)||(cs[1]!=0.0))
	for (j=1; j<=m; j++)
	{ ix=(j-1)*m+i-1;
iy=(j-1)*m+kk-2;
d=cs[0]*u[ix]+cs[1]*u[iy];
u[iy]=-cs[1]*u[ix]+cs[0]*u[iy];
u[ix]=d;
}
}
}
}
}
}

free(s);free(e);free(w); 
return(1);


}


void ppp(float a[],float e[],float s[],float v[],int m,int n) 
{ int i,j,p,q;
float d;
if (m>=n) i=n;
else i=m;
for (j=1; j<=i-1; j++)
{ a[(j-1)*n+j-1]=s[j-1];
a[(j-1)*n+j]=e[j-1];
}
a[(i-1)*n+i-1]=s[i-1];
if (m<n) a[(i-1)*n+i]=e[i-1];
for (i=1; i<=n-1; i++)
	for (j=i+1; j<=n; j++)
	{ p=(i-1)*n+j-1; q=(j-1)*n+i-1;
d=v[p]; v[p]=v[q]; v[q]=d;
}
return;
}


void sss(float fg[],float cs[])
{ float r,d;
if ((fabs(fg[0])+fabs(fg[1]))==0.0)
{ cs[0]=1.0; cs[1]=0.0; d=0.0;}
else 
{ d=(float)sqrt(fg[0]*fg[0]+fg[1]*fg[1]);
if (fabs(fg[0])>fabs(fg[1]))
{ d=(float)fabs(d);
if (fg[0]<0.0) d=-d;
}
if (fabs(fg[1])>=fabs(fg[0]))
{ d=(float)fabs(d);
if (fg[1]<0.0) d=-d;
}
cs[0]=fg[0]/d; cs[1]=fg[1]/d;
}
r=1.0;
if (fabs(fg[0])>fabs(fg[1])) r=cs[1];
else
	if (cs[0]!=0.0) r=1.0f/cs[0];
fg[0]=d; fg[1]=r;
return;
}


//传入样本点，返回椭圆的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;
}