icp.c

Image Processing, Analysis, and Machine Vision 3rd Edition (2007)

DoubleT IcpTriangClosestPoint(DoubleT point[],long tr[],PointsT modPnt,DoubleT rPoint[])
/********************************************************************
 * Computes the closest point on a given triangle to a given point.	*
 * Returns distance between them.									*
 * point        - given point (3 vector)							*
 * tr           - given triangle (3 vector of vertices' indices)	*
 * modPnt       - array of vertices of triangles					*
 * rPoint       - resulting point									*
 ********************************************************************/
{
	int i,j,maxi=0,i1,i2;
	DoubleT a=0,b=0,c=0,d=0,e=0,u,v,w,p2v[3],max=0,f=0,g=0,t;

	for (i=0;i<3;i++)
	{
		a+=(modPnt[tr[0]][i]-modPnt[tr[2]][i])*(modPnt[tr[0]][i]-modPnt[tr[2]][i]);
		b+=(modPnt[tr[0]][i]-modPnt[tr[2]][i])*(modPnt[tr[1]][i]-modPnt[tr[2]][i]);
		c+=(modPnt[tr[2]][i]-point[i])*(modPnt[tr[0]][i]-modPnt[tr[2]][i]);
		d+=(modPnt[tr[1]][i]-modPnt[tr[2]][i])*(modPnt[tr[1]][i]-modPnt[tr[2]][i]);
		e+=(modPnt[tr[2]][i]-point[i])*(modPnt[tr[1]][i]-modPnt[tr[2]][i]);
	}
	u=(b*e-c*d)/(a*d-b*b);
	v=-(e+b*u)/d;
	w=1-u-v;
	if (u<0 || u>1 || v<0 || v>1 || w<0 || w>1)
	{
		for (i=0;i<3;i++)
		{
			p2v[i]=0;
			for (j=0;j<3;j++)
				p2v[i]+=(modPnt[tr[i]][j]-point[j])*(modPnt[tr[i]][j]-point[j]);
			if (p2v[i]>max)
			{
				max=p2v[i];
				maxi=i;
			}
		}
		if (maxi==2) {i1=0;i2=1;}
		else
			if (maxi==1) {i1=0;i2=2;}
			else
				{i1=1;i2=2;}
		for (i=0;i<3;i++)
		{
			f+=(modPnt[tr[i1]][i]-point[i])*(modPnt[tr[i1]][i]-modPnt[tr[i2]][i]);
			g+=(modPnt[tr[i2]][i]-modPnt[tr[i1]][i])*(modPnt[tr[i2]][i]-modPnt[tr[i1]][i]);
		}
		t=f/g;
		if (t<0) t=0;
		else if (t>1) t=1;
		for (i=0;i<3;i++) rPoint[i]=(1-t)*modPnt[tr[i1]][i]+t*modPnt[tr[i2]][i];
	}
	else for (i=0;i<3;i++) rPoint[i]=u*modPnt[tr[0]][i]+v*modPnt[tr[1]][i]+w*modPnt[tr[2]][i];
	return((rPoint[0]-point[0])*(rPoint[0]-point[0])+(rPoint[1]-point[1])*(rPoint[1]-point[1])+(rPoint[2]-point[2])*(rPoint[2]-point[2]));
}


DoubleT IcpNoEdgeTriangClosestPoint(DoubleT point[],long tr[],PointsT modPnt,DoubleT rPoint[])
/************************************************************************
 * Computes the closest point on a given triangle to a given point.		*
 * Returns distance between them. In case that the closest point isn't	*
 * found inside of triangle returns DBL_MAX.							*
 * point        - given point (3 vector)								*
 * tr           - given triangle (3 vector of vertices' indices)		*
 * modPnt       - array of vertices of triangles						*
 * rPoint       - resulting point										*
 ************************************************************************/
{
	int i;
	DoubleT a=0,b=0,c=0,d=0,e=0,u,v,w;

	for (i=0;i<3;i++)
	{
		a+=(modPnt[tr[0]][i]-modPnt[tr[2]][i])*(modPnt[tr[0]][i]-modPnt[tr[2]][i]);
		b+=(modPnt[tr[0]][i]-modPnt[tr[2]][i])*(modPnt[tr[1]][i]-modPnt[tr[2]][i]);
		c+=(modPnt[tr[2]][i]-point[i])*(modPnt[tr[0]][i]-modPnt[tr[2]][i]);
		d+=(modPnt[tr[1]][i]-modPnt[tr[2]][i])*(modPnt[tr[1]][i]-modPnt[tr[2]][i]);
		e+=(modPnt[tr[2]][i]-point[i])*(modPnt[tr[1]][i]-modPnt[tr[2]][i]);
	}
	u=(b*e-c*d)/(a*d-b*b);
	v=-(e+b*u)/d;
	w=1-u-v;
	if (u>=0 && u<=1 && v>=0 && v<=1 && w>=0 && w<=1)
	{
		for (i=0;i<3;i++) rPoint[i]=u*modPnt[tr[0]][i]+v*modPnt[tr[1]][i]+w*modPnt[tr[2]][i];
		return((rPoint[0]-point[0])*(rPoint[0]-point[0])+(rPoint[1]-point[1])*(rPoint[1]-point[1])+(rPoint[2]-point[2])*(rPoint[2]-point[2]));
	}
	else return(DBL_MAX);
}


long IcpPsetClosestPoint(DoubleT pnt1[],ShapeT *shp,DoubleT rPoint[])
/************************************************************************
 * Computes the closest point on a point set shape shp to a given point	*
 * pnt1		- given point												*
 * shp		- given shape												*
 * rPoint	- resulting point											*
 ************************************************************************/
{
	long j,n,idxmin=0;
	PointsT ps;
	DoubleT d,dmin=DBL_MAX;

	n=(shp->pset)->n;
	ps=(shp->pset)->ps;
	
	for (j=0;j<n;j++)
	{
		d=(ps[j][0]-pnt1[0])*(ps[j][0]-pnt1[0])+(ps[j][1]-pnt1[1])*(ps[j][1]-pnt1[1])+(ps[j][2]-pnt1[2])*(ps[j][2]-pnt1[2]);
		if (d<dmin)
		{
			dmin=d;
			idxmin=j;
		}
	}
	rPoint[0]=ps[idxmin][0];rPoint[1]=ps[idxmin][1];rPoint[2]=ps[idxmin][2];
	return(idxmin);
}



long IcpTsetClosestPoint(DoubleT point[],ShapeT *shp,DoubleT yps[])
/********************************************************************
 * Finds the closest point on a triangle set shape to a given point.*
 * point	- given point											*
 * shp		- shape in triangle set form							*
 * yps		- resulting point										*
 ********************************************************************/
{
	long j,idxmin;
	DoubleT dist,dmin=DBL_MAX,p[3];
	PointsT modPnt;

	modPnt=(shp->pset)->ps;
	for (j=0;j<(shp->tset)->n;j++)
	{
		dist=IcpTriangClosestPoint(point,(shp->tset)->ts[j],modPnt,p);
		if (dist<dmin)
		{
			dmin=dist;
			idxmin=j;
			yps[0]=p[0];yps[1]=p[1];yps[2]=p[2];
		}
	}
	return(idxmin);
}


long IcpGradTsetClosestPoint(DoubleT point[],ShapeT *shp,DoubleT yps[],SurroundT surr,long couple)
/************************************************************************
 * Finds the closest point on a triangle set shape to a given point.	*
 * Starts searching on a 'couple' triangle and searches shape 			*
 * by finding minimum on surrounding triangles							*
 * point	- given point												*
 * shp		- shape in triangle set form								*
 * yps		- resulting point											*
 * surr		- list of surrounding triangles for each triangle of shape	*
 * couple	- index of starting triangle								*
 ************************************************************************/
{
	long n,idxmin,nextidxm,surrsize;
	int j,found;
	DoubleT d,dmin,p[3];
	PointsT modPnt;

	modPnt=(shp->pset)->ps;
	n=(shp->tset)->n;
	dmin=IcpTriangClosestPoint(point,(shp->tset)->ts[couple],modPnt,p);
	yps[0]=p[0];yps[1]=p[1];yps[2]=p[2];
	idxmin=couple;
	do
	{
		found=0;
		surrsize=surr[idxmin][0];
		for (j=1;j<=surrsize;j++)
		{
			d=IcpTriangClosestPoint(point,(shp->tset)->ts[surr[idxmin][j]],modPnt,p);
			if (d<dmin)
			{
				dmin=d;
				nextidxm=surr[idxmin][j];
				yps[0]=p[0];yps[1]=p[1];yps[2]=p[2];
				found=1;
			}
		}
		if (found) idxmin=nextidxm;
	}
	while (found);
	return(idxmin);
}


long IcpPartTsetClosestPoint(DoubleT point[],ShapeT *shp,DoubleT yps[],SurroundT surr,long couple,DoubleT part)
/************************************************************************
 * Finds the closest point on a triangle set to a given point.			*
 * Starts searching on a 'couple' triangle and searches shape until 	*
 * reach given part of all triangles.									*
 * point	- given point												*
 * shp		- shape in triangle set form								*
 * yps		- resulting point											*
 * surr		- list of surrounding triangles for each triangle of shape	*
 * couple	- index of starting triangle								*
 * part		- part of triangles to be searched (0.1 = 10%)				*
 ************************************************************************/
{
	long n,k,lastSelected,lastUnfolded,*selected,mini;
	int j,*info;
	PointsT modPnt;
	DoubleT d,mind,h,p[3];

	n=(shp->tset)->n;
	if ((selected=(long *) malloc(n*sizeof(long)))==NULL) IcpMemError();
	if ((info=(int *) malloc(n*sizeof(int)))==NULL) IcpMemError();
	modPnt=(shp->pset)->ps;
	for (k=0;k<n;k++) info[k]=0;
	selected[0]=couple;
	info[couple]=1;
	lastUnfolded=-1;
	lastSelected=0;
	mind=DBL_MAX;
	do
	{
		lastUnfolded++;
		k=lastSelected;
		d=IcpTriangClosestPoint(point,(shp->tset)->ts[selected[k]],modPnt,p);
		if (d<mind)
		{
			mind=d;
			mini=selected[k];
			yps[0]=p[0];yps[1]=p[1];yps[2]=p[2];
		}
		for (j=1;j<=surr[selected[lastUnfolded]][0];j++)
		{
			if (!info[surr[selected[lastUnfolded]][j]])
			{
				lastSelected++;
				selected[lastSelected]=surr[selected[lastUnfolded]][j];
				info[surr[selected[lastUnfolded]][j]]=1;
			}
		}
		h=(DoubleT) (lastSelected+1)/n;
	}
	while ( h < part && ( lastSelected > lastUnfolded ) );
	free(info);
	free(selected);
	return(mini);
}


DoubleT IcpPsetSize(PointSetT *pset)
/********************************************************************
 * Computes rough estimate of size of object defined by point set 	*
 * by computing trace of covariance matrix of given point set.		*
 * pset		- given point set										*
 ********************************************************************/
{
	DoubleT mi[3]={0,0,0},trace=0,covarM[3][3]={{0,0,0},{0,0,0},{0,0,0}};
	long i,n;
	int j,k;
	PointsT pnt;

	n=pset->n;
	pnt=pset->ps;
	for (j=0;j<3;j++)
	{
		for (i=0;i<n;i++)
		{
			mi[j]+=pnt[i][j];
			for (k=0;k<3;k++) covarM[j][k]+=pnt[i][j]*pnt[i][k];
		}
		mi[j]/=n;
	}
	for (j=0;j<3;j++)
		for (k=0;k<3;k++)
		{
			covarM[j][k]/=n;
			covarM[j][k]-=mi[j]*mi[k];
		}
	for (j=0;j<3;j++) trace+=covarM[j][j];
	return(sqrt(trace));
}


void IcpCompRegistration(PointSetT *pset,PointsT y,PointsT pk,RegResultsT *res,int elim[],long nact)
/********************************************************************************
 * Computes and applies registration											*
 * pset		- data point set													*
 * y		- closest points of model shape										*
 * pk		- points after registration											*
 * res      - results of registration											*
 * elim		- array defining which points to use (0) and which to eliminate (1)	*
 * nact 	- number of actualy used (not eliminated) points					*
 ********************************************************************************/
{
	DoubleT trace=0,mip[3]={0,0,0},miy[3]={0,0,0},crossCovarM[3][3]={{0,0,0},{0,0,0},{0,0,0}},qsymM[4][4];
	DoubleT matrix[10],vectors[16],values[4];
	DoubleT rotM[3][3]={{1,0,0},{0,1,0},{0,0,1}},maxEV=0;
	int j,k,l=0,maxEVix=0;
	long n,i;
	PointsT pnt;

	n=pset->n;
	pnt=pset->ps;
	for (j=0;j<3;j++)
	{
		for (i=0;i<n;i++)
		{
			if (!elim[i])
			{
				mip[j]+=pnt[i][j];
				miy[j]+=y[i][j];
				for (k=0;k<3;k++)
					crossCovarM[j][k]+=pnt[i][j]*y[i][k];
			}
		}
		mip[j]/=nact;
		miy[j]/=nact;
	}
	for (j=0;j<3;j++)
	for (k=0;k<3;k++)
	{
		crossCovarM[j][k]/=nact;
		crossCovarM[j][k]-=mip[j]*miy[k];
	}
/*
	printf("Cross-covariance matrix:\n");
	for (i=0;i<3;i++)
	{
		for (j=0;j<3;j++)
		printf("%f ",crossCovarM[i][j]);
		printf("\n");
	}
	printf("miP - x: %f y: %f z: %f\n",mip[0],mip[1],mip[2]);
	printf("miY - x: %f y: %f z: %f\n",miy[0],miy[1],miy[2]);
	getchar();
*/
	for (j=0;j<3;j++) trace+=crossCovarM[j][j];
	qsymM[0][1]=qsymM[1][0]=crossCovarM[1][2]-crossCovarM[2][1];
	qsymM[0][2]=qsymM[2][0]=crossCovarM[2][0]-crossCovarM[0][2];
	qsymM[0][3]=qsymM[3][0]=crossCovarM[0][1]-crossCovarM[1][0];
	for (j=1;j<4;j++)
	{
		for (k=j;k<4;k++)
		{
			qsymM[j][k]=qsymM[k][j]=crossCovarM[j-1][k-1]+crossCovarM[k-1][j-1]-((j==k)?trace:0);
		}
	}
	qsymM[0][0]=trace;

#ifdef MATRIXL
	printf("Q sym matrix:\n");
	for (i=0;i<4;i++)
	{
		for (j=0;j<4;j++) printf("%f ",qsymM[i][j]);
		printf("\n");
	}
#endif
	for (j=0;j<4;j++)
		for (k=0;k<=j;k++)
			matrix[l++]=qsymM[j][k];

	eigens(matrix,vectors,values,4);
	for (j=0;j<4;j++)
	{
#ifdef MATRIXL
		printf("Eig.val. - %d. : %f\n",j,values[j]);
#endif
		if ((values[j])>maxEV)
		{
			maxEV=(values[j]);
			maxEVix=j;
		}
	}
#ifdef MATRIXL
	printf("Max EV index: %d\n",maxEVix);
	/*getchar();*/
#endif
	for (j=0;j<4;j++)
	{
		res->q[j]=vectors[4*maxEVix+j]*((vectors[4*maxEVix]>0) ? 1 : -1);
#ifdef VECTOR
		printf("%f ",res->q[j]);
#endif
	}
#ifdef VECTOR
	printf("\n");
#endif
	/*getchar();*/
	IcpCompRotMatrix(res->q,rotM);
	for (j=0;j<3;j++)
		res->q[j+4]=miy[j]-(rotM[j][0]*mip[0]+rotM[j][1]*mip[1]+rotM[j][2]*mip[2]);
	IcpApplyRegistration(res->q+4,rotM,pset,pk);
}


void IcpLocalReg(ShapeT *datashp,ShapeT *modelshp,PointsT pk,RegParametersT *par,RegResultsT *res,SurroundT surrm,SurroundT surrd,long couples[],PointsT y,int elim[],FILE *fwreg,FILE *fwdms)
/************************************************************************
 * Registers two given shapes											*
 * datashp	- data shape												*
 * modelshp - model shape												*
 * pk		- array of points used as array containing registered 		*
 *			  points of data shape in each iteration					*
 * par 		- parameters of registration								*
 * res 		- results of registration									*
 * surrm    - list of neighbouring triangles of model shape				*
 * surrd	- list of neighbouring triangles of data shape				*
 * couples  - array of indices used as array containing index of 		*
 *			  closest point of model shape found in last iteration for 	*
 *			  each point of data shape									*
 * y		- array of points used as array of closest points			*
 * elim		- used as array containing info	which points to eliminate	*
 * fwreg	- file pointer for writing registration state vector q 		*
 * fwdms	- file pointer for writing ms error 						*
 ************************************************************************/
{
	int k=0,steps=0,j;
	long n,i,nact=0,numCorr,(*corresp)[2];