PLearn 0.1
ICP.cc
Go to the documentation of this file.
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
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines