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] );
}
*/
}
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 get_param(double *pvec,
double& xc, double& yc,
double& xa, double& ya,
double& la, double& lb)
{
int npts=0;
static double u[3][MAXP+1];
static double Aiu[3][MAXP+1];
static double L[3][MAXP+1];
static double B[3][MAXP+1];
static double Xpos[3][MAXP+1];
static double Xneg[3][MAXP+1];
static double ss1[3][MAXP+1];
static double ss2[3][MAXP+1];
static double lambda[MAXP+1];
static double uAiu[3][MAXP+1];
static double A[3][3];
static double Ai[3][3];
static double Aib[3][2];
static double b[3][2];
static double r1[2][2];
static double Ao, Ax, Ay, Axx, Ayy, Axy;
double pi = 3.14781;
double theta;
int i;
int j;
double kk;
Ao = pvec[6];
Ax = pvec[4];
Ay = pvec[5];
Axx = pvec[1];
Ayy = pvec[3];
Axy = pvec[2];
A[1][1] = Axx; A[1][2] = Axy/2;
A[2][1] = Axy/2; A[2][2] = Ayy;
b[1][1] = Ax; b[2][1] = Ay;
npts = 4;
// Generate normals linspace
for (i=0, theta=0.0; i<=npts; i++, theta=(i*2*M_PI/npts)) {
u[1][i] = cos(theta);
u[2][i] = sin(theta);
}
inverse(A,Ai,2);
AperB(Ai,b,Aib,2,2,2,1);
A_TperB(b,Aib,r1,2,1,2,1);
r1[1][1] = r1[1][1] - 4*Ao;
AperB(Ai,u,Aiu,2,2,2,npts);
for (i=1; i<=2; i++)
for (j=1; j<=npts; j++)
uAiu[i][j] = u[i][j] * Aiu[i][j];
for (j=1; j<=npts; j++) {
if ( (kk=(r1[1][1] / (uAiu[1][j]+uAiu[2][j]))) >= 0.0)
lambda[j] = sqrt(kk);
else
lambda[j] = -1.0; }
// Builds up B and L
for (j=1; j<=npts; j++)
L[1][j] = L[2][j] = lambda[j];
for (j=1; j<=npts; j++) {
B[1][j] = b[1][1];
B[2][j] = b[2][1]; }
for (j=1; j<=npts; j++) {
ss1[1][j] = 0.5 * ( L[1][j] * u[1][j] - B[1][j]);
ss1[2][j] = 0.5 * ( L[2][j] * u[2][j] - B[2][j]);
ss2[1][j] = 0.5 * ( -L[1][j] * u[1][j] - B[1][j]);
ss2[2][j] = 0.5 * ( -L[2][j] * u[2][j] - B[2][j]); }
AperB(Ai,ss1,Xpos,2,2,2,npts);
AperB(Ai,ss2,Xneg,2,2,2,npts);
for (j=1; j<=npts; j++) {
if (lambda[j]==-1.0) {
// points[1][j] = -1.0;
// points[2][j] = -1.0;
// points[1][j+npts] = -1.0;
// points[2][j+npts] = -1.0;
// printf("Hrmm\n");
}
else {
//printf("(%g %g)\n", Xpos[1][j], Xpos[2][j]);
// points[1][j] = Xpos[1][j];
// points[2][j] = Xpos[2][j];
// points[1][j+npts] = Xneg[1][j];
// points[2][j+npts] = Xneg[2][j];
}
}
double x1 = Xpos[1][1], y1 = Xpos[2][1];
double x2 = Xpos[1][2], y2 = Xpos[2][2];
double x3 = Xpos[1][3], y3 = Xpos[2][3];
double x4 = Xpos[1][4], y4 = Xpos[2][4];
la = 0.5*sqrt((x1-x3)*(x1-x3)+(y1-y3)*(y1-y3));
lb = 0.5*sqrt((x2-x4)*(x2-x4)+(y2-y4)*(y2-y4));
xc = (x1+x2+x3+x4)/4;
yc = (y1+y2+y3+y4)/4;
xa = (x1-xc);
ya = (y1-yc);
double d = sqrt(xa*xa+ya*ya);
if (d>0.0001)
{
xa /= d;
ya /= d;
}
}
