二维点云数据椭圆拟合算法及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 > & A, std::vector & x)
{
x.clear();
int n = A.size();
int m = A[0].size();
x.resize(n);
//复制系数矩阵，防止修改原矩阵
vector > Atemp(n);
for (int i = 0; i temp(m);
for (int j = 0; j max)
{
max = abs(Atemp[i][k]);
l = i;
}
}
if (l!=k)
{
//交换系数矩阵的l行和k行
for (int i = 0; i=0; k--)
{
double s = 0.0;
for (int j = k+1; jfabs(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] = (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 (mk) 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+1n) 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]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=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 (mfabs(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];
else
if (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；
http://blog.csdn.net/qq_23880193/article/details/49257769