Tr: how to compute central line form surface objects
Tim Hutton
T.Hutton at eastman.ucl.ac.uk
Tue Feb 22 08:06:14 EST 2000
>Code for ellipse-fitting attached
Sorry, forgot. Attached now.
Tim.
-------------- next part --------------
// PiluEllipseFit.h
//**************************************************************************
// This java code is an interactive demo of the first ellipse-specific
// direct fitting method presented in the papers:
// M. Pilu, A. Fitzgibbon, R.Fisher ``Ellipse-specific Direct
// least-square Fitting '' , IEEE International Conference on Image
// Processing, Lausanne, September 1996. (poscript) (HTML)
// A. Fitzgibbon, M. Pilu , R.Fisher ``Direct least-square fitting of
// Ellipses '' , International Conference on Pattern Recognition, Vienna,
// August 1996. (poscript) - Extended version available as DAI Research
// Paper #794
// The demo can be tried out at
// http://www.dai.ed.ac.uk/students/maurizp/ElliFitDemo/demo.html
// The code was written by Maurizio Pilu , University of Edinburgh. The
// applet's graphic interface was much inspired by the Curve Applet
// written by Michael Heinrichs at SFU, Vancouver:
// http://fas.sfu.ca:80/1/cs/people/GradStudents/heinrica/personal/curve.html
// Some math routines are from the "Numerical Recipes in C" by
// Press/Teukolsky/Vettering/Flannery, Cambridge Uiniversity Press,
// Second Edition (1988). PLEASE READ COPYRIGHT ISSUES ON THE NUMERICAL
// RECIPES BOOK.
// NOTE: Some parts of the program are rather scruffy. The author
// will tidy it up whan he has some spare time.
// DISCLAIMER: The authors and the department assume no responsabilities
// whatsoever for any wrong use of this code.
// COPYRIGHT: Any commercial use of the code and the method is forbidden
// without written authorization from the authors.
//**************************************************************************
// mindless hack into c from java by tim hutton
struct C2dVector { float x,y };
class PiluEllipseFit
{
public:
void paint(C2dVector *points,int np,CDC *pDC,C2dVector *output_points=NULL) {
double **D = new double*[np+1];
for(int c=0;c<np+1;c++) D[c]=new double[7];
double *S[7];
for(c=0;c<7;c++) S[c]=new double[7];
double *Const[7];
double *temp[7];
double *L[7],*invL[7];
double *C[7];
double *V[7];
double *sol[7];
for(c=0;c<7;c++)
{
L[c]=new double[7];
invL[c] = new double[7];
Const[c]=new double[7];
temp[c]=new double[7];
C[c]=new double[7];
V[c]=new double[7];
sol[c]=new double[7];
}
double d[7];
double tx,ty;
int nrot=0;
int npts=50;
int i,j;
double *XY[3];
XY[0]=new double[npts+1];
XY[1]=new double[npts+1];
XY[2]=new double[npts+1];
double pvec[7];
const int PT_SIZE=3;
// draw the control points if desired
if(pDC!=NULL)
{
for (i=0; i < np; i++) {
pDC->SetPixel(points[i].x,points[i].y,RGB(0,0,0));
}
}
// initialize the constraint matrix
for(i=0;i<7;i++)
for(j=0;j<7;j++)
Const[i][j]=0;
Const[1][3]=-2;
Const[2][2]=1;
Const[3][1]=-2;
// method doesn't work for fewer than 6 points
if (np<6)
return;
// Now first fill design matrix
for (i=1; i <= np; i++)
{
tx = points[i-1].x;
ty = points[i-1].y;
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;
}
//pm(Const,"Constraint");
// Now compute scatter matrix S
A_TperB(D,D,(double**)S,np,6,np,6);
//pm(S,"Scatter");
choldc((double**)S,6,(double**)L);
//pm(L,"Cholesky");
inverse((double**)L,(double**)invL,6);
//pm(invL,"inverse");
AperB_T((double**)Const,(double**)invL,(double**)temp,6,6,6,6);
AperB((double**)invL,(double**)temp,(double**)C,6,6,6,6);
//pm(C,"The C matrix");
jacobi((double**)C,6,d,(double**)V,nrot);
//pm(V,"The Eigenvectors"); /* OK */
//pv(d,"The eigevalues");
A_TperB((double**)invL,(double**)V,(double**)sol,6,6,6,6);
//pm(sol,"The GEV solution unnormalized"); /* SOl */
// Now normalize them
for (j=1;j<=6;j++) /* Scan columns */
{
double mod = 0.0;
for (i=1;i<=6;i++)
mod += sol[i][j]*sol[i][j];
for (i=1;i<=6;i++)
sol[i][j] /= sqrt(mod);
}
//pm(sol,"The GEV solution"); /* SOl */
double zero=10e-20;
double minev=10e+20;
int solind=0;
for (i=1; i<=6; i++)
{
if (d[i]<0 && fabs(d[i])>zero)
solind = i;
}
// Now fetch the right solution
for (j=1;j<=6;j++)
pvec[j] = sol[j][solind];
//pv(pvec,"the solution");
// ...and plot it
draw_conic(pvec,npts,XY);
// actually draw to screen if desired
if(pDC!=NULL)
{
for (i=1; i<npts; i++)
{
if (XY[1][i]==-1 || XY[1][i+1]==-1 )
continue;
else if (i<npts-1)
{
pDC->MoveTo((int)XY[1][i],(int)XY[2][i]);
pDC->LineTo((int)XY[1][i+1],(int)XY[2][i+1]);
}
else
{
pDC->MoveTo((int)XY[1][i],(int)XY[2][i]);
pDC->LineTo((int)XY[1][1],(int)XY[2][1]);
}
}
}
// return the ellipse points in output_points if desired
if(output_points!=NULL)
{
for (i=1; i<npts; i++)
output_points[i] = C2dVector((int)XY[1][i],(int)XY[2][i]);
}
// DEBUG: display ellipse rotation
float theta = 0.5*atan(pvec[2]/(pvec[1]-pvec[3]));
CString text;
text.Format("theta=%.2f degrees",theta*180.0F/3.1415926535);
AfxMessageBox(text);
}
protected:
void multMatrix(double m[4][4], double g[4][2], double mg[4][2])
{
// This function performs the meat of the calculations for the
// curve plotting. Note that it is not a matrix multiplier in the
// pure sense. The first matrix is the curve matrix (each curve type
// has its own matrix), and the second matrix is the geometry matrix
// (defined by the control points). The result is returned in the
// third matrix.
int i,j,k;
// First clear the return array
for(i=0; i<4; i++)
for(j=0; j<2; j++)
mg[i][j]=0;
// Perform the matrix math
for(i=0; i<4; i++)
for(j=0; j<2; j++)
for(k=0; k<4; k++)
mg[i][j]=mg[i][j] + (m[i][k] * g[k][j]);
}
void ROTATE(double **a, int i, int j, int k, int l, double tau, double s)
{
double g,h;
g=a[i][j];h=a[k][l];a[i][j]=g-s*(h+g*tau);
a[k][l]=h+s*(g-h*tau);
}
void jacobi(double **a, int n, double *d , double **v, int nrot)
{
int j,iq,ip,i;
double tresh,theta,tau,t,sm,s,h,g,c;
double *b = new double[n+1];
double *z = new double[n+1];
for (ip=1;ip<=n;ip++) {
for (iq=1;iq<=n;iq++) v[ip][iq]=0.0;
v[ip][ip]=1.0;
}
for (ip=1;ip<=n;ip++) {
b[ip]=d[ip]=a[ip][ip];
z[ip]=0.0;
}
nrot=0;
for (i=1;i<=50;i++) {
sm=0.0;
for (ip=1;ip<=n-1;ip++) {
for (iq=ip+1;iq<=n;iq++)
sm += fabs(a[ip][iq]);
}
if (sm == 0.0) {
/* free_vector(z,1,n);
free_vector(b,1,n); */
return;
}
if (i < 4)
tresh=0.2*sm/(n*n);
else
tresh=0.0;
for (ip=1;ip<=n-1;ip++) {
for (iq=ip+1;iq<=n;iq++) {
g=100.0*fabs(a[ip][iq]);
if (i > 4 && fabs(d[ip])+g == fabs(d[ip])
&& fabs(d[iq])+g == fabs(d[iq]))
a[ip][iq]=0.0;
else if (fabs(a[ip][iq]) > tresh) {
h=d[iq]-d[ip];
if (fabs(h)+g == fabs(h))
t=(a[ip][iq])/h;
else {
theta=0.5*h/(a[ip][iq]);
t=1.0/(fabs(theta)+sqrt(1.0+theta*theta));
if (theta < 0.0) t = -t;
}
c=1.0/sqrt(1+t*t);
s=t*c;
tau=s/(1.0+c);
h=t*a[ip][iq];
z[ip] -= h;
z[iq] += h;
d[ip] -= h;
d[iq] += h;
a[ip][iq]=0.0;
for (j=1;j<=ip-1;j++) {
ROTATE(a,j,ip,j,iq,tau,s);
}
for (j=ip+1;j<=iq-1;j++) {
ROTATE(a,ip,j,j,iq,tau,s);
}
for (j=iq+1;j<=n;j++) {
ROTATE(a,ip,j,iq,j,tau,s);
}
for (j=1;j<=n;j++) {
ROTATE(v,j,ip,j,iq,tau,s);
}
++nrot;
}
}
}
for (ip=1;ip<=n;ip++) {
b[ip] += z[ip];
d[ip]=b[ip];
z[ip]=0.0;
}
}
//printf("Too many iterations in routine JACOBI");
}
// Perform the Cholesky decomposition
// Return the lower triangular L such that L*L'=A
void choldc(double **a, int n, double **l)
{
int i,j,k;
double sum;
double *p = new double[n+1];
for (i=1; i<=n; i++) {
for (j=i; j<=n; j++) {
for (sum=a[i][j],k=i-1;k>=1;k--) sum -= a[i][k]*a[j][k];
if (i == j) {
if (sum<=0.0)
// printf("\nA is not poitive definite!");
{}
else
p[i]=sqrt(sum); }
else
{
a[j][i]=sum/p[i];
}
}
}
for (i=1; i<=n; i++)
for (j=i; j<=n; j++)
if (i==j)
l[i][i] = p[i];
else
{
l[j][i]=a[j][i];
l[i][j]=0.0;
}
}
/********************************************************************/
/** Calcola la inversa della matrice B mettendo il risultato **/
/** in InvB . Il metodo usato per l'inversione e' quello di **/
/** Gauss-Jordan. N e' l'ordine della matrice . **/
/** ritorna 0 se l'inversione corretta altrimenti ritorna **/
/** SINGULAR . **/
/********************************************************************/
int inverse(double **TB, double **InvB, int N) {
int k,i,j,p,q;
double mult;
double D,temp;
double maxpivot;
int npivot;
double **B = new double*[N+1];
double **A = new double*[N+1];
double **C = new double*[N+1];
for(i=0;i<N+1;i++)
{
B[i]=new double[N+2];
A[i]=new double[2*N+2];
C[i]=new double[N+1];
}
double eps = 10e-20;
for(k=1;k<=N;k++)
for(j=1;j<=N;j++)
B[k][j]=TB[k][j];
for (k=1;k<=N;k++)
{
for (j=1;j<=N+1;j++)
A[k][j]=B[k][j];
for (j=N+2;j<=2*N+1;j++)
A[k][j]=(float)0;
A[k][k-1+N+2]=(float)1;
}
for (k=1;k<=N;k++)
{
maxpivot=fabs((double)A[k][k]);
npivot=k;
for (i=k;i<=N;i++)
if (maxpivot<fabs((double)A[i][k]))
{
maxpivot=fabs((double)A[i][k]);
npivot=i;
}
if (maxpivot>=eps)
{ if (npivot!=k)
for (j=k;j<=2*N+1;j++)
{
temp=A[npivot][j];
A[npivot][j]=A[k][j];
A[k][j]=temp;
} ;
D=A[k][k];
for (j=2*N+1;j>=k;j--)
A[k][j]=A[k][j]/D;
for (i=1;i<=N;i++)
{
if (i!=k)
{
mult=A[i][k];
for (j=2*N+1;j>=k;j--)
A[i][j]=A[i][j]-mult*A[k][j] ;
}
}
}
else
{ // printf("\n The matrix may be singular !!") ;
return(-1);
};
}
/** Copia il risultato nella matrice InvB ***/
for (k=1,p=1;k<=N;k++,p++)
for (j=N+2,q=1;j<=2*N+1;j++,q++)
InvB[p][q]=A[k][j];
return(0);
} /* End of INVERSE */
void AperB(double **_A, double **_B, double **_res,
int _righA, int _colA, int _righB, int _colB) {
int p,q,l;
for (p=1;p<=_righA;p++)
{
for (q=1;q<=_colB;q++)
{
_res[p][q]=0.0;
for (l=1;l<=_colA;l++)
{
_res[p][q]=_res[p][q]+_A[p][l]*_B[l][q];
}
}
}
}
void A_TperB(double **_A, double **_B, double **_res,
int _righA, int _colA, int _righB, int _colB) {
int p,q,l;
for (p=1;p<=_colA;p++)
{
for (q=1;q<=_colB;q++)
{
_res[p][q]=0.0;
for (l=1;l<=_righA;l++)
{
_res[p][q]=_res[p][q]+_A[l][p]*_B[l][q];
}
}
}
}
void AperB_T(double **_A, double **_B, double **_res,
int _righA, int _colA, int _righB, int _colB) {
int p,q,l;
for (p=1;p<=_colA;p++)
{
for (q=1;q<=_colB;q++)
{
_res[p][q]=0.0;
for (l=1;l<=_righA;l++)
{
_res[p][q]=_res[p][q]+_A[p][l]*_B[q][l];
}
}
}
}
void draw_conic(double *pvec, int nptsk, double **points)
{
int npts=nptsk/2;
double *u[3];
double *Aiu[3];
double *L[3];
double *B[3];
double *Xpos[3];
double *Xneg[3];
double *ss1[3];
double *ss2[3];
double *uAiu[3];
for(int i=0;i<3;i++)
{
u[i] = new double[npts+1];
Aiu[i] = new double[npts+1];
L[i] = new double[npts+1];
B[i] = new double[npts+1];
Xpos[i] = new double[npts+1];
Xneg[i] = new double[npts+1];
ss1[i] = new double[npts+1];
ss2[i] = new double[npts+1];
uAiu[i] = new double[npts+1];
}
double *lambda = new double[npts+1];
int c;
double *A[3];
for(c=0;c<3;c++) A[c]=new double[3];
double *Ai[3];
for(c=0;c<3;c++) Ai[c]=new double[3];
double *Aib[3];
for(c=0;c<3;c++) Aib[c]=new double[2];
double *b[3];
for(c=0;c<3;c++) b[c]=new double[2];
double *r1[2];
for(c=0;c<2;c++) r1[c]=new double[2];
double Ao, Ax, Ay, Axx, Ayy, Axy;
double pi = 3.14781;
double theta;
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;
// Generate normals linspace
for (i=1, theta=0.0; i<=npts; i++, theta+=(pi/npts)) {
u[1][i] = cos(theta);
u[2][i] = sin(theta); }
inverse((double**)A,(double**)Ai,2);
AperB((double**)Ai,(double**)b,(double**)Aib,2,2,2,1);
A_TperB((double**)b,(double**)Aib,(double**)r1,2,1,2,1);
r1[1][1] = r1[1][1] - 4*Ao;
AperB((double**)Ai,(double**)u,(double**)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((double**)Ai,(double**)ss1,(double**)Xpos,2,2,2,npts);
AperB((double**)Ai,(double**)ss2,(double**)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;
}
else {
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];
}
}
}
};
More information about the vtkusers
mailing list