icp.c

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

	DoubleT dk=0,vec[3],correspdms=0,alldms,elimit,i1,i2,admsk,cdmsk;
	ShapeT *pkshape;
	PointSetT *pkpset;
	long (*DataClosestPoint)(),(*DataFasterClosestPoint)();
	long (*ModelClosestPoint)(),(*ModelFasterClosestPoint)();
	FILE *fr;
	TreeT *modeltree,*datatree ;

#ifdef TIMING
	clock_t st,en;
#endif

	n=(datashp->pset)->n;
	if ((pkshape=(ShapeT *) malloc(sizeof(ShapeT)))==NULL) IcpMemError();
	if ((pkpset=(PointSetT *) malloc(sizeof(PointSetT)))==NULL) IcpMemError();
	pkshape->repr=datashp->repr;
	pkshape->pset=pkpset;
	pkpset->ps=pk;
	pkpset->n=n;
	pkshape->tset=datashp->tset;
	
	switch (modelshp->repr)
	{
		case PSET : ModelClosestPoint=IcpPsetClosestPoint;break;
		case TSET : ModelClosestPoint=IcpTsetClosestPoint;ModelFasterClosestPoint=IcpPartTsetClosestPoint;break;
	}
	switch (datashp->repr)
	{
		case PSET : DataClosestPoint=IcpPsetClosestPoint;break;
		case TSET : DataClosestPoint=IcpTsetClosestPoint;DataFasterClosestPoint=IcpPartTsetClosestPoint;break;
	}
	
	if (IcpRangeSearchFlag) modeltree=IcpRangeSearchInit(modelshp) ; 
		
	res->dms=DBL_MAX;
	alldms=DBL_MAX;
	if (par->correspinfo)
	{
		correspdms=DBL_MAX;
		if ((fr=fopen("corresp","r"))==NULL)
		{
			printf("Unable to open the file '%s'! \n","corresp");
			exit(1);
		}
		fscanf(fr,"%ld",&numCorr);
		printf("Number of corresponding points: %ld\n",numCorr);
		if ((corresp=(long (*)[]) malloc(numCorr*2*sizeof(long)))==NULL) IcpMemError();
		for (i=0;i<numCorr;i++)
		{
			fscanf(fr,"%lf %lf",&i1,&i2);
			corresp[i][0]=(long) i1;
			corresp[i][1]=(long) i2;
		}
		fclose(fr);
	}
	elimit=(par->elimit)*(par->elimit);
	do
	{
#ifdef TIMING
		st=clock();
#endif
		nact=0;
		if (IcpRangeSearchFlag && par->eliminate) 
		  datatree=IcpRangeSearchInit(pkshape) ; 
		
		for (i=0;i<n;i++)
		{
			elim[i]=0;
			if (k==0 || !par->fastercp || modelshp->repr==PSET)	
			  {  if (IcpRangeSearchFlag) 
			        couples[i]=IcpPsetRangeClosestPoint(pk[i],modelshp,y[i],modeltree);
			 else   couples[i]=ModelClosestPoint(pk[i],modelshp,y[i]);
			  }
			else couples[i]=ModelFasterClosestPoint(pk[i],modelshp,y[i],surrm,couples[i],par->searchedpart);
	  		if (par->eliminate)
	  		  {
		  	    if (!par->fastercp || datashp->repr==PSET)	
		  	      { if (IcpRangeSearchFlag)
	                       	  IcpPsetRangeClosestPoint(y[i],pkshape,vec,datatree) ;  	      
	  	           else DataClosestPoint(y[i],pkshape,vec);
		  	      }
		  		else DataFasterClosestPoint(y[i],pkshape,vec,surrd,i,par->searchedpart);
		  		
			    elim[i]=(((vec[0]-pk[i][0])*(vec[0]-pk[i][0])+(vec[1]-pk[i][1])*(vec[1]-pk[i][1])+(vec[2]-pk[i][2])*(vec[2]-pk[i][2]))>elimit);
			  }
	  		if (!elim[i]) nact++;
		}
		if (IcpRangeSearchFlag && par->eliminate) 
		  IcpRangeSearchClose(datatree) ;

#ifdef TIMING
		en=clock();
		printf("TIME - Closest Points: %.2f sec.\n",(DoubleT) (en-st)/CLK_TCK);
#endif
		res->dms=0;
		admsk=alldms;
		alldms=0;
		for (i=0;i<n;i++)
		{
			for (j=0;j<3;j++) vec[j]=y[i][j]-pk[i][j];
			alldms+=vec[0]*vec[0]+vec[1]*vec[1]+vec[2]*vec[2];
			if (!elim[i]) res->dms+=vec[0]*vec[0]+vec[1]*vec[1]+vec[2]*vec[2];
		}
		alldms/=n;
		res->dms/=nact;
		if (par->correspinfo)
		{
			cdmsk=correspdms;
			correspdms=0;
			for (i=0;i<numCorr;i++)
			{
				for (j=0;j<3;j++) vec[j]=(modelshp->pset)->ps[corresp[i][1]][j]-pk[corresp[i][0]][j];
				correspdms+=vec[0]*vec[0]+vec[1]*vec[1]+vec[2]*vec[2];
			}
			correspdms/=numCorr;
		}
#ifdef PSETLIST
		printf("Closest points:\n");
		IcpPrintPointArray(n,y);
#endif
#ifdef TIMING
		st=clock();
#endif
		IcpCompRegistration(datashp->pset,y,pk,res,elim,nact);
#ifdef TIMING
		en=clock();
		printf("TIME - Registration: %.2f sec.\n",(DoubleT) (en-st)/CLK_TCK);
#endif
		dk=0;
		for (i=0;i<n;i++)
		{
			for (j=0;j<3;j++) vec[j]=y[i][j]-pk[i][j];
			if (!elim[i])
			{
				for (j=0;j<3;j++) vec[j]=y[i][j]-pk[i][j];
				dk+=vec[0]*vec[0]+vec[1]*vec[1]+vec[2]*vec[2];
			}
		}
		dk/=nact;
		fprintf(fwdms,"%f %ld %f %f %f %f\n",res->dms,nact,correspdms,alldms,sqrt(elimit),res->dms-dk);
		printf("cDMS: %f; aDMS: %f; elimit: %f; nact: %ld; dms-dk: %f\n",correspdms,alldms,sqrt(elimit),nact,res->dms-dk);
		for (i=0;i<7;i++) fprintf(fwreg,"%f ",res->q[i]);
	    fprintf(fwreg,"\n");
		k++;
		printf("%d DMS: %f\n",k,res->dms);
#ifdef PSETLIST
		printf("Registered points: \n");
		IcpPrintPointArray(n,pk);
#endif
		steps=par->iidepth==0 ? -1 : k;
	}
	while ((res->dms-dk > par->thr) && (steps < par->iidepth));
	
	if (IcpRangeSearchFlag) IcpRangeSearchClose(modeltree) ;
	
	res->n=k;
}


void IcpTsetNeighbours(ShapeT *shp,SurroundT surr)
/****************************************************************************
 * Creates list of neighbouring triangles for each triangle on given shape.	*
 * shp		- given shape													*
 * surr		- resulting list												*
 ****************************************************************************/
{
	long i,j,nt,np,index[50];
	int k,l,m,ntri,max=0,min=32767,yesk;
	TrianglesT tr;
	DoubleT sumsurr=0;

	nt=(shp->tset)->n;
	np=(shp->pset)->n;
	tr=(shp->tset)->ts;
	for(j=0;j<nt;j++)
	{
		if ((surr[j]=(long *) malloc((50+1)*sizeof(long)))==NULL) IcpMemError();
		surr[j][0]=0;
	}
	for(i=0;i<np;i++)
	{
		ntri=0;
		for(j=0;j<nt;j++)
		{
			for(k=0;k<3;k++)
			{
				if (tr[j][k]==i)
				{
					index[ntri]=j;
					ntri++;
					break;
				}
			}
		}
		for(m=0;m<ntri;m++)
		{
			for(k=0;k<ntri;k++)
			{
				if(m!=k)
				{
					yesk=0;
					for(l=1;l<surr[index[m]][0];l++)
					{
						if (surr[index[m]][l]==index[k])
						{
							yesk=1;
							break;
						}
					}
					if (!yesk)
					{
						surr[index[m]][0]++;
						surr[index[m]][surr[index[m]][0]]=index[k];
					}
				}
			}
		}
	}
	for(j=0;j<nt;j++)
	{
		if ((surr[j]=(long *) realloc(surr[j],(surr[j][0]+1)*sizeof(long)))==NULL) IcpMemError();
		sumsurr+=surr[j][0];
		if (surr[j][0]>max) max=surr[j][0];
		if (surr[j][0]<min) min=surr[j][0];
	}
	printf("Average memebers of a surround: %f.\n",sumsurr/nt);
	printf("Maximum memebers of a surround: %d.\n",max);
	printf("Minimum memebers of a surround: %d.\n",min);
}


void IcpCenterPointArrays(long nd,PointsT dpnt,long nm,PointsT mpnt)
/********************************************************************
 * Centers points dpnt to match centers of mass of dpnt and mpnt.	*
 * dpnt		- points to be centered									*
 * mpnt		- second array of points								*
 * nd		- number of dpnt points									*
 * nm		- number of mpnt points									*
 ********************************************************************/
{
	int j;
	long i;
	DoubleT mid[3]={0,0,0},mim[3]={0,0,0},trans[3];

	for (j=0;j<3;j++)
	{
		for (i=0;i<nd;i++) mid[j]+=dpnt[i][j];
		mid[j]/=nd;
	}
	for (j=0;j<3;j++)
	{
		for (i=0;i<nm;i++) mim[j]+=mpnt[i][j];
		mim[j]/=nm;
		trans[j]=mim[j]-mid[j];
	}
	for (j=0;j<3;j++) for (i=0;i<nd;i++) dpnt[i][j]+=trans[j];
}


void IcpRegister(ShapeT *datashp,
                 ShapeT *modelshp,
                 RegParametersT *par,
                 DoubleT *qInitial, 
                 RegResultsT *res,
                 int ident)
/********************************************************************
 * Registers two given shapes										*
 * datashp	- data shape											*
 * modelshp - model shape											*
 * par 		- parameters of registration							*
 * res 		- results of registration								*
 * ident	- number identifying current registration				*
 * 			  used in names of files created by IcpRegister			*
 ********************************************************************/
{
	DoubleT rotM[3][3],qmin[7],dmsmin=DBL_MAX;
	PointSetT *dpset;
	PointsT y,pini;
	long **surrm,**surrd,*couples,i,k;
	DoubleT *q,psetsize,mi[3],mi2[3];
	int j,*elim,depth;
	FILE *fwreg,*fwdms;
	char regFileName[256],dmsFileName[256];
#ifdef TIMING
	clock_t st,en;
#endif
	
	dpset=datashp->pset;
	psetsize=IcpPsetSize(dpset);
	printf("Pset size: %f\n",psetsize);
	if ((q=(DoubleT *) malloc(7*sizeof(DoubleT)))==NULL) IcpMemError();
	q[0]=1;for (j=1;j<7;j++)q[j]=0;
	if ((pini=(PointsT) malloc((dpset->n)*3*sizeof(DoubleT)))==NULL) IcpMemError();
	if ((couples=(long *) malloc((dpset->n)*sizeof(long)))==NULL) IcpMemError();
	if ((elim=(int *) malloc((dpset->n)*sizeof(int)))==NULL) IcpMemError();
	if ((y=(PointsT) malloc((dpset->n)*3*sizeof(DoubleT)))==NULL) IcpMemError();
	if (par->fastercp==1 && modelshp->repr==TSET)
	{
		if ((surrm=(long **) malloc(((modelshp->tset)->n)*sizeof(long *)))==NULL) IcpMemError();
#ifdef TIMING
		st=clock();
#endif
		IcpTsetNeighbours(modelshp,surrm);
#ifdef TIMING
		en=clock();
		printf("TIME - Neighbours finding: %.2f sec.\n",(DoubleT) (en-st)/CLK_TCK);
#endif
	}
	if (par->eliminate==1 && par->fastercp==1 && datashp->repr==TSET)
	{
		if ((surrd=(long **) malloc(((datashp->tset)->n)*sizeof(long *)))==NULL) IcpMemError();
#ifdef TIMING
		st=clock();
#endif
		IcpTsetNeighbours(datashp,surrd);
#ifdef TIMING
		en=clock();
		printf("TIME - Neighbours finding: %.2f sec.\n",(DoubleT) (en-st)/CLK_TCK);
#endif
	}
	if (par->global==0)
	{
		q = qInitial;
                IcpCompRotMatrix(q,rotM);
		IcpApplyRegistration(q+4,rotM,dpset,pini);
		if (par->center) IcpCenterPointArrays(dpset->n,pini,(modelshp->pset)->n,(modelshp->pset)->ps);
	}
	else
	{
		for (i=0;i<N_INIT;i++)
		{
	        for (j=0;j<3;j++)
			{
				mi[j]=0;
				for (k=0;k<dpset->n;k++) mi[j]+=dpset->ps[k][j];
				mi[j]/=dpset->n;
			}
			IcpCompRotMatrix(par->qinit[i],rotM);
	    	for (j=0;j<4;j++) q[j]=par->qinit[i][j];
			IcpApplyRegistration(q+4,rotM,dpset,pini);
			for (j=0;j<3;j++)
			{
				mi2[j]=0;
				for (k=0;k<dpset->n;k++) mi2[j]+=pini[k][j];
				mi2[j]/=dpset->n;
				q[4+j]=mi[j]-mi2[j];
			}
			for (j=0;j<3;j++) for (k=0;k<dpset->n;k++) pini[k][j]+=q[4+j];

			if (par->center) IcpCenterPointArrays(dpset->n,pini,(modelshp->pset)->n,(modelshp->pset)->ps);
#ifdef ICPOUT
	    	printf("********Inicial rotation #: %ld*********\n",i);
#endif
			sprintf(regFileName,"iter%d_%d.reg",ident,(int) i);
			sprintf(dmsFileName,"iter%d_%d.dms",ident,(int) i);
			if ((fwreg=fopen(regFileName,"w"))==NULL)
			{
				printf("Unable to open the file '%s'! \n",regFileName);
				exit(1);
			}
			if ((fwdms=fopen(dmsFileName,"w"))==NULL)
			{
				printf("Unable to open the file '%s'! \n",dmsFileName);
				exit(1);
			}
	    	IcpLocalReg(datashp,modelshp,pini,par,res,surrm,surrd,couples,y,elim,fwreg,fwdms);
	   		fclose(fwreg);
			fclose(fwdms);
			if (res->dms < dmsmin)
	    	{
				dmsmin=res->dms;
				for (j=0;j<7;j++) qmin[j]=res->q[j];
	    	}
#ifdef ICPOUT
	    	printf("DMS: %f\n",(res->dms));
#endif
/*            printf("Current state vector:\n");
	    	for (j=0;j<7;j++) printf("[%d]: %f\n",j,res->q[j]);
	    	printf("Current min. st. vec.\n");
	    	for (j=0;j<7;j++) printf("[%d]: %f\n",j,qmin[j]);*/
		}
#ifdef ICPOUT
		printf("Minimal DMS: %f\n",dmsmin);
#endif
		IcpCompRotMatrix(qmin,rotM);
		IcpApplyRegistration(qmin+4,rotM,dpset,pini);
#ifdef ICPOUT
		printf("*******Final Registration********\n");
#endif
	}
	depth=par->iidepth;
	par->iidepth=0;
	sprintf(regFileName,"final%d.reg",ident);
	sprintf(dmsFileName,"final%d.dms",ident);
	if ((fwreg=fopen(regFileName,"w"))==NULL)
	{
		printf("Unable to open the file '%s'! \n",regFileName);
		exit(1);
	}
	if ((fwdms=fopen(dmsFileName,"w"))==NULL)
	{
		printf("Unable to open the file '%s'! \n",dmsFileName);
		exit(1);
	}
	IcpLocalReg(datashp,modelshp,pini,par,res,surrm,surrd,couples,y,elim,fwreg,fwdms);
	fclose(fwreg);
	fclose(fwdms);
	par->iidepth=depth;
	free(pini);
	if (par->fastercp==1 && modelshp->repr==TSET) free(surrm);
	if (par->eliminate==1 && par->fastercp==1 && datashp->repr==TSET) free(surrd);
	free(couples);
	free(y);
	free(elim);
}