PLearn 0.1
|
00001 // -*- C++ -*- 00002 00003 // ICP.cc 00004 // 00005 // Copyright (C) 2004 Pascal Lamblin 00006 // 00007 // Redistribution and use in source and binary forms, with or without 00008 // modification, are permitted provided that the following conditions are met: 00009 // 00010 // 1. Redistributions of source code must retain the above copyright 00011 // notice, this list of conditions and the following disclaimer. 00012 // 00013 // 2. Redistributions in binary form must reproduce the above copyright 00014 // notice, this list of conditions and the following disclaimer in the 00015 // documentation and/or other materials provided with the distribution. 00016 // 00017 // 3. The name of the authors may not be used to endorse or promote 00018 // products derived from this software without specific prior written 00019 // permission. 00020 // 00021 // THIS SOFTWARE IS PROVIDED BY THE AUTHORS ``AS IS'' AND ANY EXPRESS OR 00022 // IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 00023 // OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN 00024 // NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 00025 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED 00026 // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 00027 // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 00028 // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 00029 // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 00030 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00031 // 00032 // This file is part of the PLearn library. For more information on the PLearn 00033 // library, go to the PLearn Web site at www.plearn.org 00034 00035 /* ******************************************************* 00036 * $Id: ICP.cc,v 1.23 2005/12/29 13:44:35 lamblinp Exp $ 00037 ******************************************************* */ 00038 00039 // Authors: Pascal Lamblin 00040 00044 #include "ICP.h" 00045 00046 namespace PLearn { 00047 using namespace std; 00048 00049 ICP::ICP() : 00050 weight_method( "dynamic" ), 00051 sigmoid_dmid( 500 ), 00052 sigmoid_k( 1 ), 00053 lorentz_sigma( 100 ), 00054 dynamic_d( 100 ), 00055 static_thresh( 1000 ), 00056 error_t( 0 ), 00057 dist_t( 10 ), 00058 angle_t( 0 ), 00059 trans_t( 0 ), 00060 max_iter( 20 ), 00061 normal_t( DEG2RAD * 90 ), 00062 area_weight( false ), 00063 overlap_filter( false ), 00064 overlap_delay( 0 ), 00065 smart_overlap( false ), 00066 smart_overlap_t( 0 ), 00067 n_per( 0 ), 00068 fine_matching( false ), 00069 read_trans_file( false ), 00070 write_trans_file( false ), 00071 write_vtx_match( false ), 00072 write_all_vtx_match( false ), 00073 verbosity( 1 ) 00074 {} 00075 00076 ICP::ICP( SurfMesh a_model, SurfMesh a_scene, 00077 GenericNN btl_init ) : 00078 weight_method( "dynamic" ), 00079 sigmoid_dmid( 500 ), 00080 sigmoid_k( 1 ), 00081 lorentz_sigma( 100 ), 00082 dynamic_d( 100 ), 00083 static_thresh( 1000 ), 00084 error_t( 0 ), 00085 dist_t( 10 ), 00086 angle_t( 0 ), 00087 trans_t( 0 ), 00088 max_iter( 20 ), 00089 normal_t_deg( 90 ), 00090 area_weight( false ), 00091 overlap_filter( false ), 00092 overlap_delay( 0 ), 00093 smart_overlap( false ), 00094 smart_overlap_t( 0 ), 00095 n_per( 0 ), 00096 fine_matching( false ), 00097 read_trans_file( false ), 00098 write_trans_file( false ), 00099 write_vtx_match( false ), 00100 write_all_vtx_match( false ), 00101 verbosity( 1 ) 00102 { 00103 model = a_model; 00104 scene = a_scene; 00105 00106 if( !btl_init ) 00107 { 00108 btl->setTrainingSet( scene->getVertexCoordsAndFeatures() ); 00109 btl->setOption( "nstages", "-1" ); 00110 // btl->rmin = 1; 00111 btl->build(); 00112 btl->train(); 00113 btl->num_neighbors = 1; 00114 btl->copy_input = true; 00115 btl->copy_target = false; 00116 btl->copy_weight = false; 00117 btl->copy_index = true; 00118 } 00119 else 00120 { 00121 btl = btl_init; 00122 } 00123 00124 } 00125 00126 00127 PLEARN_IMPLEMENT_OBJECT(ICP, 00128 "Performs the alignement of two surface meshes by the ICP method", 00129 "ICP stands for \"Iterative Closest Point\"" 00130 ); 00131 00132 void ICP::declareOptions(OptionList& ol) 00133 { 00134 declareOption(ol, "weight_method", &ICP::weight_method, 00135 OptionBase::buildoption, 00136 "Method used to weight the points:\n" 00137 " - \"sigmoid\": sigmoid function\n" 00138 " - \"lorentz\": Lorentzian function\n" 00139 " - \"dynamic\": dynamic threshold\n" 00140 " - \"static\": static threshold\n" ); 00141 00142 declareOption(ol, "sigmoid_dmid", &ICP::sigmoid_dmid, OptionBase::buildoption, 00143 "midpoint offset parameter in sigmoid function"); 00144 00145 declareOption(ol, "sigmoid_k", &ICP::sigmoid_k, OptionBase::buildoption, 00146 "slope parameter in sigmoid function"); 00147 00148 declareOption(ol, "lorentz_sigma", &ICP::lorentz_sigma, 00149 OptionBase::buildoption, 00150 "sigma parameter in Lorentzian function"); 00151 00152 declareOption(ol, "dynamic_d", &ICP::dynamic_d, OptionBase::buildoption, 00153 "D parameter in dynamic thresholding"); 00154 00155 declareOption(ol, "static_thresh", &ICP::static_thresh, 00156 OptionBase::buildoption, 00157 "threshold distance for static thresholding"); 00158 00159 declareOption(ol, "error_t", &ICP::error_t, OptionBase::buildoption, 00160 "stop when average point distance falls below this"); 00161 00162 declareOption(ol, "dist_t", &ICP::dist_t, OptionBase::buildoption, 00163 "stop when max points movement falls below this"); 00164 00165 declareOption(ol, "angle_t", &ICP::angle_t, OptionBase::buildoption, 00166 "stop when max rotation change falls below this (degrees)"); 00167 00168 declareOption(ol, "trans_t", &ICP::trans_t, OptionBase::buildoption, 00169 "stop when translation distance falls below this"); 00170 00171 declareOption(ol, "max_iter", &ICP::max_iter, OptionBase::buildoption, 00172 "stop when number of iterations reaches this"); 00173 00174 declareOption(ol, "normal_t_deg", &ICP::normal_t_deg, OptionBase::buildoption, 00175 "angle threshold (degrees) for compatible normal"); 00176 00177 declareOption(ol, "area_weight", &ICP::area_weight, OptionBase::buildoption, 00178 "enables weighting by area"); 00179 00180 declareOption(ol, "overlap_filter", &ICP::overlap_filter, 00181 OptionBase::buildoption, 00182 "filter non-overlapping points"); 00183 00184 declareOption(ol, "overlap_delay", &ICP::overlap_delay, 00185 OptionBase::buildoption, 00186 "number of iterations before overlap filtering is enabled"); 00187 00188 declareOption(ol, "smart_overlap_t", &ICP::smart_overlap_t, 00189 OptionBase::buildoption, 00190 "enable overlap filter when average point distance is below this"); 00191 00192 declareOption(ol, "n_per", &ICP::n_per, OptionBase::buildoption, 00193 "number of perturbations about minimum"); 00194 00195 declareOption(ol, "fine_matching", &ICP::fine_matching, 00196 OptionBase::buildoption, 00197 "if true, will match model vertices to any scene point,\n" 00198 "if false, will match vertices only to scene vertices.\n"); 00199 00200 declareOption(ol, "model_file", &ICP::model_file, OptionBase::buildoption, 00201 "file containing the model we're trying to align on the scene"); 00202 00203 declareOption(ol, "scene_file", &ICP::scene_file, OptionBase::buildoption, 00204 "file containing the scene we're trying to align the model on"); 00205 00206 declareOption(ol, "output_file", &ICP::output_file, OptionBase::buildoption, 00207 "transformed model"); 00208 00209 declareOption(ol, "initial_transform", &ICP::initial_transform, 00210 OptionBase::buildoption, 00211 "initial transformation to apply, \"tx ty tz rx ry rz\"\n" 00212 "this option is overriden by read_trans_file\n" ); 00213 00214 declareOption(ol, "read_trans_file", &ICP::read_trans_file, 00215 OptionBase::buildoption, 00216 "read initial transformation from \"trans_file\""); 00217 00218 declareOption(ol, "write_trans_file", &ICP::write_trans_file, 00219 OptionBase::buildoption, 00220 "write final transformation into file \"trans_file\""); 00221 00222 declareOption(ol, "trans_file", &ICP::trans_file, OptionBase::buildoption, 00223 "file containing a transformation. syntax of this file is:\n" 00224 "tx ty tz rx ry rz\\n"); 00225 00226 declareOption(ol, "write_vtx_match", &ICP::write_vtx_match, 00227 OptionBase::buildoption, 00228 "write final matching between model and scene vertices in \"vtx_match_file\""); 00229 00230 declareOption(ol, "write_all_vtx_match", &ICP::write_all_vtx_match, 00231 OptionBase::buildoption, 00232 "force to match _each_ model vertex to a scene vertex \n" 00233 "(wether it was used for the alignment or not).\n" ); 00234 00235 declareOption(ol, "vtx_match_file", &ICP::vtx_match_file, 00236 OptionBase::buildoption, 00237 "File containing the final match between model and vertices point indices"); 00238 00239 declareOption(ol, "verbosity", &ICP::verbosity, 00240 OptionBase::buildoption, 00241 "Level of verbosity. If 0 should not write anything on perr.\n" 00242 "If >0 may write some info on the steps performed along" 00243 " the way.\n" 00244 "The level of details written should depend on this value.\n" 00245 ); 00246 00247 /* 00248 declareOption(ol, "tx", &ICP::tx, OptionBase::buildoption, 00249 "translation wrt x"); 00250 00251 declareOption(ol, "ty", &ICP::ty, OptionBase::buildoption, 00252 "translation wrt y"); 00253 00254 declareOption(ol, "tz", &ICP::tz, OptionBase::buildoption, 00255 "translation wrt z"); 00256 00257 declareOption(ol, "rx", &ICP::rx, OptionBase::buildoption, 00258 "rotation wrt x"); 00259 00260 declareOption(ol, "ry", &ICP::ry, OptionBase::buildoption, 00261 "rotation wrt y"); 00262 00263 declareOption(ol, "rz", &ICP::rz, OptionBase::buildoption, 00264 "rotation wrt z"); 00265 */ 00266 declareOption(ol, "model", &ICP::model, OptionBase::buildoption, 00267 "the model we're trying to align on the scene"); 00268 00269 declareOption(ol, "scene", &ICP::scene, OptionBase::buildoption, 00270 "the scene we're trying to align the model on"); 00271 00272 declareOption(ol, "model_features", &ICP::model_features, 00273 OptionBase::buildoption, 00274 "Matrix containing the chemical features of each vertex of" 00275 " the model"); 00276 00277 declareOption(ol, "scene_features", &ICP::scene_features, 00278 OptionBase::buildoption, 00279 "Matrix containing the chemical features of each vertex of" 00280 " the scene"); 00281 00282 declareOption(ol, "btl", &ICP::btl, OptionBase::buildoption, 00283 "BTreeLearner, containing efficiently the scene points"); 00284 00285 declareOption(ol, "match", &ICP::match, OptionBase::learntoption, 00286 "matching of the model on the scene"); 00287 00288 // Now call the parent class' declareOptions 00289 inherited::declareOptions(ol); 00290 } 00291 00292 void ICP::build_() 00293 { 00294 normal_t = DEG2RAD * normal_t_deg; 00295 00296 if( fast_is_equal(smart_overlap_t, 0., REAL_MAX, 1e-5) ) 00297 smart_overlap = false; 00298 else 00299 smart_overlap = true; 00300 00301 string wm = lowerstring( weight_method ); 00302 if( wm == "sigmoid" ) 00303 weight_method = wm; 00304 else if( wm == "lorentz" ) 00305 weight_method = wm; 00306 else if( wm == "dynamic" ) 00307 weight_method = wm; 00308 else if( wm == "static" ) 00309 weight_method = wm; 00310 else if( wm == "oracle" ) 00311 weight_method = wm; 00312 else 00313 PLERROR( "weight_method \"%s\" not supported", weight_method.c_str() ); 00314 00315 buildMeshes(); 00316 } 00317 00318 void ICP::build() 00319 { 00320 inherited::build(); 00321 build_(); 00322 } 00323 00324 real ICP::dynamicDistanceThreshold( const Mat model_pts, const Mat scene_pts, 00325 const real d, const real d_max ) 00326 { 00327 /* calculate distance statistics */ 00328 int n = model_pts.length(); 00329 real sumd = 0; 00330 real sumd2 = 0; 00331 00332 for( int i = 0 ; i < n ; i++ ) 00333 { 00334 real d = powdistance( model_pts( i ), scene_pts( i ), 2 ); 00335 sumd2 += d; 00336 sumd += sqrt( d ); 00337 } 00338 00339 real davg = sumd/n; 00340 real dsig = sqrt( sumd2/n - davg*davg ); 00341 00342 /* set dynamic threshold */ 00343 if( davg < d ) 00344 { 00345 return davg + 3*dsig; 00346 } 00347 else if( davg < 3*d ) 00348 { 00349 return davg + 2*dsig; 00350 } 00351 else if( davg < 6*d ) 00352 { 00353 return davg + dsig; 00354 } 00355 else 00356 { 00357 return d_max; 00358 } 00359 } 00360 00361 real ICP::computeWeightedDistance( const Mat m1_pts, const Mat m2_pts, 00362 const Vec weights ) 00363 /* hey, on pourrait avoir une VMat avec des poids et tout... */ 00364 { 00365 int n = m1_pts.length(); 00366 real weight_total = 0; 00367 real error_total = 0; 00368 00369 for( int i=0 ; i<n ; i++ ) 00370 { 00371 real distance = dist( m1_pts( i ), m2_pts( i ), 2 ); 00372 weight_total += weights[ i ]; 00373 error_total += weights[ i ] * distance; 00374 } 00375 00376 if( weight_total > 0 ) 00377 { 00378 return( error_total / weight_total ); 00379 } 00380 else 00381 { 00382 return 0; 00383 } 00384 } 00385 00386 void ICP::computeWeights( const Mat model_pts, const Mat scene_pts, 00387 const Vec areas, Vec weights, 00388 real& min_wt, real& max_wt ) 00389 { 00390 int n = model_pts.length(); 00391 real total_weight = 0; 00392 max_wt = 0; 00393 min_wt = INFINITY; 00394 00395 if( area_weight ) 00396 PLERROR( "area weighting not implemented yet, please come back later" ); 00397 00398 00399 if( ( weight_method == "static" ) || ( weight_method == "dynamic" ) ) 00400 { 00401 /*if( area_weight ) 00402 PLERROR( "area weighting not implemented yet, please come back later" ); 00403 */ 00404 weights = 1./n; 00405 max_wt = weights[0]; 00406 min_wt = weights[0]; 00407 } 00408 else if( weight_method == "sigmoid" ) 00409 { 00410 for( int i=0 ; i<n ; i++ ) 00411 { 00412 real distance = dist( model_pts( i ), scene_pts( i ), 2 ); 00413 real exp_part = exp( sigmoid_k * ( sigmoid_dmid - distance ) ); 00414 weights[ i ] = exp_part / ( 1.0 + exp_part ); 00415 /*if( area_weight ) 00416 { 00417 PLERROR( "area weighting not implemented yet, please come back later" ); 00418 }*/ 00419 total_weight += weights[ i ]; 00420 } 00421 weights /= total_weight; 00422 min_wt = min( weights ); 00423 max_wt = max( weights ); 00424 } 00425 else if( weight_method == "lorentz" ) 00426 { 00427 for( int i=0 ; i<n ; i++ ) 00428 { 00429 real dist2 = powdistance( model_pts( i ), scene_pts( i ), 2 ); 00430 dist2 /= ( lorentz_sigma * lorentz_sigma ); 00431 weights[ i ] = 1.0 / ( 1.0 + 0.5 * dist2 ); 00432 /*if( area_weight ) 00433 { 00434 PLERROR( "area weighting not implemented yet, please come back later" ); 00435 }*/ 00436 total_weight += weights[ i ]; 00437 } 00438 weights /= total_weight; 00439 min_wt = min( weights ); 00440 max_wt = max( weights ); 00441 } 00442 00443 else if( weight_method == "oracle" ) 00444 { 00445 for( int i=0 ; i<n ; i++ ) 00446 { 00447 real w = weightOracle( (int) vtx_matching( i, 0 ), model_pts(i), 00448 (int) vtx_matching( i, 1 ), scene_pts(i) ); 00449 weights[i] = w; 00450 total_weight += w; 00451 } 00452 weights /= total_weight; 00453 min_wt = min( weights ); 00454 max_wt = max( weights ); 00455 } 00456 00457 } 00458 00459 00460 bool ICP::iterativeReweight( Mat model_pts, const Mat scene_pts, 00461 const Vec areas, Vec weights, 00462 const Mat init_corners, const int max_iter, 00463 Mat total_delta_rot, Vec total_delta_trans ) 00464 { 00465 Mat delta_rot( 3, 3 ); 00466 Vec delta_trans( 3 ); 00467 Vec angles( 3 ); 00468 00469 int n = model_pts.length(); 00470 if( n < 3 ) 00471 { 00472 PLERROR("not enough points to calculate transformation"); 00473 } 00474 00475 // initialize 00476 int n_iter = 0; 00477 00478 real max_corner_motion = INFINITY; 00479 Mat cur_corners( 8, 3 ); 00480 cur_corners << init_corners; 00481 Mat last_corners( 8, 3 ); 00482 last_corners << init_corners; 00483 00484 // loop until bounding box motion is small 00485 while( ( max_corner_motion > dist_t ) && ( n_iter < max_iter ) ) 00486 { 00487 n_iter++; 00488 00489 real min_wt, max_wt; 00490 computeWeights( model_pts, scene_pts, areas, weights, min_wt, max_wt ); 00491 00492 // calculate best transformation 00493 real error = INFINITY; 00494 weightedTransformationFromMatchedPoints( model_pts, scene_pts, weights, 00495 delta_rot, delta_trans, error ); 00496 // compute max motion of bounding box and update corner points 00497 last_corners << cur_corners; 00498 transformPoints( delta_rot, delta_trans, cur_corners, cur_corners ); 00499 max_corner_motion = maxPointMotion( last_corners, cur_corners ); 00500 00501 // accumulate transformation ensuring normalization 00502 if( n_iter == 1 ) // first time 00503 { 00504 total_delta_rot << delta_rot; 00505 total_delta_trans << delta_trans; 00506 } 00507 else 00508 { 00509 Mat tmp_mat( 3, 3 ); 00510 product( tmp_mat, delta_rot, total_delta_rot ); 00511 angles << fixedAnglesFromRotation( tmp_mat ); 00512 00513 total_delta_rot << 00514 rotationFromFixedAngles( angles[0], angles[1], angles[2] ); 00515 00516 Vec tmp_vec( 3 ); 00517 product( tmp_vec, delta_rot, total_delta_trans ); 00518 total_delta_trans += tmp_vec; 00519 } 00520 00521 // update model points using new transformation 00522 00523 transformPoints( delta_rot, delta_trans, model_pts, model_pts ); 00524 00525 if( verbosity >= 3 ) 00526 { 00527 pout << " " << n_iter << " * delta trans " << delta_trans 00528 << " * delta rot " << fixedAnglesFromRotation( delta_rot ) << endl 00529 << " max weight " << max_wt << " * min weight " << min_wt 00530 << " * max motion " << max_corner_motion << endl; 00531 } 00532 } 00533 00534 return true; 00535 } 00536 00537 int ICP::iterate() 00538 { 00539 real error = REAL_MAX; 00540 real max_corner_motion = REAL_MAX; 00541 real delta_trans_length = REAL_MAX; // ? 00542 real delta_angle_length = REAL_MAX; // ? 00543 00544 int n_iter = 0; 00545 00546 if( verbosity >=2 ) 00547 pout << "Initial transform: " << *match << endl; // 00548 00549 Mat total_rot = match->rot; 00550 Vec total_trans = match->trans; 00551 00552 if( area_weight ) 00553 PLERROR( "area_weight not implemented yet. please come back later." ); 00554 00555 // cache scene faces by point 00556 TVec< set<int> > face_cache = scene->cacheNeighborFaces(); 00557 00558 // stores scene vertex coordinates 00559 Mat all_scene_coords = scene->getVertexCoords(); 00560 00561 // compute vertices of bounding box and apply initial transform 00562 Mat bbox = model->boundingBox(); 00563 Mat orig_corners = boundingBoxToVertices( bbox ); 00564 Mat cur_corners( 8, 3); 00565 cur_corners << orig_corners; 00566 00567 transformPoints( total_rot, total_trans, cur_corners, cur_corners ); 00568 00569 real avg_pt_dist; 00570 int avg_pt_dist_cnt; 00571 00572 // apply initial transform to 00573 // loop until convergence or other stopping criterion reached 00574 while( (error > error_t) && 00575 (max_corner_motion > dist_t ) && 00576 (delta_trans_length > trans_t) && 00577 (delta_angle_length > angle_t) && 00578 (n_iter < max_iter) ) 00579 { 00580 n_iter++; 00581 00582 // find scene points closest to model points 00583 // Vec model_vtx; vtx_matching[0,] 00584 Mat model_coords_feats; 00585 // Vec scene_vtx; vtx_matching[1,] 00586 Mat scene_coords_feats; 00587 00588 real closest_dist; 00589 real dynamic_thresh = dynamic_dmax0 = 20 * dynamic_d; 00590 00591 // record average distance between closest points for smart overlap enabler 00592 if( smart_overlap && !overlap_filter ) 00593 { 00594 avg_pt_dist = 0; 00595 avg_pt_dist_cnt = 0; 00596 } 00597 00598 int n_verts = model->numVertices(); 00599 vtx_matching.resize( 0, 2 ); 00600 00601 vertex_iterator vi, vi_end; 00602 tie(vi, vi_end) = vertices( *model->p_mesh ); 00603 int feat_size = model->getVertex( *vi )->features.size(); 00604 00605 for( int i=0 ; vi != vi_end ; i++, vi++ ) 00606 { 00607 MVertex mv = model->getVertex( *vi ); 00608 Vec model_coord(3); 00609 product( model_coord, total_rot, mv->coord ); 00610 model_coord += total_trans; 00611 00612 Vec model_coord_feat(3+feat_size); 00613 model_coord_feat.subVec(0,3) << model_coord; 00614 model_coord_feat.subVec(3,feat_size) << mv->features; 00615 00616 Vec model_norm(3); 00617 product( model_norm, total_rot, mv->norm ); 00618 00619 Vec scene_coord_feat(3+feat_size); 00620 00621 real init_dist_t = REAL_MAX; 00622 // vtx_matching[i] = false; 00623 00624 int j; 00625 00626 if( fine_matching ) 00627 { 00628 Vec scene_coord(3); 00629 bool is_overlapping = isOverlapping( model_coord, model_norm, 00630 scene, face_cache, btl, 00631 init_dist_t, normal_t, 00632 j, scene_coord, 00633 closest_dist ); 00634 scene_coord_feat.subVec(0,3) << scene_coord; 00635 00636 if( overlap_filter && !is_overlapping && (n_iter > overlap_delay) ) 00637 continue; 00638 00639 if( smart_overlap && !overlap_filter ) 00640 { 00641 avg_pt_dist += closest_dist; 00642 avg_pt_dist_cnt++; 00643 } 00644 } 00645 else 00646 { 00647 getNearestVertex( model_coord_feat, scene, btl, 00648 j, scene_coord_feat, closest_dist ); 00649 } 00650 00651 // skip points beyond threshold 00652 if( (weight_method == "static") && (closest_dist > static_thresh) ) 00653 continue; 00654 if( (weight_method == "dynamic") && (closest_dist > dynamic_thresh) ) 00655 continue; 00656 00657 // get vertex coordinates if we haven't them 00658 if( is_missing( scene_coord_feat[0] ) || 00659 is_missing( scene_coord_feat[1] ) || 00660 is_missing( scene_coord_feat[2] ) ) 00661 { 00662 scene_coord_feat.subVec(0,3) << all_scene_coords(j); 00663 closest_dist = dist( scene_coord_feat.subVec(0,3), model_coord, 2 ); 00664 } 00665 00666 00667 // add to list of pts to use in icp 00668 Vec vtx_matching_pair( 2 ); 00669 vtx_matching_pair[0] = i; 00670 vtx_matching_pair[1] = j; 00671 vtx_matching.appendRow( vtx_matching_pair ); 00672 model_coords_feats.appendRow( model_coord_feat ); 00673 scene_coords_feats.appendRow( scene_coord_feat ); 00674 // vtx_matching[i] = true; 00675 00676 } 00677 00678 //vtx_matching << model_vtx; // we keep track of the matching vertices 00679 00680 int n_good = vtx_matching.length(); 00681 if( n_good < 3 ) 00682 { 00683 cerr << "Not enough points to compute transform" << endl; 00684 return( n_iter ); 00685 } 00686 00687 Mat delta_rot( 3, 3 ); 00688 Vec delta_trans( 3 ); 00689 00690 int n_inner_iter; 00691 if( (weight_method == "static") || (weight_method == "dynamic") 00692 || (weight_method == "oracle") ) 00693 n_inner_iter = 1; 00694 else 00695 n_inner_iter = 10; 00696 00697 Vec weights( n_good ); 00698 // iteratively reweight until convergence - updates model pts 00699 Mat model_coords = model_coords_feats.subMatColumns(0,3); 00700 Mat scene_coords = scene_coords_feats.subMatColumns(0,3); 00701 if( !iterativeReweight( model_coords, scene_coords, 00702 Vec(), weights, 00703 cur_corners, n_inner_iter, 00704 delta_rot, delta_trans ) ) 00705 { 00706 break; 00707 } 00708 // accumulate transformation ensuring normalization 00709 00710 Mat tmp_mat( 3, 3 ); 00711 product( tmp_mat, delta_rot, total_rot ); 00712 Vec angles = fixedAnglesFromRotation( tmp_mat ); 00713 00714 total_rot = rotationFromFixedAngles( angles[0], angles[1], angles[2] ); 00715 00716 Vec tmp_vec( 3 ); 00717 product( tmp_vec, delta_rot, total_trans ); 00718 total_trans = tmp_vec + delta_trans; 00719 00720 // compute error 00721 error = computeWeightedDistance( model_coords_feats, scene_coords_feats, 00722 weights ); 00723 00724 if( weight_method == "dynamic" ) // compute dynamic threshold 00725 { 00726 dynamic_thresh = dynamicDistanceThreshold( model_coords, scene_coords, 00727 dynamic_d, dynamic_dmax0 ); 00728 } 00729 00730 // update corners and compute corner motion from start of this iteration 00731 Mat last_corners( 8, 3 ); 00732 last_corners << cur_corners; 00733 transformPoints( total_rot, total_trans, orig_corners, cur_corners ); 00734 max_corner_motion = maxPointMotion( last_corners, cur_corners ); 00735 00736 delta_trans_length = norm( delta_trans, 2 ); 00737 delta_angle_length = norm( fixedAnglesFromRotation( delta_rot ), 2 ); 00738 00739 if( verbosity >= 3 ) 00740 { 00741 pout << n_iter << " trans " << total_trans << " * rotate " 00742 << fixedAnglesFromRotation( total_rot ) << endl 00743 << " Wgt avg Err " << error << " * max motion " 00744 << max_corner_motion << " * n_pts " << n_good << " / " << n_verts 00745 << " * dist thresh " << dynamic_thresh 00746 << endl; 00747 } 00748 00749 if( smart_overlap && !overlap_filter ) 00750 { 00751 if( (error < smart_overlap_t) || (n_iter == overlap_delay) ) 00752 { 00753 if( verbosity >= 2 ) 00754 { 00755 pout << 00756 "\n*********\n---- activating overlap filter now -----\n*********" 00757 << endl << endl; 00758 } 00759 overlap_filter = true; 00760 overlap_delay = n_iter; 00761 } 00762 } 00763 00764 } 00765 00766 /* store transformation in MMatch math */ 00767 match->rot << total_rot; 00768 match->trans << total_trans; 00769 match->error = error; 00770 match->angles << fixedAnglesFromRotation( total_rot ); 00771 00772 return n_iter; 00773 } 00774 00775 void ICP::buildMeshes() 00776 { 00777 if( !model ) 00778 { 00779 if( model_file.length() == 0 ) 00780 PLERROR( "You need to provide either 'model' or 'model_file' option"); 00781 00782 model = new SurfaceMesh(); 00783 00784 // read in the model mesh 00785 if( model_features.isNull() ) 00786 { 00787 if( !(model->readVRMLFile( model_file )) ) 00788 PLERROR( "Problem reading %s\n", model_file.c_str() ); 00789 } 00790 else 00791 { 00792 if( !(model->readVRMLFile( model_file, model_features )) ) 00793 PLERROR( "Problem reading %s\n", model_file.c_str() ); 00794 } 00795 } 00796 00797 if( !scene ) 00798 { 00799 if( scene_file.length() == 0 ) 00800 PLERROR( "You need to provide either 'scene' or 'scene_file' option"); 00801 00802 scene = new SurfaceMesh(); 00803 00804 // red in the scene mesh 00805 if( scene_features.isNull() ) 00806 { 00807 if( !(scene->readVRMLFile( scene_file )) ) 00808 PLERROR( "Problem reading %s\n", scene_file.c_str() ); 00809 } 00810 else 00811 { 00812 if( !(scene->readVRMLFile( scene_file, scene_features )) ) 00813 PLERROR( "Problem reading %s\n", scene_file.c_str() ); 00814 } 00815 } 00816 00817 // check options compatible with information in meshes (mesh_type) 00818 if( scene->mesh_type == "PointSet" || model->mesh_type == "PointSet" ) 00819 PLWARNING( "ICP without edge information is untested. Beware!" ); 00820 if( scene->mesh_type != "FaceSet" && fine_matching ) 00821 { 00822 PLWARNING( "Disabling 'fine_matching' because scene has no face information." ); 00823 fine_matching = false; 00824 } 00825 00826 // create BinBallTrees for closest point in scene computations 00827 BallTreeNN btnn = new BallTreeNearestNeighbors(); 00828 btnn->setTrainingSet( scene->getVertexCoordsAndFeatures() ); 00829 btnn->setOption( "nstages", "-1" ); 00830 btnn->rmin = 1; 00831 btnn->build(); 00832 btnn->train(); 00833 btnn->num_neighbors = 1; 00834 btnn->copy_input = true; 00835 btnn->copy_target = false; 00836 btnn->copy_weight = false; 00837 btnn->copy_index = true; 00838 00839 btl = btnn; 00840 00841 real tx=0, ty=0, tz=0; 00842 real rx=0, ry=0, rz=0; 00843 00844 if( initial_transform != "" ) 00845 { 00846 istringstream in( initial_transform ); 00847 if( !( in >> tx >> ty >> tz >> rx >> ry >> rz ) ) 00848 { 00849 PLERROR( "initial_transform option in ICP should have syntax: \"tx ty tz rx ry rz\"" ); 00850 } 00851 } 00852 00853 if( read_trans_file ) 00854 { 00855 ifstream in( trans_file.c_str() ); 00856 if( !in ) 00857 PLERROR( "Cannot open file: %s (trans_file option).\n", 00858 trans_file.c_str() ); 00859 00860 in >> tx >> ty >> tz >> rx >> ry >> rz; 00861 } 00862 00863 // initial match 00864 match = new MeshMatch(); 00865 match->rot = rotationFromFixedAngles( rx, ry, rz ); 00866 match->trans[0] = tx; 00867 match->trans[1] = ty; 00868 match->trans[2] = tz; 00869 match->angles[0] = rx; 00870 match->angles[1] = ry; 00871 match->angles[2] = rz; 00872 00873 } 00874 00875 void ICP::run() 00876 { 00877 if( verbosity >= 1 ) 00878 { 00879 pout << "Weight method: " << weight_method << endl 00880 << "Area weight: " << area_weight << endl 00881 << "Overlap filter: " << overlap_filter << endl 00882 << "Overlap Threshold: " << smart_overlap_t << endl 00883 << endl; 00884 } 00885 00886 // determine the first best transformation 00887 int total_iterations = iterate(); 00888 MMatch m_best = match; 00889 00890 if( verbosity >= 2 ) 00891 pout << "Best registration so far\n" << *m_best << endl; // ?? 00892 00893 // randomly perturb transformation about minimum to get global minimum 00894 for( int i=0 ; i<n_per ; i++ ) 00895 { 00896 Mat random_r( 3, 3 ); 00897 Vec random_t( 3 ); 00898 randomTransformation( 5, model->getResolution(), random_r, random_t ); 00899 00900 // init transform 00901 product( match->rot, m_best->rot, random_r ); 00902 match->angles = fixedAnglesFromRotation( match->rot ); 00903 match->trans = m_best->trans + random_t; 00904 00905 total_iterations += iterate(); 00906 00907 if( match->error < m_best->error ) 00908 { 00909 m_best = match; 00910 } 00911 00912 if( verbosity >=3 ) 00913 { 00914 pout << "Best registration after " << i << " perturbations\n" 00915 << *m_best << endl; 00916 } 00917 } 00918 00919 if( verbosity >=2 ) 00920 { 00921 pout << "Best Registration\n" << *m_best << endl; 00922 pout << "Total iterations: " << total_iterations << endl; 00923 } 00924 00925 if( output_file.length()!=0 ) // write out transformed points 00926 { 00927 transformMesh( m_best->rot, m_best->trans, model ); 00928 model->writeVRMLFile( output_file ); 00929 } 00930 00931 if( write_trans_file ) 00932 { 00933 ofstream out( trans_file.c_str(), ios::trunc ); 00934 if( !out ) 00935 { 00936 PLERROR( "Cannot open file: %s.\n", trans_file.c_str() ); 00937 } 00938 00939 out << m_best->trans[0] << " " << m_best->trans[1] << " " << m_best->trans[2] << " " << m_best->angles[0] << " " << m_best->angles[1] << " " << m_best->angles[2] 00940 << endl; 00941 } 00942 00943 if( write_vtx_match ) 00944 { 00945 Mat vtx_match; 00946 00947 ofstream out( vtx_match_file.c_str(), ios::trunc ); 00948 if( !out ) 00949 PLERROR( "Cannot open file: %s.\n", vtx_match_file.c_str() ); 00950 00951 // vtx_match = computeVertexMatches(); 00952 out << vtx_matching << endl; 00953 } 00954 00955 } 00956 00957 /* 00958 Mat ICP::computeVertexMatches() 00959 { 00960 Mat vtx_match( 0, 0 ); 00961 00962 vertex_iterator vi, vi_end; 00963 tie(vi, vi_end) = vertices( *model->p_mesh ); 00964 for( int i=0 ; vi != vi_end ; vi++, i++ ) 00965 { 00966 if( !write_all_vtx_match && !vtx_matching[i] ) 00967 continue; 00968 00969 MVertex mv = model->getVertex( *vi ); 00970 00971 Vec dists; 00972 Vec outputs; 00973 btl->computeOutputAndCosts( mv->coord, Vec(), outputs, dists ); 00974 int closest_vertex = (int) outputs[0]; 00975 00976 // store match 00977 Vec vmatch(2); 00978 vmatch[0] = i; 00979 vmatch[1] = closest_vertex; 00980 vtx_match.appendRow( vmatch ); 00981 } 00982 return vtx_match; 00983 } 00984 */ 00985 00986 00987 // This should become a pointer to a function returning the weight of 00988 // one pair of points, given their indices and coordinates 00989 real ICP::weightOracle( int, Vec, int, Vec ) 00990 { 00991 return real(1); 00992 } 00993 00994 void ICP::makeDeepCopyFromShallowCopy(CopiesMap& copies) 00995 { 00996 inherited::makeDeepCopyFromShallowCopy(copies); 00997 00998 // ### Call deepCopyField on all "pointer-like" fields 00999 // ### that you wish to be deepCopied rather than 01000 // ### shallow-copied. 01001 // ### ex: 01002 // deepCopyField(trainvec, copies); 01003 deepCopyField( model_features, copies ); 01004 deepCopyField( scene_features, copies ); 01005 01006 01007 // ### Remove this line when you have fully implemented this method. 01008 PLERROR("ICP::makeDeepCopyFromShallowCopy not fully (correctly) implemented yet!"); 01009 } 01010 01011 } // end of namespace PLearn