最小二乘法椭圆拟合

【原文：http://blog.csdn.net/alop_daoyan/article/details/42296565】

  对平面上的一些点拟合有很多手段，其中椭圆拟合在图像轮廓划分等很多方面都很重要，当然，我们一般还是用最小二乘法来拟合椭圆，

      在这里，我实现了两种算法，一种是

[html]  view plain copy

1. http://wenku.baidu.com/link?url=7kIrC8LoOMCtlmAH8yqkpUQfiKwWnVe4EoUJekkQSgQ1qTWfLAuEXTYvYTv7SATGIJYX4IxcTIB94-iO0SpUgztWgx661O2VEOwm_dvoSqO  

    这篇文章给出的，核心也是最小二乘法，利用gauss消去法解方程组，不过他给出的代码有些小bug，所以我改了一下，也去掉了opencv的东西。

    还有一个就是利用奇异值分解法来求超定方程的最小二乘法的思想来求出椭圆的五个参数，关于奇异值分解法可以参考

    http://blog.csdn.net/wangzhiqing3/article/details/7446444

   下面是我的代码实现：

//LSEllipse.h

[cpp]  view plain copy

1. /************************************************************************* 
2.     版本：     2014-12-31 
3.     功能说明： 对平面上的一些列点给出最小二乘的椭圆拟合，利用奇异值分解法 
4.                解得最小二乘解作为椭圆参数。 
5.     调用形式： cvFitEllipse2f（arrayx,arrayy,box）；     
6.     参数说明： arrayx: arrayx[n],每个值为x轴一个点 
7.                arrayx: arrayy[n],每个值为y轴一个点 
8.                n     : 点的个数 
9.                box   : box[5],椭圆的五个参数，分别为center.x,center.y,2a,2b,xtheta 
10.                esp: 解精度，通常取1e-6,这个是解方程用的说 
11. ***************************************************************************/  
12.   
13.   
14.   
15.   
16. #include"stdafx.h"  
17. #include<cstdlib>  
18. #include<float.h>  
19. #include<vector>  
20. using namespace std;  
21.   
22. class LSEllipse  
23. {  
24. public:  
25.     LSEllipse(void);  
26.     ~LSEllipse(void);  
27.     vector<double> getEllipseparGauss(vector<CPoint> vec_point);  
28.     void cvFitEllipse2f( int *arrayx, int *arrayy,int n,float *box );  
29. private:  
30.     int SVD(float *a,int m,int n,float b[],float x[],float esp);  
31.     int gmiv(float a[],int m,int n,float b[],float x[],float aa[],float eps,float u[],float v[],int ka);  
32.     int ginv(float a[],int m,int n,float aa[],float eps,float u[],float v[],int ka);  
33.     int muav(float a[],int m,int n,float u[],float v[],float eps,int ka);  
34. };  

//LSEllipse.cpp

[cpp]  view plain copy

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

利用VC效果如下：

其中青色的线为第一种方法的效果，黑色为第二种方法的拟合效果。