point_matching.h
Go to the documentation of this file.00001 /**************************************************************************** 00002 * VCGLib o o * 00003 * Visual and Computer Graphics Library o o * 00004 * _ O _ * 00005 * Copyright(C) 2004 \/)\/ * 00006 * Visual Computing Lab /\/| * 00007 * ISTI - Italian National Research Council | * 00008 * \ * 00009 * All rights reserved. * 00010 * * 00011 * This program is free software; you can redistribute it and/or modify * 00012 * it under the terms of the GNU General Public License as published by * 00013 * the Free Software Foundation; either version 2 of the License, or * 00014 * (at your option) any later version. * 00015 * * 00016 * This program is distributed in the hope that it will be useful, * 00017 * but WITHOUT ANY WARRANTY; without even the implied warranty of * 00018 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 00019 * GNU General Public License (http://www.gnu.org/licenses/gpl.txt) * 00020 * for more details. * 00021 * * 00022 ****************************************************************************/ 00023 00024 /**************************************************************************** 00025 History 00026 00027 $Log: point_matching.h,v $ 00028 00029 ****************************************************************************/ 00030 #ifndef _VCG_MATH_POINTMATCHING_H 00031 #define _VCG_MATH_POINTMATCHING_H 00032 00033 #include <vcg/math/matrix33.h> 00034 #include <vcg/math/quaternion.h> 00035 #include <vcg/math/lin_algebra.h> 00036 namespace vcg 00037 { 00038 template<class ScalarType> 00039 class PointMatching 00040 { 00041 public: 00042 typedef Point3<ScalarType> Point3x; 00043 typedef Matrix33<ScalarType> Matrix33x; 00044 typedef Matrix44<ScalarType> Matrix44x; 00045 typedef Quaternion<ScalarType> Quaternionx; 00046 00047 00048 /* 00049 Compute a similarity matching (rigid + uniform scaling) 00050 simply create a temporary point set with the correct scaling factor 00051 00052 00053 */ 00054 static bool ComputeSimilarityMatchMatrix( Matrix44x &res, 00055 std::vector<Point3x> &Pfix, // vertici corrispondenti su fix (rossi) 00056 std::vector<Point3x> &Pmov) // normali scelti su mov (verdi) 00057 { 00058 Quaternionx qtmp; 00059 Point3x tr; 00060 00061 std::vector<Point3x> Pnew(Pmov.size()); 00062 00063 ScalarType scalingFactor=0; 00064 00065 for(size_t i=0;i<( Pmov.size()-1);++i) 00066 { 00067 scalingFactor += Distance(Pmov[i],Pmov[i+1])/ Distance(Pfix[i],Pfix[i+1]); 00068 #ifdef _DEBUG 00069 printf("Scaling Factor is %f",scalingFactor/(i+1)); 00070 #endif 00071 } 00072 scalingFactor/=(Pmov.size()-1); 00073 00074 for(size_t i=0;i<Pmov.size();++i) 00075 Pnew[i]=Pmov[i]/scalingFactor; 00076 00077 00078 bool ret=ComputeRigidMatchMatrix(res,Pfix,Pnew,qtmp,tr); 00079 if(!ret) return false; 00080 Matrix44x scaleM; scaleM.SetDiagonal(1.0/scalingFactor); 00081 00082 res = res * scaleM; 00083 return true; 00084 } 00085 00086 00087 00088 static bool ComputeRigidMatchMatrix( Matrix44x &res, 00089 std::vector<Point3x> &Pfix, // vertici corrispondenti su fix (rossi) 00090 std::vector<Point3x> &Pmov) // normali scelti su mov (verdi) 00091 { 00092 Quaternionx qtmp; 00093 Point3x tr; 00094 return ComputeRigidMatchMatrix(res,Pfix,Pmov,qtmp,tr); 00095 } 00096 00097 00098 /* 00099 Calcola la matrice di rototraslazione 00100 che porta i punti Pmov su Pfix 00101 00102 Basata sul paper 00103 00104 Besl, McKay 00105 A method for registration o f 3d Shapes 00106 IEEE TPAMI Vol 14, No 2 1992 00107 00108 00109 Esempio d'uso 00110 const int np=1000; 00111 std::vector<Point3x> pfix(np),pmov(np); 00112 00113 Matrix44x Rot,Trn,RotRes; 00114 Rot.Rotate(30,Point3x(1,0,1)); 00115 Trn.Translate(0,0,100); 00116 Rot=Trn*Rot; 00117 00118 for(int i=0;i<np;++i){ 00119 pfix[i]=Point3x(-150+rand()%1000,-150+rand()%1000,0); 00120 pmov[i]=Rot.Apply(pfix[i]); 00121 } 00122 00123 ComputeRigidMatchMatrix(RotRes,pfix,pmov); 00124 00125 RotRes.Invert(); 00126 assert( RotRes==Rot); 00127 assert( RotRes.Apply(pmov[i]) == pfix[i] ); 00128 00129 */ 00130 static 00131 bool ComputeWeightedRigidMatchMatrix(Matrix44x &res, 00132 std::vector<Point3x> &Pfix, 00133 std::vector<Point3x> &Pmov, 00134 std::vector<ScalarType> weights, 00135 Quaternionx &q, 00136 Point3x &tr 00137 ) 00138 { 00139 00140 Matrix33x ccm; 00141 Point3x bfix,bmov; // baricenter of src e trg 00142 ccm.WeightedCrossCovariance(weights,Pmov,Pfix,bmov,bfix); 00143 Matrix33x cyc; // the cyclic components of the cross covariance matrix. 00144 00145 cyc=ccm - ccm.transpose(); 00146 00147 Matrix44x QQ; 00148 QQ.SetZero(); 00149 Point3x D(cyc[1][2],cyc[2][0],cyc[0][1]); 00150 00151 Matrix33x RM; 00152 RM.SetZero(); 00153 RM[0][0]=-ccm.Trace(); 00154 RM[1][1]=-ccm.Trace(); 00155 RM[2][2]=-ccm.Trace(); 00156 RM += ccm + ccm.transpose(); 00157 00158 QQ[0][0] = ccm.Trace(); 00159 QQ[0][1] = D[0]; QQ[0][2] = D[1]; QQ[0][3] = D[2]; 00160 QQ[1][0] = D[0]; QQ[2][0] = D[1]; QQ[3][0] = D[2]; 00161 00162 int i,j; 00163 for(i=0;i<3;i++) 00164 for(j=0;j<3;j++) 00165 QQ[i+1][j+1]=RM[i][j]; 00166 00167 // printf(" Quaternion Matrix\n"); 00168 // print(QQ); 00169 Point4d d; 00170 Matrix44x v; 00171 int nrot; 00172 Jacobi(QQ,d,v,nrot); 00173 // printf("Done %i iterations\n %f %f %f %f\n",nrot,d[0],d[1],d[2],d[3]); 00174 // print(v); 00175 // Now search the maximum eigenvalue 00176 double maxv=0; 00177 int maxind=-1; 00178 for(i=0;i<4;i++) 00179 if(maxv<fabs(d[i])) { 00180 q=Quaternionx(v[0][i],v[1][i],v[2][i],v[3][i]); 00181 maxind=i; 00182 maxv=d[i]; 00183 } 00184 // The corresponding eigenvector define the searched rotation, 00185 Matrix44x Rot; 00186 q.ToMatrix(Rot); 00187 // the translation (last row) is simply the difference between the transformed src barycenter and the trg baricenter 00188 tr= (bfix - Rot *bmov); 00189 //res[3][0]=tr[0];res[3][1]=tr[1];res[3][2]=tr[2]; 00190 Matrix44x Trn; 00191 Trn.SetTranslate(tr); 00192 00193 res=Rot*Trn; 00194 return true; 00195 } 00196 00197 static 00198 bool ComputeRigidMatchMatrix(Matrix44x &res, 00199 std::vector<Point3x> &Pfix, 00200 std::vector<Point3x> &Pmov, 00201 Quaternionx &q, 00202 Point3x &tr) 00203 { 00204 00205 Matrix33x ccm; 00206 Point3x bfix,bmov; // baricenter of src e trg 00207 ccm.CrossCovariance(Pmov,Pfix,bmov,bfix); 00208 Matrix33x cyc; // the cyclic components of the cross covariance matrix. 00209 00210 cyc=ccm-ccm.transpose(); 00211 00212 Matrix44x QQ; 00213 QQ.SetZero(); 00214 Point3x D(cyc[1][2],cyc[2][0],cyc[0][1]); 00215 00216 Matrix33x RM; 00217 RM.SetZero(); 00218 RM[0][0]=-ccm.Trace(); 00219 RM[1][1]=-ccm.Trace(); 00220 RM[2][2]=-ccm.Trace(); 00221 RM += ccm + ccm.transpose(); 00222 00223 QQ[0][0] = ccm.Trace(); 00224 QQ[0][1] = D[0]; QQ[0][2] = D[1]; QQ[0][3] = D[2]; 00225 QQ[1][0] = D[0]; QQ[2][0] = D[1]; QQ[3][0] = D[2]; 00226 00227 int i,j; 00228 for(i=0;i<3;i++) 00229 for(j=0;j<3;j++) 00230 QQ[i+1][j+1]=RM[i][j]; 00231 00232 // printf(" Quaternion Matrix\n"); 00233 // print(QQ); 00234 Point4d d; 00235 Matrix44x v; 00236 int nrot; 00237 //QQ.Jacobi(d,v,nrot); 00238 Jacobi(QQ,d,v,nrot); 00239 // printf("Done %i iterations\n %f %f %f %f\n",nrot,d[0],d[1],d[2],d[3]); 00240 // print(v); 00241 // Now search the maximum eigenvalue 00242 double maxv=0; 00243 int maxind=-1; 00244 for(i=0;i<4;i++) 00245 if(maxv<fabs(d[i])) { 00246 q=Quaternionx(v[0][i],v[1][i],v[2][i],v[3][i]); 00247 maxind=i; 00248 maxv=d[i]; 00249 } 00250 // The corresponding eigenvector define the searched rotation, 00251 Matrix44x Rot; 00252 q.ToMatrix(Rot); 00253 // the translation (last row) is simply the difference between the transformed src barycenter and the trg baricenter 00254 tr= (bfix - Rot*bmov); 00255 //res[3][0]=tr[0];res[3][1]=tr[1];res[3][2]=tr[2]; 00256 Matrix44x Trn; 00257 Trn.SetTranslate(tr); 00258 00259 res=Trn*Rot; 00260 return true; 00261 } 00262 00263 // Dati due insiemi di punti e normali corrispondenti calcola la migliore trasformazione 00264 // che li fa corrispondere 00265 static bool ComputeMatchMatrix( Matrix44x &res, 00266 std::vector<Point3x> &Ps, // vertici corrispondenti su src (rossi) 00267 std::vector<Point3x> &Ns, // normali corrispondenti su src (rossi) 00268 std::vector<Point3x> &Pt) // vertici scelti su trg (verdi) 00269 // vector<Point3x> &Nt) // normali scelti su trg (verdi) 00270 { 00271 assert(0); 00272 // Da qui in poi non compila che ha bisogno dei minimiquadrati 00273 #if 0 00274 int sz=Ps.size(); 00275 00276 Matrix<double> A(sz,12); 00277 Vector<double> b(sz); 00278 Vector<double> x(12); 00279 00280 //inizializzo il vettore per minimi quadrati 00281 // la matrice di trasf che calcolo con LeastSquares cerca avvicinare il piu' 00282 // possibile le coppie di punti che trovo ho scelto 00283 // Le coppie di punti sono gia' trasformate secondo la matrice <In> quindi come scelta iniziale 00284 // per il metodo minimiquadrati scelgo l'identica (e.g. se ho allineato a mano perfettamente e 00285 // le due mesh sono perfettamente uguali DEVE restituire l'identica) 00286 00287 res.SetIdentity(); 00288 int i,j,k; 00289 for(i=0; i<=2; ++i) 00290 for(j=0; j<=3; ++j) 00291 x[i*4+j] = res[i][j]; 00292 00293 00294 //costruzione della matrice 00295 for(i=0;i<sz;++i) 00296 { 00297 for(j=0;j<3;++j) 00298 for(k=0;k<4;++k) 00299 if(k<3) 00300 { 00301 A[i][k+j*4] = Ns[i][j]*Pt[i][k]; 00302 } 00303 else 00304 { 00305 A[i][k+j*4] = Ns[i][j]; 00306 } 00307 b[i] = Ps[i]*Ns[i]; 00308 } 00309 const int maxiter = 4096; 00310 int iter; 00311 LSquareGC(x,A,b,1e-16,maxiter,iter); 00312 00313 TRACE("LSQ Solution"); 00314 for(int ind=0; ind<12; ++ind) { 00315 if((ind%4)==0) TRACE("\n"); 00316 TRACE("%8.5lf ", x[ind]); 00317 } TRACE("\n"); 00318 00319 if(iter==maxiter) 00320 { 00321 TRACE("I minimi quadrati non convergono!!\n"); 00322 return false; 00323 } 00324 else { TRACE("Convergenza in %d passi\n",iter); } 00325 00326 //Devo riapplicare la matrice di trasformazione globale a 00327 //trg inserendo il risultato nel vettore trgvert contenente 00328 //copia dei suoi vertici 00329 Matrix44x tmp; 00330 for(i=0; i<=2; ++i) 00331 for(j=0; j<=3; ++j) 00332 res[j][i] = x[i*4+j]; 00333 res[0][3] = 0.0; 00334 res[1][3] = 0.0; 00335 res[2][3] = 0.0; 00336 res[3][3] = 1.0; 00337 /* 00338 res.Transpose(); 00339 Point3x scv,shv,rtv,trv; 00340 res.Decompose(scv,shv,rtv,trv); 00341 vcg::print(res); 00342 printf("Scale %f %f %f\n",scv[0],scv[1],scv[2]); 00343 printf("Shear %f %f %f\n",shv[0],shv[1],shv[2]); 00344 printf("Rotat %f %f %f\n",rtv[0],rtv[1],rtv[2]); 00345 printf("Trans %f %f %f\n",trv[0],trv[1],trv[2]); 00346 00347 printf("----\n"); res.Decompose(scv,shv,rtv,trv); 00348 vcg::print(res); 00349 printf("Scale %f %f %f\n",scv[0],scv[1],scv[2]); 00350 printf("Shear %f %f %f\n",shv[0],shv[1],shv[2]); 00351 printf("Rotat %f %f %f\n",rtv[0],rtv[1],rtv[2]); 00352 printf("Trans %f %f %f\n",trv[0],trv[1],trv[2]); 00353 00354 res.Transpose(); 00355 */ 00356 #endif 00357 return true; 00358 } 00359 00360 /* 00361 ****** Questa parte per compilare ha bisogno di leastsquares e matrici generiche 00362 ****** Da controllare meglio 00363 00364 00365 static void CreatePairMatrix( Matrix<double> & A2, const Point3x & p, const Point3x & n, double d ) 00366 { 00367 double t1 = p[0]*p[0]; 00368 double t2 = n[0]*n[0]; 00369 double t4 = t1*n[0]; 00370 double t5 = t4*n[1]; 00371 double t6 = t4*n[2]; 00372 double t7 = p[0]*t2; 00373 double t8 = t7*p[1]; 00374 double t9 = p[0]*n[0]; 00375 double t10 = p[1]*n[1]; 00376 double t11 = t9*t10; 00377 double t12 = p[1]*n[2]; 00378 double t13 = t9*t12; 00379 double t14 = t7*p[2]; 00380 double t15 = p[2]*n[1]; 00381 double t16 = t9*t15; 00382 double t17 = p[2]*n[2]; 00383 double t18 = t9*t17; 00384 double t19 = t9*n[1]; 00385 double t20 = t9*n[2]; 00386 double t21 = t9*d; 00387 double t22 = n[1]*n[1]; 00388 double t25 = t1*n[1]*n[2]; 00389 double t26 = p[0]*t22; 00390 double t27 = t26*p[1]; 00391 double t28 = p[0]*n[1]; 00392 double t29 = t28*t12; 00393 double t30 = t26*p[2]; 00394 double t31 = t28*t17; 00395 double t32 = t28*n[2]; 00396 double t33 = t28*d; 00397 double t34 = n[2]*n[2]; 00398 00399 double t36 = p[0]*t34; 00400 double t41 = p[1]*p[1]; double t43 = t41*n[0]; 00401 double t46 = p[1]*t2; double t48 = p[1]*n[0]; 00402 double t49 = t48*t15; double t50 = t48*t17; 00403 double t51 = t48*n[1]; double t52 = t48*n[2]; 00404 double t57 = p[1]*t22; double t59 = t10*t17; 00405 double t60 = t10*n[2]; double t63 = p[1]*t34; 00406 double t66 = p[2]*p[2]; double t68 = t66*n[0]; 00407 double t72 = p[2]*n[0]; double t73 = t72*n[1]; 00408 double t74 = t72*n[2]; double t80 = t15*n[2]; 00409 00410 A2[0][0] = t1*t2; A2[0][1] = t5; A2[0][2] = t6; 00411 A2[0][3] = t8; A2[0][4] = t11; A2[0][5] = t13; 00412 A2[0][6] = t14; A2[0][7] = t16; A2[0][8] = t18; 00413 A2[0][9] = t7; A2[0][10] = t19; A2[0][11] = t20; 00414 A2[0][12] = -t21; 00415 00416 A2[1][1] = t1*t22; A2[1][2] = t25; A2[1][3] = t11; 00417 A2[1][4] = t27; A2[1][5] = t29; A2[1][6] = t16; 00418 A2[1][7] = t30; A2[1][8] = t31; A2[1][9] = t19; 00419 A2[1][10] = t26; A2[1][11] = t32; A2[1][12] = -t33; 00420 00421 A2[2][2] = t1*t34; A2[2][3] = t13; A2[2][4] = t29; 00422 A2[2][5] = t36*p[1]; A2[2][6] = t18; A2[2][7] = t31; 00423 A2[2][8] = t36*p[2]; A2[2][9] = t20; A2[2][10] = t32; 00424 A2[2][11] = t36; A2[2][12] = -p[0]*n[2]*d; 00425 00426 A2[3][3] = t41*t2; A2[3][4] = t43*n[1]; A2[3][5] = t43*n[2]; 00427 A2[3][6] = t46*p[2]; A2[3][7] = t49; A2[3][8] = t50; 00428 A2[3][9] = t46; A2[3][10] = t51; A2[3][11] = t52; 00429 A2[3][12] = -t48*d; 00430 00431 A2[4][4] = t41*t22; A2[4][5] = t41*n[1]*n[2]; A2[4][6] = t49; 00432 A2[4][7] = t57*p[2]; A2[4][8] = t59; A2[4][9] = t51; 00433 A2[4][10] = t57; A2[4][11] = t60; A2[4][12] = -t10*d; 00434 00435 A2[5][5] = t41*t34; A2[5][6] = t50; A2[5][7] = t59; 00436 A2[5][8] = t63*p[2]; A2[5][9] = t52; A2[5][10] = t60; 00437 A2[5][11] = t63; A2[5][12] = -t12*d; 00438 00439 A2[6][6] = t66*t2; A2[6][7] = t68*n[1]; A2[6][8] = t68*n[2]; 00440 A2[6][9] = p[2]*t2; A2[6][10] = t73; A2[6][11] = t74; 00441 A2[6][12] = -t72*d; 00442 00443 A2[7][7] = t66*t22; A2[7][8] = t66*n[1]*n[2]; A2[7][9] = t73; 00444 A2[7][10] = p[2]*t22; A2[7][11] = t80; A2[7][12] = -t15*d; 00445 00446 A2[8][8] = t66*t34; A2[8][9] = t74; A2[8][10] = t80; 00447 A2[8][11] = p[2]*t34; A2[8][12] = -t17*d; 00448 00449 A2[9][9] = t2; A2[9][10] = n[0]*n[1]; 00450 A2[9][11] = n[0]*n[2]; A2[9][12] = -n[0]*d; 00451 00452 A2[10][10] = t22; A2[10][11] = n[1]*n[2]; A2[10][12] = -n[1]*d; 00453 A2[11][11] = t34; A2[11][12] = -n[2]*d; 00454 A2[12][12] = d*d; 00455 } 00456 00457 // Dati due insiemi di punti e normali corrispondenti calcola la migliore trasformazione 00458 // che li fa corrispondere 00459 static bool ComputeMatchMatrix2( Matrix44x &res, 00460 std::vector<Point3x> &Ps, // vertici corrispondenti su src (rossi) 00461 std::vector<Point3x> &Ns, // normali corrispondenti su src (rossi) 00462 std::vector<Point3x> &Pt) // vertici scelti su trg (verdi) 00463 //vector<Point3x> &Nt) // normali scelti su trg (verdi) 00464 { 00465 const int N = 13; 00466 int i,j,k; 00467 00468 Matrixd AT(N,N); 00469 Matrixd TT(N,N); 00470 // Azzeramento matrice totale (solo tri-superiore) 00471 for(i=0;i<N;++i) 00472 for(j=i;j<N;++j) 00473 AT[i][j] = 0; 00474 // Calcolo matrici locali e somma 00475 for(k=0;k<Ps.size();++k) 00476 { 00477 CreatePairMatrix(TT,Pt[k],Ns[k],Ps[k]*Ns[k]); 00478 for(i=0;i<N;++i) 00479 for(j=i;j<N;++j) 00480 AT[i][j] += TT[i][j]; 00481 } 00482 00483 for(i=0;i<N;++i) 00484 for(j=0;j<i;++j) 00485 AT[i][j] = AT[j][i]; 00486 00487 std::vector<double> q; 00488 double error; 00489 affine_ls2(AT,q,error); 00490 //printf("error: %g \n",error); 00491 res[0][0] = q[0]; 00492 res[0][1] = q[1]; 00493 res[0][2] = q[2]; 00494 res[0][3] = 0; 00495 res[1][0] = q[3]; 00496 res[1][1] = q[4]; 00497 res[1][2] = q[5]; 00498 res[1][3] = 0; 00499 res[2][0] = q[6]; 00500 res[2][1] = q[7]; 00501 res[2][2] = q[8]; 00502 res[2][3] = 0; 00503 res[3][0] = q[9]; 00504 res[3][1] = q[10]; 00505 res[3][2] = q[11]; 00506 res[3][3] = q[12]; 00507 00508 return true; 00509 } 00510 */ 00511 }; 00512 } // end namespace 00513 00514 #endif