mreg.c

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

/* Multiple point sets registration algorithm */
/* based on article of A.J.Stoddard, A.Hilton in Proceedings of ICRP'96 */
/* and an ICP implementation by P. Kucera, FEL CVUT, Prague */

/* Implemented by Jan Kybic ([e-mail]) */

/* Program expects on standard input the number of views on the first line */
/* Then a file name, number of points and an initial transformation vector (7D) */
/* for each view */

/* This version includes adaptive step-size modification based on step doubling */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <ctype.h>
#include <string.h>

#include "mreg.h" 	/* This includes also clrange.h and icp.h */

#define MATLABOUT 1     /* generate Matlab output */
#define REGISTER  1     /* register if 1, if 0 assume i<->i */
#define SCROUT    0     /* write the transformations on the screen as */

static int flag_scrout ;

#define MAXERR 1e-3     /* an absolute error boundary used in Compute for step-size control */

#if MATLABOUT
FILE *matf ;
#endif

#define DEBUG 1

ViewT view[MAXNVIEWS] ;
int nviews=0 ;          /* actual number of views */

/* whether to use elimination and at what distance */
BoolT eliminate=1 ; 
DoubleT elimdist=10.0 ;

DoubleT dt=0.0001 ;   /* time increment --- initial value */
DoubleT sumsqd ; /* sum of the squares of the distances between corr. pts. */
int numsqd ;


/* precision limit for inner and outer loop of iteration */
#define INNERLIM (0.0001)
#define OUTERLIM (0.1)

/* number of iterations in Compute */
#define NITIN 5000
#define NITOUT 50

DoubleT g,G ;  /* linear and rotational drag */


int main()
{
#if DEBUG
  puts("mreg started.") ;
#endif

#if MATLABOUT
 if ((matf=fopen("mreg.out","w"))==NULL)
   perror("Cannot open mreg.out for writing.") ;
#endif MATLABOUT   

 InitViews() ;
 flag_scrout=SCROUT ; /* should Display print ? */
 Compute() ;
 flag_scrout=1 ;
 Display() ;
 CloseViews() ;

#if MATLABOUT
 fclose(matf) ;
#endif 

#if DEBUG
  puts("Finished.") ;
#endif

 return 0 ;
}

void InitViews() /* read and initialize views */
{
 int i ; DoubleT rmax=0.0 ;
 
 scanf("%d",&nviews) ;
 if (nviews<2 || nviews>MAXNVIEWS)
   { fprintf(stderr,"Wrong number of views.\n") ;
     exit(1) ;
   }
 #if DEBUG
 printf("Number of views: %d\n",nviews) ;
 #endif
 
 for(i=0;i<nviews;i++)
  { 
#if DEBUG
    printf("Reading view %d.\n",i) ;
#endif      
    ReadView(&view[i]) ;
    SetupView(&view[i]) ;
    if (rmax<view[i].rms) rmax=view[i].rms ;
  }
  
 /* recommended parameter settings from the article */ 
 g=1.0 ; G=0.5*g*rmax*rmax ; 
}

void ReadView(ViewT *v) /* read one view */
{
  int c ; char *p ;
  
  while ((c=getchar())==' ' || c=='\n' || c=='\r' || c=='\t') ;
  
  p=v->filename ; *p++=c ;
  
  while (!isspace(c=getchar()) && p<v->filename+FILENAMELEN-1) *p++=c ;
  *p='\0' ;
  
#if DEBUG
  printf("filename: %s\n",v->filename) ;
#endif

  scanf("%d %lf %lf %lf %lf %lf %lf %lf",&v->n,
        &v->itransf[0],&v->itransf[1],&v->itransf[2],&v->itransf[3],
        &v->itransf[4],&v->itransf[5],&v->itransf[6]) ;

#if DEBUG
  printf("num=%d itransf=(%.3f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f)\n",v->n,
        v->itransf[0],v->itransf[1],v->itransf[2],v->itransf[3],
        v->itransf[4],v->itransf[5],v->itransf[6]) ;
#endif
  
  IcpNormalizeVector(v->itransf,4) ;
  IcpReadPsetFile(v->filename,v->n,&v->shp) ;

#if DEBUG  
  puts("File read.") ;
#endif
    
}      
            
  
void SetupView(ViewT *v) /* precomputes cm, rms and sets R,T */
{ int i ;

  if ((v->corr=malloc(v->n * sizeof(int)))==NULL) IcpMemError() ;
  if ((v->tp=malloc(v->n * sizeof(VectorT)))==NULL) IcpMemError() ;

  v->cm[0]=v->cm[1]=v->cm[2]=0.0 ;  
  
  for(i=0;i<v->n;i++) 
   { v->cm[0]+=v->shp.pset->ps[i][0] ; 
     v->cm[1]+=v->shp.pset->ps[i][1] ;
     v->cm[2]+=v->shp.pset->ps[i][2] ;
   }
  v->cm[0]/=v->n ; v->cm[1]/=v->n ; v->cm[2]/=v->n ;

  
#define DIST(x) (((x)[0]-v->cm[0]) * ((x)[0]-v->cm[0]) +\
        ((x)[1]-v->cm[1]) * ((x)[1]-v->cm[1]) +\
        ((x)[2]-v->cm[2]) * ((x)[2]-v->cm[2]))
               
  
  v->rms=0.0 ;  
  
  for(i=0;i<v->n;i++) v->rms+=DIST(v->shp.pset->ps[i]) ;
   
  v->rms=sqrt(v->rms/v->n) ; 

#if DEBUG
  printf("Center of mass=(%.3f,%.3f,%.3f), rmsdist=%.3f\n",
  v->cm[0],v->cm[1],v->cm[2],v->rms) ;
#endif  
 
  IcpCompRotMatrix(v->itransf,v->R) ;
  v->T[0]=v->itransf[4] ;    v->T[1]=v->itransf[5] ;    v->T[2]=v->itransf[6] ;  
}   
         
void CloseViews()
{ int i ;

#if DEBUG
  puts("Deallocating memory") ;
#endif
  
  for(i=0;i<nviews;i++)
    { free(view[i].shp.pset->ps) ;
      free(view[i].shp.pset) ;
      free(view[i].tp) ;
      free(view[i].corr) ;
    }
}                 

void TransformAllSets()          
{ int i ;
  for(i=0;i<nviews;i++)    
    {
     #if DEBUG
     printf("Transforming set %d.\n",i) ;
     #endif    
     IcpApplyRegistration(view[i].T,view[i].R,view[i].shp.pset,view[i].tp) ;
    }   
}      

void RegisterAllSets()
{ int i,j ;

  sumsqd=0.0 ; numsqd=0 ;
  
  for(i=0;i<nviews;i++)
    for(j=i+1;j<nviews;j++)
      { 
       #if DEBUG
       printf("Registering %d versus %d.\n",i,j) ;
       #endif             
       Register(&view[i],&view[j]) ; Register(&view[j],&view[i]) ;
       if (eliminate)
         { 
          #if DEBUG
          printf("Eliminate %d versus %d.\n",i,j) ;
          #endif
          Eliminate(&view[i],&view[j]) ; Eliminate(&view[j],&view[i]) ; 
          #if DEBUG
          printf("We shall use %d points in %d and %d in %d\n",
               view[i].nu,i,view[j].nu,j) ;
          #endif 
         } 
       #if DEBUG
       puts("Computing averages and correlation matrices.") ;
       #endif 
       ComputeAQ(i,j) ; ComputeAQ(j,i) ;
      }
  printf("Average square distance: %f\n",sumsqd/numsqd) ;    
}         

#undef DEBUG
#define DEBUG 1

DoubleT ComputeAllForces()
/* returns the sum of the squares of magnitudes of all F and tor */
{ int i,j ;

  DoubleT sum ;

  for(i=0;i<nviews;i++) 
   { view[i].F[0]=view[i].F[1]=view[i].F[2]=0.0 ;
     view[i].tor[0]=view[i].tor[1]=view[i].tor[2]=0.0 ;
   }
     
  for(i=0;i<nviews;i++)
   for(j=0;j<nviews;j++)
    if (i!=j)
     {
      #if 0
      printf("Computing forces induced on view %d by view %d.\n",i,j) ;
      #endif   
      ComputeForces(i,j) ;
     }    

  sum=0.0 ;        
  for(i=0;i<nviews;i++)
   {
    #if 0
    printf("view %d - F=(%.3f,%.3f,%.3f) tor=(%.3f,%.3f,%.3f)\n",i,
      view[i].F[0],view[i].F[1],view[i].F[2],
      view[i].tor[0],view[i].tor[1],view[i].tor[2]) ;
    #endif
    for(j=0;j<3;j++)
     sum+=view[i].F[j]*view[i].F[j]+view[i].tor[j]*view[i].tor[j] ;
   }

#if 0
  printf("dt=%g sum=%.5f\n",dt,sum) ;
#endif  
  return sum ;     
}
#undef DEBUG
#define DEBUG 0


void UpdateAllTransforms(DoubleT dt)
{ int i,k ;
  VectorT a,b,c ; DoubleT q[4],angle,sina ;
  ViewT *v ; MatrixT rm,om ;
  
  for(i=0;i<nviews;i++)
   { 
     #if DEBUG
     printf("Updating transformations for view %d.\n",i) ;
     #endif
     
     v=&view[i] ;
  
     for(k=0;k<3;k++) q[k+1]=dt*v->tor[k] ;
     angle=sqrt(q[1]*q[1]+q[2]*q[2]+q[3]*q[3]) ;
     if (angle>1e-20)
       { q[0]=0.0 ;
         IcpNormalizeVector(q,4) ; }
     q[0]=cos(angle/2) ;
     sina=sin(angle/2) ;
     for(k=0;k<3;k++) q[k+1]*=sina ;     
     IcpCompRotMatrix(q,rm) ;
     memcpy(om,v->R,sizeof(MatrixT)) ;
     MatrixTimesMatrix(v->R,rm,om) ;
     for(k=0;k<3;k++) a[k]=dt * v->F[k] ;
     VectorSub(c,v->T,v->cm) ;
     MatrixTimesVector(b,rm,c) ;
     VectorAdd(c,b,v->cm) ;
     VectorAdd(v->T,c,a) ;
   } 
}   
#undef DEBUG
#define DEBUG 0

void Compute()  /* performs the computation */
{ int nitout,nitin ; /* outer/inner iteration counters */
  
  DoubleT sum,prevsum,osum,dif,rat ; 
 
 #if 1
 puts("Starting the computation") ;
 #endif  
 
 nitout=0 ; sum=0.0 ;
 do { 
    nitout++ ; osum=sum ;
    #if 1
    printf("Iteration no.:%d\n",nitout) ;
    #endif  
    TransformAllSets() ;
    RegisterAllSets() ;
    Display() ;
         
   nitin=0 ; sum=1e50 ;
   do
    { nitin++ ; prevsum=sum ;

      /* save the current position to slot 0*/    
      SaveAllTransforms(0) ;
      Display() ;

      try_again:
      /* make one full size step */
      sum=ComputeAllForces() ;
      UpdateAllTransforms(dt) ;
      SaveAllTransforms(1) ;

      /* now make two half size steps */
      RestoreAllTransforms(0) ;
      sum=ComputeAllForces() ;