icp: icpCpp.cpp Source File

00001 /*=================================================================
00002  *
00003  * ICP algorithm implemented in c++
00004  *
00005  * written by
00006  *
00007  * Per Bergstr�m 2007-10-09
00008  *
00009  * email: per.bergstrom@ltu.se
00010  *
00011  * Uses kd-tree written by Guy Shechter
00012  * http://www.mathworks.com/matlabcentral/fileexchange/loadFile.do?objectId=4586&objectType=file
00013  *
00014  *=================================================================*/
00015 
00016 #include <math.h>
00017 #include <stdio.h>
00018 #include <stdlib.h>
00019 
00020 
00021 #include "icp/kdtree_common.h"
00022 
00023 namespace icp {
00024 
00025 /* Input Arguments */
00026 
00027 //#define       model   prhs[0]
00028 //#define       data    prhs[1]
00029 //#define       weights prhs[2]
00030 //#define       randvec prhs[3]
00031 //#define       sizerand        prhs[4]
00032 //#define       treeptr prhs[5]
00033 //#define       iter    prhs[6]
00034 
00035 
00036 /* Output Arguments */
00037 
00038 #define TR      plhs[0]
00039 #define TT     plhs[1]
00040 
00041 #if !defined(MAX)
00042 #define MAX(A, B)       ((A) > (B) ? (A) : (B))
00043 #endif
00044 
00045 #if !defined(MIN)
00046 #define MIN(A, B)       ((A) < (B) ? (A) : (B))
00047 #endif
00048 
00049 double pwr2(double a){
00050     return a*a;
00051 }
00052 
00053 void icp(
00054 double *trpr,
00055 double *ttpr,
00056 double *modelz,
00057 unsigned int nmodelz,
00058 double *dataz,
00059 double *qltyz,
00060 unsigned int ndataz,
00061 unsigned int *randvecz,
00062 unsigned int nrandvecz,
00063 unsigned int nrandz,
00064 unsigned int iimax,
00065 Tree *pointer_to_tree
00066 )
00067 {
00068     
00069     unsigned int i,itemp,j,k,ii,cou=0;
00070     double R[9],T[3],distcc;
00071     double datam[3],modelm[3],MM[3],Spx[9],quasum;
00072     unsigned short int bol=1;
00073     double SIGMA[3];
00074     double SpxTSpx[6];
00075     double A,B,C;
00076     double sqrtA23B;
00077     double x0,f0;
00078     double SIp,difl;
00079     double invmat[6];
00080     double V[9];
00081     double U[9];
00082     Tree *tree;
00083     
00084     tree = pointer_to_tree;
00085     
00086     trpr[0]=1.0;
00087     trpr[4]=1.0;
00088     trpr[8]=1.0;
00089     
00090     if((nrandz+1)>ndataz){
00091         bol=0;
00092     }
00093     
00094     for(ii=0;ii<iimax;ii++){
00095         
00096         for(i=0;i<9;i++){
00097             Spx[i]=0.0;
00098         }
00099         for(i=0;i<3;i++){
00100             modelm[i]=0.0;
00101             datam[i]=0.0;
00102         }
00103         
00104         quasum=0.0;
00105         for(itemp=0;itemp<ndataz;itemp++){
00106             
00107             if(bol){
00108                 if(itemp<nrandz){
00109                     i=randvecz[cou];
00110                     
00111                     if(cou<(nrandvecz-1)){
00112                         cou++;
00113                     }
00114                     else{
00115                         cou=0;
00116                     }
00117                 }
00118                 else{
00119                     break;
00120                 }
00121             }
00122             else{
00123                 i=itemp;
00124             }
00125             
00126             if(qltyz[i]>0.0){
00127                 
00128                 T[0]=trpr[0]*dataz[3*i]+trpr[3]*dataz[3*i+1]+trpr[6]*dataz[3*i+2]+ttpr[0];
00129                 T[1]=trpr[1]*dataz[3*i]+trpr[4]*dataz[3*i+1]+trpr[7]*dataz[3*i+2]+ttpr[1];
00130                 T[2]=trpr[2]*dataz[3*i]+trpr[5]*dataz[3*i+1]+trpr[8]*dataz[3*i+2]+ttpr[2];
00131                 
00132                 distcc=0.0;
00133                 A=0.0;
00134                 
00135                 run_queries(tree->rootptr,T,1,3,&A,&distcc, RETURN_INDEX);
00136                 
00137                 k=(unsigned int)A;
00138                 
00139                 MM[0]=qltyz[i]*modelz[3*k];
00140                 MM[1]=qltyz[i]*modelz[3*k+1];
00141                 MM[2]=qltyz[i]*modelz[3*k+2];
00142                 
00143                 datam[0]+=qltyz[i]*T[0];
00144                 datam[1]+=qltyz[i]*T[1];
00145                 datam[2]+=qltyz[i]*T[2];
00146                 
00147                 modelm[0]+=MM[0];
00148                 modelm[1]+=MM[1];
00149                 modelm[2]+=MM[2];
00150                 
00151                 for(j=0;j<3;j++){
00152                     Spx[j]+= MM[j]*T[0];
00153                     Spx[j+3]+= MM[j]*T[1];
00154                     Spx[j+6]+= MM[j]*T[2];
00155                 }
00156                 quasum+=qltyz[i];
00157             }
00158             
00159         }
00160         
00161         if(quasum<1e-12){
00162             break;
00163         }
00164         
00165         for(j=0;j<3;j++){
00166             modelm[j]=modelm[j]/quasum;
00167             datam[j]=datam[j]/quasum;
00168         }
00169         
00170         for(j=0;j<3;j++){
00171             Spx[j]-=quasum*(modelm[j]*datam[0]);
00172             Spx[j+3]-=quasum*(modelm[j]*datam[1]);
00173             Spx[j+6]-=quasum*(modelm[j]*datam[2]);
00174         }
00175         
00176         k=1;
00177         if(ii<1){
00178             
00179             distcc=0.0;
00180             for(j=0;j<9;j++){
00181                 distcc+=Spx[j]*Spx[j];
00182             }
00183             
00184             distcc=distcc/pwr2(quasum);
00185             
00186             if(distcc<1e-3){
00187                 k=0;
00188 //                T[0]=modelm[0]-datam[0];
00189 //                T[1]=modelm[1]-datam[1];
00190 //                T[2]=modelm[2]-datam[2];
00191                 T[0] = modelz[0] - dataz[0];
00192                 T[1] = modelz[1] - dataz[1];
00193                 T[2] = modelz[2] - dataz[2];
00194                 R[0]=1.0;R[3]=0.0;R[6]=0.0;
00195                 R[1]=0.0;R[4]=1.0;R[7]=0.0;
00196                 R[2]=0.0;R[5]=0.0;R[8]=1.0;
00197             }
00198             
00199         }
00200         
00201         if(k) {
00202             
00203     /* get R*/
00204             
00205             SpxTSpx[0]=Spx[0]*Spx[0]+Spx[1]*Spx[1]+Spx[2]*Spx[2];
00206             SpxTSpx[1]=Spx[3]*Spx[3]+Spx[4]*Spx[4]+Spx[5]*Spx[5];
00207             SpxTSpx[2]=Spx[6]*Spx[6]+Spx[7]*Spx[7]+Spx[8]*Spx[8];
00208             SpxTSpx[3]=Spx[0]*Spx[3]+Spx[1]*Spx[4]+Spx[5]*Spx[2];
00209             SpxTSpx[4]=Spx[3]*Spx[6]+Spx[4]*Spx[7]+Spx[5]*Spx[8];
00210             SpxTSpx[5]=Spx[0]*Spx[6]+Spx[1]*Spx[7]+Spx[2]*Spx[8];
00211             
00212             
00213     /*   CharacteristicPolynomial  sigma^3-A*sigma^2-B*sigma+C   */
00214             
00215             A=SpxTSpx[2]+SpxTSpx[1]+SpxTSpx[0];
00216             B=SpxTSpx[5]*SpxTSpx[5]+SpxTSpx[4]*SpxTSpx[4]-SpxTSpx[2]*SpxTSpx[1]-SpxTSpx[2]*SpxTSpx[0]+SpxTSpx[3]*SpxTSpx[3]-SpxTSpx[1]*SpxTSpx[0];
00217             C=-2*SpxTSpx[5]*SpxTSpx[3]*SpxTSpx[4]+SpxTSpx[5]*SpxTSpx[5]*SpxTSpx[1]+SpxTSpx[4]*SpxTSpx[4]*SpxTSpx[0]+SpxTSpx[2]*SpxTSpx[3]*SpxTSpx[3]-SpxTSpx[2]*SpxTSpx[1]*SpxTSpx[0];
00218             
00219             sqrtA23B=sqrt(A*A+3*B);
00220             
00221             x0=(A-sqrtA23B)/3.0;
00222             f0=(x0*x0-A*x0-B)*x0;
00223             
00224             SIGMA[2]=MIN(MAX((x0*(C+2*f0-2*sqrt(f0*(f0+C)))/f0),0),0.5*x0);
00225             
00226             x0=(A+sqrtA23B)/3.0;
00227             f0=x0*x0*x0-A*x0*x0-B*x0+C;
00228             
00229             SIGMA[0]=MAX(MIN((x0*(B*A-C)+2*x0*f0+2*(x0-A)*sqrt(f0*(f0+B*A-C))-A*f0)/(f0+B*A-C),A),0.5*(A+x0));
00230             
00231             for(k=0;k<3;k++){
00232                 
00233                 if(k==0){
00234                     j=0;
00235                 }
00236                 else if(k==1){
00237                     j=2;
00238                 }
00239                 else if(k==2){
00240                     j=1;
00241                     SIGMA[1]=A-SIGMA[0]-SIGMA[2];
00242                 }
00243                 
00244         /* Newton-Raphson */
00245                 
00246                 for(i=0;i<50;i++){
00247                     SIp=SIGMA[j];
00248                     difl=(-3*SIGMA[j]+2*A)*SIGMA[j]+B;
00249                     if(fabs(difl)>1e-15){
00250                         SIGMA[j]=((-2*SIGMA[j]+A)*SIGMA[j]*SIGMA[j]+C)/difl;
00251                         if(fabs(SIGMA[j]-SIp)<1e-25){
00252                             break;
00253                         }
00254                     }
00255                     else {
00256                         break;
00257                     }
00258                 }
00259             }
00260             
00261             k=0;
00262             if(fabs(SIGMA[1]-SIGMA[0])<1e-12){
00263                 k=1;
00264             }
00265             
00266     /* eigenvalues found, corresponding eigenvectors V[i] ... */
00267             
00268             for(i=0;i<3;i++){
00269                 
00270                 invmat[0]=SpxTSpx[2]*SpxTSpx[1]-SpxTSpx[1]*SIGMA[i]-SIGMA[i]*SpxTSpx[2]+SIGMA[i]*SIGMA[i]-SpxTSpx[4]*SpxTSpx[4];
00271                 invmat[1]=SpxTSpx[4]*SpxTSpx[5]-SpxTSpx[3]*SpxTSpx[2]+SpxTSpx[3]*SIGMA[i];
00272                 invmat[2]=SpxTSpx[3]*SpxTSpx[4]-SpxTSpx[5]*SpxTSpx[1]+SpxTSpx[5]*SIGMA[i];
00273                 invmat[3]=SpxTSpx[2]*SpxTSpx[0]-SpxTSpx[0]*SIGMA[i]-SIGMA[i]*SpxTSpx[2]+SIGMA[i]*SIGMA[i]-SpxTSpx[5]*SpxTSpx[5];
00274                 invmat[4]=-SpxTSpx[4]*SpxTSpx[0]+SpxTSpx[4]*SIGMA[i]+SpxTSpx[3]*SpxTSpx[5];
00275                 invmat[5]=SpxTSpx[1]*SpxTSpx[0]-SpxTSpx[0]*SIGMA[i]-SpxTSpx[1]*SIGMA[i]+SIGMA[i]*SIGMA[i]-SpxTSpx[3]*SpxTSpx[3];
00276                 
00277                 if(i<2){
00278                     V[3*i]=invmat[0];
00279                     V[3*i+1]=invmat[1];
00280                     V[3*i+2]=invmat[2];
00281                     
00282                     if(k){
00283                         if(i==1){
00284                             V[3]=invmat[1];
00285                             V[4]=invmat[3];
00286                             V[5]=invmat[4];
00287                             
00288                             distcc=V[3]*V[0]+V[4]*V[1]+V[5]*V[2];
00289                             V[3]=V[3]-distcc*V[0];
00290                             V[4]=V[4]-distcc*V[1];
00291                             V[5]=V[5]-distcc*V[2];
00292                         }
00293                     }
00294                     
00295                 }
00296                 else {
00297                     
00298                     /* Eigen vectors corresponding to symmetric positiv definite matrices
00299                      are orthogonal. */
00300                     
00301                     V[6]=V[1]*V[5]-V[2]*V[4];
00302                     V[7]=V[2]*V[3]-V[0]*V[5];
00303                     V[8]=V[0]*V[4]-V[1]*V[3];
00304                 }
00305                 
00306                 for(j=0;j<10;j++){
00307                     
00308                     MM[0]=V[3*i];
00309                     MM[1]=V[3*i+1];
00310                     MM[2]=V[3*i+2];
00311                     
00312                     V[3*i]=invmat[0]*MM[0]+invmat[1]*MM[1]+invmat[2]*MM[2];
00313                     V[3*i+1]=invmat[1]*MM[0]+invmat[3]*MM[1]+invmat[4]*MM[2];
00314                     V[3*i+2]=invmat[2]*MM[0]+invmat[4]*MM[1]+invmat[5]*MM[2];
00315                     
00316                     if(k){
00317                         if(i==1){
00318                             distcc=V[3]*V[0]+V[4]*V[1]+V[5]*V[2];
00319                             V[3]=V[3]-distcc*V[0];
00320                             V[4]=V[4]-distcc*V[1];
00321                             V[5]=V[5]-distcc*V[2];
00322                         }
00323                     }
00324                     
00325                     distcc=sqrt(pwr2(V[3*i])+pwr2(V[3*i+1])+pwr2(V[3*i+2]));
00326                     
00327                     V[3*i]=V[3*i]/distcc;
00328                     V[3*i+1]=V[3*i+1]/distcc;
00329                     V[3*i+2]=V[3*i+2]/distcc;
00330                     
00331                     if(j>2){
00332                         if((pwr2(V[3*i]-MM[0])+pwr2(V[3*i+1]-MM[1])+pwr2(V[3*i+2]-MM[2]))<1e-29){
00333                             break;
00334                         }
00335                     }
00336                     
00337                 }
00338             }
00339             
00340             /* singular values & V[i] of Spx found, U[i] ... */
00341             
00342             SpxTSpx[0]=Spx[0]*Spx[0]+Spx[3]*Spx[3]+Spx[6]*Spx[6];
00343             SpxTSpx[1]=Spx[1]*Spx[1]+Spx[4]*Spx[4]+Spx[7]*Spx[7];
00344             SpxTSpx[2]=Spx[2]*Spx[2]+Spx[5]*Spx[5]+Spx[8]*Spx[8];
00345             SpxTSpx[3]=Spx[0]*Spx[1]+Spx[3]*Spx[4]+Spx[6]*Spx[7];
00346             SpxTSpx[4]=Spx[1]*Spx[2]+Spx[4]*Spx[5]+Spx[7]*Spx[8];
00347             SpxTSpx[5]=Spx[0]*Spx[2]+Spx[3]*Spx[5]+Spx[6]*Spx[8];
00348             
00349             for(i=0;i<3;i++){
00350                 
00351                 invmat[0]=SpxTSpx[2]*SpxTSpx[1]-SpxTSpx[1]*SIGMA[i]-SIGMA[i]*SpxTSpx[2]+SIGMA[i]*SIGMA[i]-SpxTSpx[4]*SpxTSpx[4];
00352                 invmat[1]=SpxTSpx[4]*SpxTSpx[5]-SpxTSpx[3]*SpxTSpx[2]+SpxTSpx[3]*SIGMA[i];
00353                 invmat[2]=SpxTSpx[3]*SpxTSpx[4]-SpxTSpx[5]*SpxTSpx[1]+SpxTSpx[5]*SIGMA[i];
00354                 invmat[3]=SpxTSpx[2]*SpxTSpx[0]-SpxTSpx[0]*SIGMA[i]-SIGMA[i]*SpxTSpx[2]+SIGMA[i]*SIGMA[i]-SpxTSpx[5]*SpxTSpx[5];
00355                 invmat[4]=-SpxTSpx[4]*SpxTSpx[0]+SpxTSpx[4]*SIGMA[i]+SpxTSpx[3]*SpxTSpx[5];
00356                 invmat[5]=SpxTSpx[1]*SpxTSpx[0]-SpxTSpx[0]*SIGMA[i]-SpxTSpx[1]*SIGMA[i]+SIGMA[i]*SIGMA[i]-SpxTSpx[3]*SpxTSpx[3];
00357                 
00358                 if(i<2){
00359                     U[3*i]=invmat[0];
00360                     U[3*i+1]=invmat[1];
00361                     U[3*i+2]=invmat[2];
00362                     
00363                     if(k){
00364                         if(i==1){
00365                             U[3]=invmat[1];
00366                             U[4]=invmat[3];
00367                             U[5]=invmat[4];
00368                             
00369                             distcc=U[3]*U[0]+U[4]*U[1]+U[5]*U[2];
00370                             U[3]=U[3]-distcc*U[0];
00371                             U[4]=U[4]-distcc*U[1];
00372                             U[5]=U[5]-distcc*U[2];
00373                         }
00374                     }
00375                     
00376                 }
00377                 else {
00378                     
00379                     /* Eigenvectors corresponding to symmetric positiv definite matrices
00380                      are orthogonal. */
00381                     
00382                     U[6]=U[1]*U[5]-U[2]*U[4];
00383                     U[7]=U[2]*U[3]-U[0]*U[5];
00384                     U[8]=U[0]*U[4]-U[1]*U[3];
00385                 }
00386                 
00387                 for(j=0;j<10;j++){
00388                     
00389                     MM[0]=U[3*i];
00390                     MM[1]=U[3*i+1];
00391                     MM[2]=U[3*i+2];
00392                     
00393                     U[3*i]=invmat[0]*MM[0]+invmat[1]*MM[1]+invmat[2]*MM[2];
00394                     U[3*i+1]=invmat[1]*MM[0]+invmat[3]*MM[1]+invmat[4]*MM[2];
00395                     U[3*i+2]=invmat[2]*MM[0]+invmat[4]*MM[1]+invmat[5]*MM[2];
00396                     
00397                     if(k){
00398                         if(i==1){
00399                             distcc=U[3]*U[0]+U[4]*U[1]+U[5]*U[2];
00400                             U[3]=U[3]-distcc*U[0];
00401                             U[4]=U[4]-distcc*U[1];
00402                             U[5]=U[5]-distcc*U[2];
00403                         }
00404                     }
00405                     
00406                     distcc=sqrt(pwr2(U[3*i])+pwr2(U[3*i+1])+pwr2(U[3*i+2]));
00407                     
00408                     U[3*i]=U[3*i]/distcc;
00409                     U[3*i+1]=U[3*i+1]/distcc;
00410                     U[3*i+2]=U[3*i+2]/distcc;
00411                     
00412                     if(j>2){
00413                         if((pwr2(U[3*i]-MM[0])+pwr2(U[3*i+1]-MM[1])+pwr2(U[3*i+2]-MM[2]))<1e-29){
00414                             break;
00415                         }
00416                     }
00417                     
00418                 }
00419                 
00420             }
00421             
00422             k=0;
00423             for(i=0;i<3;i++){
00424                 A=(Spx[0]*V[3*i]+Spx[3]*V[3*i+1]+Spx[6]*V[3*i+2])*U[3*i]+(Spx[1]*V[3*i]+Spx[4]*V[3*i+1]+Spx[7]*V[3*i+2])*U[3*i+1]+(Spx[2]*V[3*i]+Spx[5]*V[3*i+1]+Spx[8]*V[3*i+2])*U[3*i+2];
00425                 if(A<0){
00426                     k++;
00427                     U[3*i]=-U[3*i];
00428                     U[3*i+1]=-U[3*i+1];
00429                     U[3*i+2]=-U[3*i+2];
00430                 }
00431             }
00432                
00433       /* Get R=U*diag([1,1,det(U*V')])*V' */
00434             
00435             if(k==0 || k==2){           /* det(U*V')=+1 */
00436                 for(i=0;i<3;i++){
00437                     for(j=0;j<3;j++){
00438                         R[i+3*j]=U[i]*V[j]+U[i+3]*V[j+3]+U[i+6]*V[j+6];
00439                     }
00440                 }
00441             }
00442             else{                        /* det(U*V')=-1 */
00443                 for(i=0;i<3;i++){
00444                     for(j=0;j<3;j++){
00445                         R[i+3*j]=U[i]*V[j]+U[i+3]*V[j+3]-U[i+6]*V[j+6];
00446                     }
00447                 }
00448             }
00449             
00450     /* Get T=modelm-R*datam */
00451             
00452             T[0]=modelm[0]-R[0]*datam[0]-R[3]*datam[1]-R[6]*datam[2];
00453             T[1]=modelm[1]-R[1]*datam[0]-R[4]*datam[1]-R[7]*datam[2];
00454             T[2]=modelm[2]-R[2]*datam[0]-R[5]*datam[1]-R[8]*datam[2];
00455             
00456         }
00457         
00458     /* Get TR */
00459         
00460         for(j=0;j<3;j++){
00461             MM[0]=trpr[3*j];
00462             MM[1]=trpr[3*j+1];
00463             MM[2]=trpr[3*j+2];
00464             
00465             trpr[3*j]=R[0]*MM[0]+R[3]*MM[1]+R[6]*MM[2];
00466             trpr[1+3*j]=R[1]*MM[0]+R[4]*MM[1]+R[7]*MM[2];
00467             trpr[2+3*j]=R[2]*MM[0]+R[5]*MM[1]+R[8]*MM[2];
00468         }
00469         
00470     /* Get TT */
00471         
00472         MM[0]=ttpr[0];
00473         MM[1]=ttpr[1];
00474         MM[2]=ttpr[2];
00475         
00476         ttpr[0]=R[0]*MM[0]+R[3]*MM[1]+R[6]*MM[2]+T[0];
00477         ttpr[1]=R[1]*MM[0]+R[4]*MM[1]+R[7]*MM[2]+T[1];
00478         ttpr[2]=R[2]*MM[0]+R[5]*MM[1]+R[8]*MM[2]+T[2];
00479 
00480     }
00481     
00482 }
00483 //
00484 //void mexFunction( int nlhs, mxArray *plhs[],
00485 //int nrhs, const mxArray*prhs[] )
00486 //{
00487 //
00488 //    if(nrhs != 7){
00489 //        fprintf(stderr,("7 input arguments required.");
00490 //    } else if (nlhs > 2) {
00491 //        fprintf(stderr,("Too many output arguments.");
00492 //    }
00493 //    else if (mxGetM(model)!=3){
00494 //        fprintf(stderr,("Dimension of model points must be 3.");
00495 //    }
00496 //    else if (mxGetM(data)!=3){
00497 //        fprintf(stderr,("Dimension of data points must be 3.");
00498 //    }
00499 //    else if ((mxGetM(weights) != mxGetN(data)) && (mxGetN(weights) != mxGetN(data))){
00500 //        fprintf(stderr,("Number of weights must be equal to the number of data points.");
00501 //    }
00502 //    else if ((mxGetM(randvec) != mxGetN(data)) && (mxGetN(randvec) != mxGetN(data))){
00503 //        fprintf(stderr,("Length of randvec must be equal to the number of data points.");
00504 //    }
00505 //
00506 //    TR = mxCreateDoubleMatrix(3,3, mxREAL);
00507 //
00508 //    TT = mxCreateDoubleMatrix(3,1, mxREAL);
00509 //
00510 //    //icp(mxGetPr(TR),mxGetPr(TT),mxGetPr(model),(unsigned int)mxGetN(model),mxGetPr(data),mxGetPr(weights),(unsigned int)mxGetN(data),mxGetPr(randvec),(unsigned int)mxGetN(randvec),(unsigned int)mxGetScalar(sizerand),(unsigned int)mxGetScalar(iter),mxGetPr(treeptr));
00511 //    icp((double*)mxGetPr(TR),(double*)mxGetPr(TT),(double*)mxGetPr(model),(unsigned int)mxGetN(model),(double*)mxGetPr(data),(double*)mxGetPr(weights),(unsigned int)mxGetN(data),(unsigned int*)mxGetPr(randvec),(unsigned int)mxGetN(randvec),(unsigned int)mxGetScalar(sizerand),(unsigned int)mxGetScalar(iter),(double*)mxGetPr(treeptr));
00512 //
00513 //    return;
00514 //
00515 //}
00516 
00517 
00518 }