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; } }