二维点云数据椭圆拟合算法及C++实现

椭圆的标准方程

当焦点在x轴时，椭圆的标准方程是：x^2/a^2+y^2/b^2=1，(a>b>0)；

当焦点在y轴时，椭圆的标准方程是：y^2/a^2+x^2/b^2=1，(a>b>0)；

      现在考虑一种特殊情况，假设二维点云拟合出的椭圆方程交点不在X轴或Y轴，即在上面的标准方程基础上发生“偏转+平移”，此时的椭圆方程形式该是什么样子？

先考虑旋转：

平面上一点P(X,Y)旋转θ角度，旋转后的坐标为(X',Y')，那么:
X' = X*cos(θ) - Y*sin(θ) , Y' = X*sin(θ) + Y*cos(θ) ;

旋转后椭圆 X^2/A^2 + Y^2/B^2 = 1 方程就变成了:[ X*cos(θ) - Y*sin(θ)]^2/A^2 +[X*sin(θ) + Y*cos(θ)]^2/B^2 = 1;

再考虑旋转：X' = X+S ,Y' = Y+T ;

旋转和平移综合起来考虑，椭圆 X^2/A^2 + Y^2/B^2 = 1 方程就变成了：

[(X+S)*cos(θ) - (Y+T)*sin(θ)]^2/A^2 +[(X+S)*sin(θ) + (Y+T)*cos(θ)]^2/B^2 = 1;

椭圆拟合算法

http://blog.csdn.net/zhazhiqiang/article/details/45824957，

https://wenku.baidu.com/view/5ec29d573c1ec5da50e270cf.html

C++算法实现

//椭圆类class LSEllipse{public:LSEllipse(void);~LSEllipse(void);void cvFitEllipse2f(double *arrayx, double *arrayy, int n, double *box);private:int SVD(double *a, int m, int n, double b[], double x[], double esp);int gmiv(double a[], int m, int n, double b[], double x[], double aa[], double eps, double u[], double v[], int ka);int ginv(double a[], int m, int n, double aa[], double eps, double u[], double v[], int ka);int muav(double a[], int m, int n, double u[], double v[], double eps, int ka);double a = DBL_MAX;};

//椭圆类的实现LSEllipse::LSEllipse(void){}LSEllipse::~LSEllipse(void){}bool RGauss(const std::vector<std::vector<double> > & A, std::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;}void  LSEllipse::cvFitEllipse2f(double *arrayx, double *arrayy, int n, double *box){double cx = 0, cy = 0;double rp[5], t;double *A1 = new double[n*5];double *A2 = new double[2*2];double *A3 = new double[n*3];double *B1 = new double[n], *B2 = new double[2], *B3 = new double[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;double 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;}double *x1 = new double[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];double *x2 = new double[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++){double 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;elset = 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] = (double)rp[0]+cx;box[1] = (double)rp[1]+cy;box[2] = (double)(rp[2]*2);box[3] = (double)(rp[3]*2);if (box[2]>box[3]){double tmp = box[2];box[2] = box[3];box[3] = tmp;}box[4] = (double)(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(double *a, int m, int n, double b[], double x[], double esp){double *aa;double *u;double *v;aa = new double[n*m];u = new  double[m*m];v = new  double[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(double a[], int m, int n, double b[], double x[], double aa[], double eps, double u[], double 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(double a[], int m, int n, double aa[], double eps, double u[], double v[], int ka){//  int muav(double a[],int m,int n,double u[],double v[],double 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(double a[], int m, int n, double u[], double v[], double eps, int ka){int i, j, k, l, it, ll, kk, ix, iy, mm, nn, iz, m1, ks;double d, dd, t, sm, sm1, em1, sk, ek, b, c, shh, fg[2], cs[2];double *s, *e, *w;//void ppp();// void sss();void ppp(double a[], double e[], double s[], double v[], int m, int n);void sss(double fg[], double cs[]);s = (double *)malloc(ka*sizeof(double));e = (double *)malloc(ka*sizeof(double));w = (double *)malloc(ka*sizeof(double));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] = (double)sqrt(d);if (s[kk-1]!=0.0){ix = (kk-1)*n+kk-1;if (a[ix]!=0.0){s[kk-1] = (double)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] = (double)sqrt(d);if (e[kk-1]!=0.0){if (e[kk]!=0.0){e[kk-1] = (double)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 = (double)(fabs(s[kk-1])+fabs(s[kk]));dd = (double)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+(double)fabs(e[ks-1]);if (ks!=kk+1) d = d+(double)fabs(e[ks-2]);dd = (double)fabs(s[ks-1]);if (dd>eps*d) ks = ks-1;else s[ks-1] = 0.0;}if (ks==kk){kk = kk+1;d = (double)fabs(s[mm-1]);t = (double)fabs(s[mm-2]);if (t>d) d = t;t = (double)fabs(e[mm-2]);if (t>d) d = t;t = (double)fabs(s[kk-1]);if (t>d) d = t;t = (double)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 = (double)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(double a[], double e[], double s[], double v[], int m, int n){int i, j, p, q;double 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(double fg[], double cs[]){double r, d;if ((fabs(fg[0])+fabs(fg[1]))==0.0){cs[0] = 1.0; cs[1] = 0.0; d = 0.0;}else{d = (double)sqrt(fg[0]*fg[0]+fg[1]*fg[1]);if (fabs(fg[0])>fabs(fg[1])){d = (double)fabs(d);if (fg[0]<0.0) d = -d;}if (fabs(fg[1])>=fabs(fg[0])){d = (double)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];elseif (cs[0]!=0.0) r = 1.0f/cs[0];fg[0] = d; fg[1] = r;return;}

其他方法比较

上述方法是基于代数距离，即竖直方向最小二乘实现的拟合，http://blog.csdn.net/xiao_lxl/article/details/46725985，还有基于几何距离，即法线方向的椭圆拟合，请参见：
http://blog.csdn.net/xiamentingtao/article/details/54934467；

另外，找到一篇关于opencv实现椭圆拟合的方法介绍：

http://blog.csdn.net/qq_23880193/article/details/49257769