最小二乘法椭圆拟合
对平面上的一些点拟合有很多手段，其中椭圆拟合在图像轮廓划分等很多方面都很重要，当然，我们一般还是用最小二乘法来拟合椭圆， 在这里，我实现了两种算法，一种是view plaincopy
[*]http://wenku.baidu.com/link?url=7kIrC8LoOMCtlmAH8yqkpUQfiKwWnVe4EoUJekkQSgQ1qTWfLAuEXTYvYTv7SATGIJYX4IxcTIB94-iO0SpUgztWgx661O2VEOwm_dvoSqO
这篇文章给出的，核心也是最小二乘法，利用gauss消去法解方程组，不过他给出的代码有些小bug，所以我改了一下，也去掉了opencv的东西。
还有一个就是利用奇异值分解法来求超定方程的最小二乘法的思想来求出椭圆的五个参数，关于奇异值分解法可以参考 http://blog.csdn.net/wangzhiqing3/article/details/7446444 下面是我的代码实现：
//LSEllipse.h
view plaincopy
[*]/*************************************************************************
[*] 版本： 2014-12-31
[*] 功能说明： 对平面上的一些列点给出最小二乘的椭圆拟合，利用奇异值分解法
[*] 解得最小二乘解作为椭圆参数。
[*] 调用形式： cvFitEllipse2f（arrayx,arrayy,box）；
[*] 参数说明： arrayx: arrayx,每个值为x轴一个点
[*] arrayx: arrayy,每个值为y轴一个点
[*] n : 点的个数
[*] box : box,椭圆的五个参数，分别为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);
[*]};
//LSEllipse.cpp
view plaincopy
[*]#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.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 = A;
[*] }
[*] Atemp = 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) > max)
[*] {
[*] max = abs(Atemp);
[*] l = i;
[*] }
[*] }
[*] if (l != k)
[*] {
[*] //交换系数矩阵的l行和k行
[*] for (int i = 0; i < m; i++)
[*] {
[*] double temp = Atemp;
[*] Atemp = Atemp;
[*] Atemp = temp;
[*] }
[*] }
[*] //消元
[*] for (int i = k+1; i < n; i++)
[*] {
[*] double l = Atemp/Atemp;
[*] for (int j = k; j < m; j++)
[*] {
[*] Atemp = Atemp - l*Atemp;
[*] }
[*] }
[*] }
[*] //回代
[*] x = Atemp/Atemp;
[*] for (int k = n-2; k >= 0; k--)
[*] {
[*] double s = 0.0;
[*] for (int j = k+1; j < n; j++)
[*] {
[*] s += Atemp*x;
[*] }
[*] x = (Atemp - s)/Atemp;
[*] }
[*] 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.x ;
[*] double yi = vec_point.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;
[*] resul = -(x3y1);
[*] resul = -(x2y2);
[*] resul = -(xxx3);
[*] resul = -(x2y1);
[*] resul = -(xxx2);
[*] long double Bb,Cc,Dd,Ee,Aa;
[*] Bb = x1y3, Cc = x2y1, Dd = x1y2, Ee = x1y1, Aa = x2y2;
[*] Bb = yyy4, Cc = x1y2, Dd = yyy3, Ee = yyy2, Aa = x1y3;
[*] Bb = x1y2, Cc = xxx2, Dd = x1y1, Ee = xxx1, Aa = x2y1;
[*] Bb = yyy3, Cc= x1y1, Dd = yyy2, Ee = yyy1, Aa = x1y2;
[*] Bb= yyy2, Cc= xxx1, Dd = yyy1, Ee = N, Aa= x1y1;
[*]
[*] vector<vector<double>>Ma(5);
[*] vector<double>Md(5);
[*] for(int i=0;i<5;i++)
[*] {
[*] Ma.push_back(Aa);
[*] Ma.push_back(Bb);
[*] Ma.push_back(Cc);
[*] Ma.push_back(Dd);
[*] Ma.push_back(Ee);
[*] Ma.push_back(resul);
[*] }
[*]
[*] RGauss(Ma,Md);
[*] long double A=Md;
[*] long double B=Md;
[*] long double C=Md;
[*] long double D=Md;
[*] long double E=Md;
[*] 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;
[*]}
[*]
[*]voidLSEllipse::cvFitEllipse2f(int *arrayx, int *arrayy,int n,float *box )
[*]{
[*] float cx=0,cy=0;
[*] double rp, t;
[*] float *A1=new float;
[*] float *A2=new float;
[*] float *A3=new float;
[*] float *B1=new float,*B2=new float,*B3=new float;
[*] const double min_eps = 1e-6;
[*] int i;
[*] for( i = 0; i < n; i++ )
[*] {
[*]
[*] cx += arrayx*1.0;
[*] cy += arrayy*1.0;
[*]
[*] }
[*] cx /= n;
[*] cy /= n;
[*] for( i = 0; i < n; i++ )
[*] {
[*] int step=i*5;
[*] float px,py;
[*] px = arrayx*1.0;
[*] py = arrayy*1.0;
[*] px -= cx;
[*] py -= cy;
[*] B1 = 10000.0;
[*] A1 = -px * px;
[*] A1 = -py * py;
[*] A1 = -px * py;
[*] A1 = px;
[*] A1 = py;
[*] }
[*] float *x1=new float;
[*] //解出Ax^2+By^2+Cxy+Dx+Ey=10000的最小二乘解！
[*] SVD(A1,n,5,B1,x1,min_eps);
[*] A2=2*x1,A2=A2=x1,A2=2*x1;
[*] B2=x1,B2=x1;
[*] float *x2=new float;
[*] //标准化，将一次项消掉，求出center.x和center.y;
[*] SVD(A2,2,2,B2,x2,min_eps);
[*] rp=x2,rp=x2;
[*] for( i = 0; i < n; i++ )
[*] {
[*] float px,py;
[*] px = arrayx*1.0;
[*] py = arrayy*1.0;
[*] px -= cx;
[*] py -= cy;
[*] B3 = 1.0;
[*] int step=i*3;
[*] A3 = (px - rp) * (px - rp);
[*] A3 = (py - rp) * (py - rp);
[*] A3 = (px - rp) * (py - rp);
[*]
[*] }
[*] //求出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 = -0.5 * atan2(x1, x1 - x1);
[*] t = sin(-2.0 * rp);
[*] if( fabs(t) > fabs(x1)*min_eps )
[*] t = x1/t;
[*] else
[*] t = x1 - x1;
[*] rp = fabs(x1 + x1 - t);
[*] if( rp > min_eps )
[*] rp = sqrt(2.0 / rp);
[*] rp = fabs(x1 + x1 + t);
[*] if( rp > min_eps )
[*] rp = sqrt(2.0 / rp);
[*]
[*] box = (float)rp + cx;
[*] box= (float)rp + cy;
[*] box= (float)(rp*2);
[*] box = (float)(rp*2);
[*] if( box > box )
[*] {
[*] double tmp=box;
[*] box=box;
[*] box=tmp;
[*] }
[*] box = (float)(90 + rp*180/3.1415926);
[*] if( box < -180 )
[*] box += 360;
[*] if( box > 360 )
[*] box -= 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;
[*] u=newfloat;
[*] v=newfloat;
[*]
[*] int ka;
[*] intflag;
[*] 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=0.0;
[*] for (j=0; j<=m-1; j++)
[*] x=x+aa*b;
[*] }
[*] 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!=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=0.0;
[*] for (l=0; l<=k; l++)
[*] { f=l*n+i; p=j*m+l; q=l*n+l;
[*] aa=aa+v*u/a;
[*] }
[*] }
[*] 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,cs;
[*] 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*a;}
[*] s=(float)sqrt(d);
[*] if (s!=0.0)
[*] { ix=(kk-1)*n+kk-1;
[*] if (a!=0.0)
[*] { s=(float)fabs(s);
[*] if (a<0.0) s=-s;
[*] }
[*] for (i=kk; i<=m; i++)
[*] { iy=(i-1)*n+kk-1;
[*] a=a/s;
[*] }
[*] a=1.0f+a;
[*] }
[*] s=-s;
[*] }
[*] if (n>=kk+1)
[*] { for (j=kk+1; j<=n; j++)
[*] { if ((kk<=k)&&(s!=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*a;
[*] }
[*] 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=a+d*a;
[*] }
[*] }
[*] e=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=a;
[*] }
[*] }
[*] if (kk<=l)
[*] { d=0.0;
[*] for (i=kk+1; i<=n; i++)
[*] d=d+e*e;
[*] e=(float)sqrt(d);
[*] if (e!=0.0)
[*] { if (e!=0.0)
[*] { e=(float)fabs(e);
[*] if (e<0.0) e=-e;
[*] }
[*] for (i=kk+1; i<=n; i++)
[*] e=e/e;
[*] e=1.0f+e;
[*] }
[*] e=-e;
[*] if ((kk+1<=m)&&(e!=0.0))
[*] { for (i=kk+1; i<=m; i++) w=0.0;
[*] for (j=kk+1; j<=n; j++)
[*] for (i=kk+1; i<=m; i++)
[*] w=w+e*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=a-w*e/e;
[*] }
[*] }
[*] for (i=kk+1; i<=n; i++)
[*] v[(i-1)*n+kk-1]=e;
[*] }
[*] }
[*] }
[*] mm=n;
[*] if (m+1<n) mm=m+1;
[*] if (k<n) s=a;
[*] if (m<mm) s=0.0;
[*] if (l+1<mm) e=a;
[*] e=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!=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*u/u;
[*] }
[*] d=-d;
[*] for (i=kk; i<=m; i++)
[*] { ix=(i-1)*m+j-1;
[*] iy=(i-1)*m+kk-1;
[*] u=u+d*u;
[*] }
[*] }
[*] for (i=kk; i<=m; i++)
[*] { ix=(i-1)*m+kk-1; u=-u;}
[*] u=1.0f+u;
[*] 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!=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*v/v;
[*] }
[*] d=-d;
[*] for (i=kk+1; i<=n; i++)
[*] { ix=(i-1)*n+j-1; iy=(i-1)*n+kk-1;
[*] v=v+d*v;
[*] }
[*] }
[*] }
[*] for (i=1; i<=n; i++)
[*] v[(i-1)*n+kk-1]=0.0;
[*] v=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)!=0.0))
[*] { d=(float)(fabs(s)+fabs(s));
[*] dd=(float)fabs(e);
[*] if (dd>eps*d) kk=kk-1;
[*] else e=0.0;
[*] }
[*] if (kk==mm-1)
[*] { kk=kk+1;
[*] if (s<0.0)
[*] { s=-s;
[*] for (i=1; i<=n; i++)
[*] { ix=(i-1)*n+kk-1; v=-v;}
[*] }
[*] while ((kk!=m1)&&(s<s))
[*] { d=s; s=s; s=d;
[*] if (kk<n)
[*] for (i=1; i<=n; i++)
[*] { ix=(i-1)*n+kk-1; iy=(i-1)*n+kk;
[*] d=v; v=v; v=d;
[*] }
[*] if (kk<m)
[*] for (i=1; i<=m; i++)
[*] { ix=(i-1)*m+kk-1; iy=(i-1)*m+kk;
[*] d=u; u=u; u=d;
[*] }
[*] kk=kk+1;
[*] }
[*] it=60;
[*] mm=mm-1;
[*] }
[*] else
[*] { ks=mm;
[*] while ((ks>kk)&&(fabs(s)!=0.0))
[*] { d=0.0;
[*] if (ks!=mm) d=d+(float)fabs(e);
[*] if (ks!=kk+1) d=d+(float)fabs(e);
[*] dd=(float)fabs(s);
[*] if (dd>eps*d) ks=ks-1;
[*] else s=0.0;
[*] }
[*] if (ks==kk)
[*] { kk=kk+1;
[*] d=(float)fabs(s);
[*] t=(float)fabs(s);
[*] if (t>d) d=t;
[*] t=(float)fabs(e);
[*] if (t>d) d=t;
[*] t=(float)fabs(s);
[*] if (t>d) d=t;
[*] t=(float)fabs(e);
[*] if (t>d) d=t;
[*] sm=s/d; sm1=s/d;
[*] em1=e/d;
[*] sk=s/d; ek=e/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=(sk+sm)*(sk-sm)-shh;
[*] fg=sk*ek;
[*] for (i=kk; i<=mm-1; i++)
[*] { sss(fg,cs);
[*] if (i!=kk) e=fg;
[*] fg=cs*s+cs*e;
[*] e=cs*e-cs*s;
[*] fg=cs*s;
[*] s=cs*s;
[*] if ((cs!=1.0)||(cs!=0.0))
[*] for (j=1; j<=n; j++)
[*] { ix=(j-1)*n+i-1;
[*] iy=(j-1)*n+i;
[*] d=cs*v+cs*v;
[*] v=-cs*v+cs*v;
[*] v=d;
[*] }
[*] sss(fg,cs);
[*] s=fg;
[*] fg=cs*e+cs*s;
[*] s=-cs*e+cs*s;
[*] fg=cs*e;
[*] e=cs*e;
[*] if (i<m)
[*] if ((cs!=1.0)||(cs!=0.0))
[*] for (j=1; j<=m; j++)
[*] { ix=(j-1)*m+i-1;
[*] iy=(j-1)*m+i;
[*] d=cs*u+cs*u;
[*] u=-cs*u+cs*u;
[*] u=d;
[*] }
[*] }
[*] e=fg;
[*] it=it-1;
[*] }
[*] else
[*] { if (ks==mm)
[*] { kk=kk+1;
[*] fg=e; e=0.0;
[*] for (ll=kk; ll<=mm-1; ll++)
[*] { i=mm+kk-ll-1;
[*] fg=s;
[*] sss(fg,cs);
[*] s=fg;
[*] if (i!=kk)
[*] { fg=-cs*e;
[*] e=cs*e;
[*] }
[*] if ((cs!=1.0)||(cs!=0.0))
[*] for (j=1; j<=n; j++)
[*] { ix=(j-1)*n+i-1;
[*] iy=(j-1)*n+mm-1;
[*] d=cs*v+cs*v;
[*] v=-cs*v+cs*v;
[*] v=d;
[*] }
[*] }
[*] }
[*] else
[*] { kk=ks+1;
[*] fg=e;
[*] e=0.0;
[*] for (i=kk; i<=mm; i++)
[*] { fg=s;
[*] sss(fg,cs);
[*] s=fg;
[*] fg=-cs*e;
[*] e=cs*e;
[*] if ((cs!=1.0)||(cs!=0.0))
[*] for (j=1; j<=m; j++)
[*] { ix=(j-1)*m+i-1;
[*] iy=(j-1)*m+kk-2;
[*] d=cs*u+cs*u;
[*] u=-cs*u+cs*u;
[*] u=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;
[*] a[(j-1)*n+j]=e;
[*] }
[*] a[(i-1)*n+i-1]=s;
[*] if (m<n) a[(i-1)*n+i]=e;
[*] 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; v=v; v=d;
[*] }
[*] return;
[*]}
[*]
[*]
[*]void sss(float fg[],float cs[])
[*] { float r,d;
[*] if ((fabs(fg)+fabs(fg))==0.0)
[*] { cs=1.0; cs=0.0; d=0.0;}
[*] else
[*] { d=(float)sqrt(fg*fg+fg*fg);
[*] if (fabs(fg)>fabs(fg))
[*] { d=(float)fabs(d);
[*] if (fg<0.0) d=-d;
[*] }
[*] if (fabs(fg)>=fabs(fg))
[*] { d=(float)fabs(d);
[*] if (fg<0.0) d=-d;
[*] }
[*] cs=fg/d; cs=fg/d;
[*] }
[*] r=1.0;
[*] if (fabs(fg)>fabs(fg)) r=cs;
[*] else
[*] if (cs!=0.0) r=1.0f/cs;
[*] fg=d; fg=r;
[*] return;
[*]}
利用VC效果如下：
其中青色的线为第一种方法的效果，黑色为第二种方法的拟合效果。
点赞！
ObjectARX 同道友人{:1_12:}
页:
[1]