icp.c

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

/***************************************************************
 *
 *		Iterative Closest (Reciprocal) Point Algoritmus
 *
 *		Basic functions
 *
 *		C file: 	icp.c
 *
 *		Author:		Pavel Kucera
 *
 *		Language:	C
 *
 *		09/11/96
 *
 ***************************************************************/

#define TIMING
#undef MATRIXL
#undef PSETLIST
#undef DMSLIST
#define ICPOUT
#undef VECTOR

#include <stdio.h>
#include <stdlib.h>
#include <float.h>
#include <math.h>
#include <string.h>
#ifdef TIMING
#include <time.h>
#endif


#include "icp.h"

void IcpMemError(void)
/********************************************************************
 * Message in case of low memory									*
 ********************************************************************/
{
	printf("Memory allocation failed! \n");
	exit(1);
}

void IcpPrintPointArray(long n,PointsT pnt)
/********************************************************************
 * Prints n points of array											*
 * n 	- number of points to print									*
 * pnt	- array of points											*
 ********************************************************************/
{
	long i;
	int j;

	for (i=0;i<n;i++)
	{
		for (j=0;j<3;j++) printf("%f ",pnt[i][j]);
		printf("\n");
	}
}

void IcpCompRotMatrix(DoubleT q[],DoubleT rotM[][3])
/********************************************************************
 * Computes rotation matrix											*
 * q[0..3]	- unit quaternion (rotation vector)						*
 * rotM		- rotation matrix (3x3)									*
 ********************************************************************/
{	rotM[0][0]=q[0]*q[0]+q[1]*q[1]-q[2]*q[2]-q[3]*q[3];
	rotM[0][1]=2*(q[1]*q[2]-q[0]*q[3]);
	rotM[0][2]=2*(q[1]*q[3]+q[0]*q[2]);
	rotM[1][0]=2*(q[1]*q[2]+q[0]*q[3]);
	rotM[1][1]=q[0]*q[0]+q[2]*q[2]-q[1]*q[1]-q[3]*q[3];
	rotM[1][2]=2*(q[2]*q[3]-q[0]*q[1]);
	rotM[2][0]=2*(q[1]*q[3]-q[0]*q[2]);
	rotM[2][1]=2*(q[2]*q[3]+q[0]*q[1]);
	rotM[2][2]=q[0]*q[0]+q[3]*q[3]-q[1]*q[1]-q[2]*q[2];
}


void IcpApplyRegistration(DoubleT t[],DoubleT rotMx[][3],PointSetT *pset,PointsT rPnts)
/************************************************************************
 * Applies registration on point set									*
 * t[]          translation vector (3 vector)							*
 * rotMx        rotation matrix											*
 * pset         point set registration is applyed on					*
 * resPnts      resulting points										*
 ************************************************************************/
{
	long i;
	int j;

	for (i=0;i<pset->n;i++)
		for (j=0;j<3;j++)	rPnts[i][j]=(rotMx[j][0]*pset->ps[i][0])+(rotMx[j][1]*pset->ps[i][1])+(rotMx[j][2]*pset->ps[i][2])+t[j];
}


void IcpTransformPset(PointSetT *psetin,PointSetT *psetout,DoubleT q[])
/********************************************************************
 * Transforms given point set by transformation vector q.			*
 * psetin	- given point set										*
 * psetout	- resulting point set									*
 * q		- transformation vector (0-3 rotation, 4-6 translation)	*
 ********************************************************************/
{
	DoubleT rotM[3][3];
	PointsT pnt;
	long i;
	int j;

	if ((pnt=(PointsT) malloc((psetin->n)*3*sizeof(DoubleT)))==NULL) IcpMemError();
	IcpCompRotMatrix(q,rotM);
	IcpApplyRegistration(q+4,rotM,psetin,pnt);
	for (i=0;i<psetin->n;i++) for (j=0;j<3;j++) psetout->ps[i][j]=pnt[i][j];
	free(pnt);
}


void IcpReadParamFile(char paramFileName[],char dataFileName[],long *ndat1,long *ndat2,RepresentationT *drep,char modelFileName[],long *nmod1,long *nmod2,RepresentationT *mrep,DoubleT *q,RegParametersT *param)
/****************************************************************************
 * Reads file with registration parameters									*
 * paramFileName- name of parameters file to be read						*
 * dataFileName	- name of file(s) containing data shape						*
 * ndat1,ndat2  - numbers of data elements (0,points or triangles,vertices) *
 * drep         - representation type of data shape                         *
 * modelFileName- name of file(s) containing model shape					*
 * nmod1,nmod2	- numbers of model elements (0,points or triangles,vertices)*
 * mrep         - representation type of model shape	
 * q            - initial transformation 					*
 * param	- registration parameters structure							*
 ****************************************************************************/
{
	FILE *fr;
	char s[90];
	int j;

	if ((fr=fopen(paramFileName,"r"))==NULL)
	{
		printf("Unable to open the file '%s'! \n",paramFileName);
		exit(1);
	}
	while (fscanf(fr,"%s",s) != EOF)
	{
		if (s[0]=='#')
		{
			fgets(s,90,fr);
			continue;
		}
		if (strcmp(s,"datafilename")==0)
		{
			fscanf(fr,"%s",s);
			strcpy(dataFileName,s);
			continue;
		} else
		if (strcmp(s,"inittrans")==0)
		{
		      fscanf(fr,"%lf %lf %lf %lf %lf %lf %lf",&q[0],&q[1],&q[2],&q[3],&q[4],&q[5],&q[6]);
		       continue;
		} else
                if (strcmp(s,"rangesearch")==0)
		{
			fscanf(fr,"%d",&IcpRangeSearchFlag);
			continue;
		}

		else
			if (strcmp(s,"threshold")==0)
			{
				fscanf(fr,"%lf",&(param->thr));
				continue;
			}
			else
				if (strcmp(s,"global")==0)
				{
					fscanf(fr,"%d",&(param->global));
					continue;
				}
				else
					if (strcmp(s,"iidepth")==0)
					{
						fscanf(fr,"%d",&(param->iidepth));
						continue;
					}
					else
						if (strcmp(s,"fastercp")==0)
						{
							fscanf(fr,"%d",&(param->fastercp));
							continue;
						}
						else
							if (strcmp(s,"correspinfo")==0)
							{
								fscanf(fr,"%d",&(param->correspinfo));
								continue;
							}
							else
								if (strcmp(s,"searchedpart")==0)
								{
									fscanf(fr,"%lf",&(param->searchedpart));
									continue;
								}
								else
									if (strcmp(s,"modelfilename")==0)
									{
										fscanf(fr,"%s",s);
										strcpy(modelFileName,s);
										continue;
									}
									else
										if (strcmp(s,"modelelements")==0)
										{
											fscanf(fr,"%ld %ld",nmod1,nmod2);
											continue;
										}
										else
											if (strcmp(s,"dataelements")==0)
											{
												fscanf(fr,"%ld %ld",ndat1,ndat2);
												continue;
											}
											else
												if (strcmp(s,"centering")==0)
												{
													fscanf(fr,"%d",&(param->center));
													continue;
												}
												else
													if (strcmp(s,"eliminate")==0)
													{
														fscanf(fr,"%d",&(param->eliminate));
														continue;
													}
													else
														if (strcmp(s,"datarepr")==0)
														{
															fscanf(fr,"%s",s);
															if (strcmp(s,"TSET")==0) *drep=TSET;
															else
																if (strcmp(s,"PSET")==0) *drep=PSET;
															continue;
														}
														else
															if (strcmp(s,"modelrepr")==0)
															{
																fscanf(fr,"%s",s);
																if (strcmp(s,"TSET")==0) *mrep=TSET;
																else
																	if (strcmp(s,"PSET")==0) *mrep=PSET;
															}
															else
																if (strcmp(s,"elimit")==0)
																{
																	fscanf(fr,"%lf",&(param->elimit));
																	continue;
																}
	}
	fclose(fr);
}

void IcpReadTsetFiles(char fileName[],long tnum,long pnum,ShapeT *shp)
/****************************************************************************
 * Reads file with triangle set and fills structure shp with its contents.	*
 * fileName	- name of files containing triangle set shape 					*
 * 			  (without extensions .vtx and .tri)							*
 * tnum		- number of triangles											*
 * pnum		- number of vertices											*
 * shp		- shape structure												*
 ****************************************************************************/
{
	long i;
	int j;
	DoubleT h;
	FILE *fr;
	char fName[256];
	TriangSetT *tset;
	PointSetT *pset;
	TrianglesT trng;
	PointsT pnt;

	if ((tset=(TriangSetT *) malloc(sizeof(TriangSetT)))==NULL) IcpMemError();
	if ((pset=(PointSetT *) malloc(sizeof(PointSetT)))==NULL) IcpMemError();
	pset->n=pnum;
	if ((pnt=(PointsT) malloc(pnum*3*sizeof(DoubleT)))==NULL) IcpMemError();
	pset->ps=pnt;
	strcpy(fName,fileName);
	strcat(fileName,".vtx");
	if ((fr=fopen(fileName,"r"))==NULL)
	{
		printf("Unable to open the file '%s'! \n",fileName);
		exit(1);
	}
	for (i=0;i<pnum;i++) for (j=0;j<3;j++) fscanf(fr,"%lf ",*(pnt+i)+j);
	tset->n=tnum;
	strcat(fName,".tri");
	if ((fr=fopen(fName,"r"))==NULL)
	{
		printf("Unable to open the file '%s'! \n",fName);
		exit(1);
	}
	if ((trng=(TrianglesT) malloc(tnum*3*sizeof(long)))==NULL) IcpMemError();
	tset->ts=trng;
	for (i=0;i<tnum;i++)
	{
		for (j=0;j<3;j++)
		{
			fscanf(fr,"%lf ",&h);
			trng[i][j]=(long) h;
		}
	}
	shp->repr=TSET;
	shp->tset=tset;
	shp->pset=pset;
	fclose(fr);
}


void IcpReadPsetFile(char fileName[],long num,ShapeT *shp)
/************************************************************************
 * Reads file with point set and fills structure shp with its contents	*
 * fileName	- name of file containing point set shape					*
 * 			  (without extension .vtx)									*
 * num		- number of points											*
 * shp		- shape structure											*
 ************************************************************************/
{
	long i;
	int j;
	FILE *fr;
	PointSetT *pset;
	PointsT pnt;

	if ((pset=(PointSetT *) malloc(sizeof(PointSetT)))==NULL) IcpMemError();
	strcat(fileName,".vtx");
	if ((fr=fopen(fileName,"r"))==NULL)
	{
		printf("Unable to open the file '%s'! \n",fileName);
		exit(1);
	}
	pset->n=num;
	if ((pnt=(PointsT) malloc(num*3*sizeof(DoubleT)))==NULL) IcpMemError();
	pset->ps=pnt;
	for (i=0;i<num;i++)	for (j=0;j<3;j++) fscanf(fr,"%lf ",*(pnt+i)+j);
	shp->repr=PSET;
	shp->pset=pset;
	fclose(fr);
}

void IcpWritePsetFile(char fileName[],long np,PointsT ps)
/****************************************************************
 * Writes ascii file with points' coordinates					*
 * fileName	- name of resulting file							*
 * np		- number of points									*
 * ps		- array of points									*
 ****************************************************************/
{
	long i;
	int j;
	FILE *fw;

	if ((fw=fopen(fileName,"w"))==NULL)
	{
		printf("Unable to open the file '%s'! \n",fileName);
		exit(1);
	}
	for (i=0;i<np;i++)
	{
		for (j=0;j<3;j++) fprintf(fw,"%f ",ps[i][j]);
		fprintf(fw,"\n");
	}
	fclose(fw);
}


void IcpNormalizeVector(DoubleT q[],int n)
/************************************************************************
 * Normalizes vector q[0..n-1] so that its size can be 1.00000.			*
 * q	- given vector													*
 * n 	- size of vector q												*
 ************************************************************************/
{
	DoubleT x=0;
	int i;

	for (i=0;i<n;i++) x+=q[i]*q[i];
	x=sqrt(x);
	for (i=0;i<n;i++) q[i]=q[i]/x;
}


void IcpGenInitStates(DoubleT qinit[][4])
/************************************************************************
 * Generates 40 initial rotation states.								*
 * qinit	- list of resulting rotations								*
 ************************************************************************/
{
	DoubleT q[]={1,1,1,1};
	int i,j;

	for (i=0;i<40;i++)
	{
	for (j=0;j<4;j++) qinit[i][j]=q[j];
	q[3]--;
	if (q[3]<-1)
	{
	    q[3]=1;
	    q[2]--;
	    if (q[2]<-1)
	    {
		q[2]=1;
		q[1]--;
		if (q[1]<-1)
		{
		    q[1]=1;
		    q[0]--;
		}
	    }
	}
	IcpNormalizeVector(qinit[i],4);
	}
}