bool EllipseFitting(POINT* rgPoints, int nPoints, double &a, double &b, double &c, double &d, double &e, double &f)
{
int i, j;
int np = nPoints;
int nrot=0;
Matrix<double> D(np+1,7); //Design matrix
Matrix<double> S(7,7); //Scatter matrix
Matrix<double> Const(7,7); //Constraint matrix
Matrix<double> temp(7,7);
Matrix<double> L(7,7); //The lower triangular matrix L * L' = S
Matrix<double> C(7,7);
Matrix<double> invL(7,7); //Inverse matrix of L
Matrix<double> V(7,7); //The eigen vectors
Matrix<double> sol(7,7); //The GVE solution
double ev[7]; //The eigen value
double pvec[7];
if (np < 6)
{
return false;
}
//Build design matrix D
for (i = 0; i < np; i++)
{
D[i][1] = rgPoints[i].x * rgPoints[i].x;
D[i][2] = rgPoints[i].x * rgPoints[i].y;
D[i][3] = rgPoints[i].y * rgPoints[i].y;
D[i][4] = rgPoints[i].x;
D[i][5] = rgPoints[i].y;
D[i][6] = 1.0;
}
//Build Scatter matrix S
A_TperB(D, D, S, np, 6, np, 6);
//Build 6*6 constraint matrix
Const[1][3] = -2;
Const[2][2] = 1;
Const[3][1] = -2;
//Sovle generalized eigen system
choldc(S, 6, L);
if (!inverse(L, invL, 6))
{
return false;
}
AperB_T(Const, invL, temp, 6, 6, 6, 6);
AperB(invL, temp, C, 6, 6, 6, 6);
jacobi(C, 6, ev, V, nrot);
A_TperB(invL, V, sol, 6, 6, 6, 6);
//Normalize the solution
for (j = 1; j <= 6; j++)
{
double mod = 0.0;
for (i = 1; i <= 6; i++)
{
mod += sol[i][j] * sol[i][j];
}
mod = sqrt(mod);
for (i = 1 ; i <= 6; i++)
{
sol[i][j] /= mod;
}
}
double zero = 10e-20;
int solind = 0;
//Find the only negative eigen value
for (i = 1; i <= 6; i++)
{
if ((ev[i] < 0) && (fabs(ev[i]) > zero))
{
solind = i;
}
}
//Get the fitted parameters
for (j = 1; j <= 6; j++)
{
pvec[j] = sol[j][solind];
}
a = pvec[1];
b = pvec[2];
c = pvec[3];
d = pvec[4];
e = pvec[5];
f = pvec[6];
return true;
}
void Apply(float *x_in, float *y_in, int num_points,
double& xc, double& yc,
double& xa, double& ya,
double& la, double& lb)
{
int np = num_points;
static double invL[7][7];
static double D[MAXP+1][7];
static double S[7][7];
static double Const[7][7];
static double temp[7][7];
static double L[7][7];
static double C[7][7];
static double d [7];
static double V[7][7];
static double sol[7][7];
static double tx,ty;
int nrot=0;
int npts=50;
static double XY[3][MAXP+1];
assert(num_points<MAXP);
int mode = FPF;
for (int i=0; i<7; i++)
{
d[i] = 0;
for (int j=0; j<MAXP+1; j++)
{
D[j][i] = 0;
}
for (int j=0; j<7; j++)
{
S[i][j] = 0;
Const[i][j] = 0;
temp[i][j] = 0;
L[i][j] = 0;
C[i][j] = 0;
invL[i][j] = 0;
V[i][j] = 0;
sol[i][j] = 0;
}
}
switch (mode) {
case(FPF):
//System.out.println("FPF mode");
Const[1][3]=-2;
Const[2][2]=1;
Const[3][1]=-2;
break;
case(TAUBIN):
// g.drawString(warn_taub_str,size().width/18 , size().height/18 );
break;
case(BOOKSTEIN):
//System.out.println("BOOK mode");
Const[1][1]=2;
Const[2][2]=1;
Const[3][3]=2;
}
if (np<6)
return;
// Now first fill design matrix
for (int i=1; i <= np; i++)
{
tx = x_in[i-1];
ty = y_in[i-1];
// printf("%d %d, %g, %g\n", i, np, tx, ty);
D[i][1] = tx*tx;
D[i][2] = tx*ty;
D[i][3] = ty*ty;
D[i][4] = tx;
D[i][5] = ty;
D[i][6] = 1.0;
}
// printf("Done\n");
//pm(Const,"Constraint");
// Now compute scatter matrix S
A_TperB(D,D,S,np,6,np,6);
//pm(S,"Scatter");
choldc(S,6,L);
//pm(L,"Cholesky");
inverse7(L,invL,6);
//pm(invL,"inverse");
AperB_T(Const,invL,temp,6,6,6,6);
AperB(invL,temp,C,6,6,6,6);
//pm(C,"The C matrix");
jacobi(C,6,d,V,nrot);
//pm(V,"The Eigenvectors");
//pv(d,"The eigevalues");
A_TperB(invL,V,sol,6,6,6,6);
//pm(sol,"The GEV solution unnormalized");
// Now normalize them
for (int j=1;j<=6;j++)
{
double mod = 0.0;
for (int i=1;i<=6;i++)
mod += sol[i][j]*sol[i][j];
for (int i=1;i<=6;i++)
sol[i][j] /= sqrt(mod);
}
//pm(sol,"The GEV solution");
double zero=10e-20;
double minev=10e+20;
int solind=0;
switch (mode) {
case(BOOKSTEIN): // smallest eigenvalue
for (int i=1; i<=6; i++)
if (d[i]<minev && fabs(d[i])>zero)
solind = i;
break;
case(FPF):
for (int i=1; i<=6; i++)
if (d[i]<0 && fabs(d[i])>zero)
solind = i;
}
// Now fetch the right solution
for (int j=1;j<=6;j++)
{
pvec[j] = sol[j][solind];
//params[j-1] = sol[j][solind];
}
//pv(pvec,"the solution");
// double xc, yc, xa, ya, a, b;
get_param(pvec,xc,yc,xa,ya,la,lb);
// ...and plot it
// draw_conic(pvec,4);
/*
for (int i=1; i<npts; i++) {
if (XY[1][i]==-1 || XY[1][i+1]==-1 )
continue;
else
if (i<npts)
g.drawLine((int)XY[1][i],(int)XY[2][i],(int)XY[1][i+1],(int)XY[2][i+1] );
else
g.drawLine((int)XY[1][i],(int)XY[2][i],(int)XY[1][1],(int)XY[2][1] );
}
*/
}
