椭圆的标准方程
当焦点在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;
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 (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];
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
扫一扫
442
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
