icp.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  * Point Cloud Library (PCL) - www.pointclouds.org
00005  * Copyright (c) 2009-2012, Willow Garage, Inc.
00006  * Copyright (c) 2012-, Open Perception, Inc.
00007  *
00008  * All rights reserved.
00009  *
00010  * Redistribution and use in source and binary forms, with or without
00011  * modification, are permitted provided that the following conditions
00012  * are met:
00013  *
00014  *  * Redistributions of source code must retain the above copyright
00015  *    notice, this list of conditions and the following disclaimer.
00016  *  * Redistributions in binary form must reproduce the above
00017  *    copyright notice, this list of conditions and the following
00018  *    disclaimer in the documentation and/or other materials provided
00019  *    with the distribution.
00020  *  * Neither the name of the copyright holder(s) nor the names of its
00021  *    contributors may be used to endorse or promote products derived
00022  *    from this software without specific prior written permission.
00023  *
00024  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00025  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00026  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00027  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00028  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00029  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00030  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00031  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00032  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00033  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00034  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00035  * POSSIBILITY OF SUCH DAMAGE.
00036  *
00037  * $Id$
00038  *
00039  */
00040 
00041 #include <pcl/apps/in_hand_scanner/icp.h>
00042 
00043 #include <limits>
00044 #include <cstdlib>
00045 #include <iomanip>
00046 #include <cmath>
00047 
00048 #include <pcl/kdtree/kdtree_flann.h>
00049 #include <pcl/common/centroid.h>
00050 #include <pcl/common/time.h>
00051 
00052 #include <pcl/apps/in_hand_scanner/utils.h>
00053 
00055 
00056 pcl::ihs::ICP::ICP ()
00057   : kd_tree_ (new pcl::KdTreeFLANN <PointNormal> ()),
00058 
00059     epsilon_        (10e-6f),
00060     max_iterations_ (50),
00061     min_overlap_    (.75f),
00062     max_fitness_    (.1f),
00063 
00064     factor_ (9.f),
00065     max_angle_ (45.f)
00066 {
00067 }
00068 
00070 
00071 void
00072 pcl::ihs::ICP::setEpsilon (const float epsilon)
00073 {
00074   if (epsilon > 0) epsilon_ = epsilon;
00075 }
00076 
00077 float
00078 pcl::ihs::ICP::getEpsilon () const
00079 {
00080   return (epsilon_);
00081 }
00082 
00084 
00085 void
00086 pcl::ihs::ICP::setMaxIterations (const unsigned int max_iter)
00087 {
00088   max_iterations_ = max_iter < 1 ? 1 : max_iter;
00089 }
00090 
00091 unsigned int
00092 pcl::ihs::ICP::getMaxIterations () const
00093 {
00094   return (max_iterations_);
00095 }
00096 
00098 
00099 void
00100 pcl::ihs::ICP::setMinOverlap (const float overlap)
00101 {
00102   min_overlap_ = pcl::ihs::clamp (overlap, 0.f, 1.f);
00103 }
00104 
00105 float
00106 pcl::ihs::ICP::getMinOverlap () const
00107 {
00108   return (min_overlap_);
00109 }
00110 
00112 
00113 void
00114 pcl::ihs::ICP::setMaxFitness (const float fitness)
00115 {
00116   if (fitness > 0) max_fitness_ = fitness;
00117 }
00118 
00119 float
00120 pcl::ihs::ICP::getMaxFitness () const
00121 {
00122   return (max_fitness_);
00123 }
00124 
00126 
00127 void
00128 pcl::ihs::ICP::setCorrespondenceRejectionFactor (const float factor)
00129 {
00130   factor_ = factor < 1.f ? 1.f : factor;
00131 }
00132 
00133 float
00134 pcl::ihs::ICP::getCorrespondenceRejectionFactor () const
00135 {
00136   return (factor_);
00137 }
00138 
00140 
00141 void
00142 pcl::ihs::ICP::setMaxAngle (const float angle)
00143 {
00144   max_angle_ = pcl::ihs::clamp (angle, 0.f, 180.f);
00145 }
00146 
00147 float
00148 pcl::ihs::ICP::getMaxAngle () const
00149 {
00150   return (max_angle_);
00151 }
00152 
00154 
00155 bool
00156 pcl::ihs::ICP::findTransformation (const MeshConstPtr&              mesh_model,
00157                                    const CloudXYZRGBNormalConstPtr& cloud_data,
00158                                    const Eigen::Matrix4f&           T_init,
00159                                    Eigen::Matrix4f&                 T_final)
00160 {
00161   // Check the input
00162   // TODO: Double check the minimum number of points necessary for icp
00163   const size_t n_min = 4;
00164 
00165   if(mesh_model->sizeVertices () < n_min || cloud_data->size () < n_min)
00166   {
00167     std::cerr << "ERROR in icp.cpp: Not enough input points!\n";
00168     return (false);
00169   }
00170 
00171   // Time measurements
00172   pcl::StopWatch sw;
00173   pcl::StopWatch sw_total;
00174   double t_select     = 0.;
00175   double t_build      = 0.;
00176   double t_nn_search  = 0.;
00177   double t_calc_trafo = 0.;
00178 
00179   // Convergence and registration failure
00180   float current_fitness  = 0.f;
00181   float previous_fitness = std::numeric_limits <float>::max ();
00182   float delta_fitness    = std::numeric_limits <float>::max ();
00183   float overlap          = std::numeric_limits <float>::quiet_NaN ();
00184 
00185   // Outlier rejection
00186   float squared_distance_threshold = std::numeric_limits<float>::max();
00187 
00188   // Transformation
00189   Eigen::Matrix4f T_cur = T_init;
00190 
00191   // Point selection
00192   sw.reset ();
00193   const CloudNormalConstPtr cloud_model_selected = this->selectModelPoints (mesh_model, T_init.inverse ());
00194   const CloudNormalConstPtr cloud_data_selected  = this->selectDataPoints (cloud_data);
00195   t_select = sw.getTime ();
00196 
00197   const size_t n_model = cloud_model_selected->size ();
00198   const size_t n_data  = cloud_data_selected->size ();
00199   if(n_model < n_min) {std::cerr << "ERROR in icp.cpp: Not enough model points after selection!\n"; return (false);}
00200   if(n_data < n_min)  {std::cerr << "ERROR in icp.cpp: Not enough data points after selection!\n"; return (false);}
00201 
00202   // Build a kd-tree
00203   sw.reset ();
00204   kd_tree_->setInputCloud (cloud_model_selected);
00205   t_build = sw.getTime ();
00206 
00207   std::vector <int>   index (1);
00208   std::vector <float> squared_distance (1);
00209 
00210   // Clouds with one to one correspondences
00211   CloudNormal cloud_model_corr;
00212   CloudNormal cloud_data_corr;
00213 
00214   cloud_model_corr.reserve (n_data);
00215   cloud_data_corr.reserve (n_data);
00216 
00217   // ICP main loop
00218   unsigned int iter = 1;
00219   PointNormal pt_d;
00220   const float dot_min = std::cos (max_angle_ * 17.45329252e-3); // deg to rad
00221   while (true)
00222   {
00223     // Accumulated error
00224     float squared_distance_sum = 0.f;
00225 
00226     // NN search
00227     cloud_model_corr.clear ();
00228     cloud_data_corr.clear ();
00229     sw.reset ();
00230     for (CloudNormal::const_iterator it_d = cloud_data_selected->begin (); it_d!=cloud_data_selected->end (); ++it_d)
00231     {
00232       // Transform the data point
00233       pt_d = *it_d;
00234       pt_d.getVector4fMap ()       = T_cur * pt_d.getVector4fMap ();
00235       pt_d.getNormalVector4fMap () = T_cur * pt_d.getNormalVector4fMap ();
00236 
00237       // Find the correspondence to the model points
00238       if (!kd_tree_->nearestKSearch (pt_d, 1, index, squared_distance))
00239       {
00240         std::cerr << "ERROR in icp.cpp: nearestKSearch failed!\n";
00241         return (false);
00242       }
00243 
00244       // Check the distance threshold
00245       if (squared_distance [0] < squared_distance_threshold)
00246       {
00247         if (index [0] >= cloud_model_selected->size ())
00248         {
00249           std::cerr << "ERROR in icp.cpp: Segfault!\n";
00250           std::cerr << "  Trying to access index " << index [0] << " >= " << cloud_model_selected->size () << std::endl;
00251           exit (EXIT_FAILURE);
00252         }
00253 
00254         const PointNormal& pt_m = cloud_model_selected->operator [] (index [0]);
00255 
00256         // Check the normals threshold
00257         if (pt_m.getNormalVector4fMap ().dot (pt_d.getNormalVector4fMap ()) > dot_min)
00258         {
00259           squared_distance_sum += squared_distance [0];
00260 
00261           cloud_model_corr.push_back (pt_m);
00262           cloud_data_corr.push_back (pt_d);
00263         }
00264       }
00265     }
00266 
00267     t_nn_search += sw.getTime ();
00268 
00269     const size_t n_corr = cloud_data_corr.size ();
00270     if (n_corr < n_min)
00271     {
00272       std::cerr << "ERROR in icp.cpp: Not enough correspondences: " << n_corr << " < " << n_min << std::endl;
00273       return (false);
00274     }
00275 
00276     // NOTE: The fitness is calculated with the transformation from the previous iteration (I don't re-calculate it after the transformation estimation). This means that the actual fitness will be one iteration "better" than the calculated fitness suggests. This should be no problem because the difference is small at the state of convergence.
00277     previous_fitness           = current_fitness;
00278     current_fitness            = squared_distance_sum / static_cast <float> (n_corr);
00279     delta_fitness              = std::abs (previous_fitness - current_fitness);
00280     squared_distance_threshold = factor_ * current_fitness;
00281     overlap                    = static_cast <float> (n_corr) / static_cast <float> (n_data);
00282 
00283     //    std::cerr << "Iter: " << std::left << std::setw(3) << iter
00284     //              << " | Overlap: " << std::setprecision(2) << std::setw(4) << overlap
00285     //              << " | current fitness: " << std::setprecision(5) << std::setw(10) << current_fitness
00286     //              << " | delta fitness: " << std::setprecision(5) << std::setw(10) << delta_fitness << std::endl;
00287 
00288     // Minimize the point to plane distance
00289     sw.reset ();
00290     Eigen::Matrix4f T_delta = Eigen::Matrix4f::Identity ();
00291     if (!this->minimizePointPlane (cloud_data_corr, cloud_model_corr, T_delta))
00292     {
00293       std::cerr << "ERROR in icp.cpp: minimizePointPlane failed!\n";
00294       return (false);
00295     }
00296     t_calc_trafo += sw.getTime ();
00297 
00298     T_cur = T_delta * T_cur;
00299 
00300     // Convergence
00301     if (delta_fitness < epsilon_) break;
00302     ++iter;
00303     if (iter > max_iterations_)   break;
00304 
00305   } // End ICP main loop
00306 
00307   // Some output
00308   std::cerr << "Registration:\n"
00309 
00310             << "  - num model     / num data       : "
00311             << std::setw (8) << std::right << n_model << " / "
00312             << std::setw (8) << std::left  << n_data << "\n"
00313 
00314             << std::scientific << std::setprecision (1)
00315 
00316             << "  - delta fitness / epsilon        : "
00317             << std::setw (8) << std::right << delta_fitness << " / "
00318             << std::setw (8) << std::left  << epsilon_
00319             << (delta_fitness < epsilon_ ? " <-- :-)\n" : "\n")
00320 
00321             << "  - fitness       / max fitness    : "
00322             << std::setw (8) << std::right << current_fitness << " / "
00323             << std::setw (8) << std::left  << max_fitness_
00324             << (current_fitness > max_fitness_ ? " <-- :-(\n" : "\n")
00325 
00326             << std::fixed << std::setprecision (2)
00327 
00328             << "  - iter          / max iter       : "
00329             << std::setw (8) << std::right << iter << " / "
00330             << std::setw (8) << std::left  << max_iterations_
00331             << (iter > max_iterations_ ? " <-- :-(\n" : "\n")
00332 
00333             << "  - overlap       / min overlap    : "
00334             << std::setw (8) << std::right << overlap << " / "
00335             << std::setw (8) << std::left  << min_overlap_
00336             << (overlap < min_overlap_ ? " <-- :-(\n" : "\n")
00337 
00338             << std::fixed << std::setprecision (0)
00339 
00340             << "  - time select                    : "
00341             << std::setw (8) << std::right << t_select << " ms\n"
00342 
00343             << "  - time build kd-tree             : "
00344             << std::setw (8) << std::right << t_build << " ms\n"
00345 
00346             << "  - time nn-search / trafo / reject: "
00347             << std::setw (8) << std::right << t_nn_search << " ms\n"
00348 
00349             << "  - time minimize                  : "
00350             << std::setw (8) << std::right << t_calc_trafo << " ms\n"
00351 
00352             << "  - total time                     : "
00353             << std::setw (8) << std::right << sw_total.getTime () << " ms\n";
00354 
00355   if (iter > max_iterations_ || overlap <  min_overlap_ || current_fitness > max_fitness_)
00356   {
00357     return (false);
00358   }
00359   else if (delta_fitness <= epsilon_)
00360   {
00361     T_final = T_cur;
00362     return (true);
00363   }
00364   else
00365   {
00366     std::cerr << "ERROR in icp.cpp: Congratulations! you found a bug.\n";
00367     exit (EXIT_FAILURE);
00368   }
00369 }
00370 
00372 
00373 pcl::ihs::ICP::CloudNormalConstPtr
00374 pcl::ihs::ICP::selectModelPoints (const MeshConstPtr&    mesh_model,
00375                                   const Eigen::Matrix4f& T_inv) const
00376 {
00377   const CloudNormalPtr cloud_model_out (new CloudNormal ());
00378   cloud_model_out->reserve (mesh_model->sizeVertices ());
00379 
00380   const Mesh::VertexDataCloud& cloud = mesh_model->getVertexDataCloud ();
00381 
00382   for (Mesh::VertexDataCloud::const_iterator it=cloud.begin (); it!=cloud.end (); ++it)
00383   {
00384     // Don't consider points that are facing away from the camera.
00385     if ((T_inv * it->getNormalVector4fMap ()).z () < 0.f)
00386     {
00387       PointNormal pt;
00388       pt.getVector4fMap ()       = it->getVector4fMap ();
00389       pt.getNormalVector4fMap () = it->getNormalVector4fMap ();
00390 
00391       // NOTE: Not the transformed points!!
00392       cloud_model_out->push_back (pt);
00393     }
00394   }
00395 
00396   return (cloud_model_out);
00397 }
00398 
00400 
00401 pcl::ihs::ICP::CloudNormalConstPtr
00402 pcl::ihs::ICP::selectDataPoints (const CloudXYZRGBNormalConstPtr& cloud_data) const
00403 {
00404   const CloudNormalPtr cloud_data_out (new CloudNormal ());
00405   cloud_data_out->reserve (cloud_data->size ());
00406 
00407   CloudXYZRGBNormal::const_iterator it_in = cloud_data->begin ();
00408   for (; it_in!=cloud_data->end (); ++it_in)
00409   {
00410     if (!boost::math::isnan (it_in->x))
00411     {
00412       PointNormal pt;
00413       pt.getVector4fMap ()       = it_in->getVector4fMap ();
00414       pt.getNormalVector4fMap () = it_in->getNormalVector4fMap ();
00415 
00416       cloud_data_out->push_back (pt);
00417     }
00418   }
00419 
00420   return (cloud_data_out);
00421 }
00422 
00424 
00425 bool
00426 pcl::ihs::ICP::minimizePointPlane (const CloudNormal& cloud_source,
00427                                    const CloudNormal& cloud_target,
00428                                    Eigen::Matrix4f&   T) const
00429 {
00430   // Check the input
00431   // n < n_min already checked in the icp main loop
00432   const size_t n = cloud_source.size ();
00433   if (cloud_target.size () != n)
00434   {
00435     std::cerr << "ERROR in icp.cpp: Input must have the same size!\n";
00436     return (false);
00437   }
00438 
00439   // For numerical stability
00440   // - Low: Linear Least-Squares Optimization for Point-to-Plane ICP Surface Registration (2004), in the discussion: "To improve the numerical stability of the computation, it is important to use a unit of distance that is comparable in magnitude with the rotation angles. The simplest way is to rescale and move the two input surfaces so that they are bounded within a unit sphere or cube centered at the origin."
00441   // - Gelfand et al.: Geometrically Stable Sampling for the ICP Algorithm (2003), in sec 3.1: "As is common with PCA methods, we will shift the center of mass of the points   to the origin." ... "Therefore, af- ter shifting the center of mass, we will scale the point set so that the average distance of points from the origin is 1."
00442   // - Hartley, Zisserman: - Multiple View Geometry (2004), page 109: They normalize to sqrt(2)
00443   // TODO: Check the resulting C matrix for the conditioning.
00444 
00445   // Subtract the centroid and calculate the scaling factor
00446   Eigen::Vector4f c_s (0.f, 0.f, 0.f, 1.f);
00447   Eigen::Vector4f c_t (0.f, 0.f, 0.f, 1.f);
00448   pcl::compute3DCentroid (cloud_source, c_s); c_s.w () = 1.f;
00449   pcl::compute3DCentroid (cloud_target, c_t); c_t.w () = 1.f;
00450 
00451   // The normals are only needed for the target
00452   typedef std::vector <Eigen::Vector4f, Eigen::aligned_allocator <Eigen::Vector4f> > Vec4Xf;
00453   Vec4Xf xyz_s, xyz_t, nor_t;
00454   xyz_s.reserve (n);
00455   xyz_t.reserve (n);
00456   nor_t.reserve (n);
00457 
00458   CloudNormal::const_iterator it_s = cloud_source.begin ();
00459   CloudNormal::const_iterator it_t = cloud_target.begin ();
00460 
00461   float accum = 0.f;
00462   Eigen::Vector4f pt_s, pt_t;
00463   for (; it_s!=cloud_source.end (); ++it_s, ++it_t)
00464   {
00465     // Subtract the centroid
00466     pt_s = it_s->getVector4fMap () - c_s;
00467     pt_t = it_t->getVector4fMap () - c_t;
00468 
00469     xyz_s.push_back (pt_s);
00470     xyz_t.push_back (pt_t);
00471     nor_t.push_back (it_t->getNormalVector4fMap ());
00472 
00473     // Calculate the radius (L2 norm) of the bounding sphere through both shapes and accumulate the average
00474     // TODO: Change to squared norm and adapt the rest accordingly
00475     accum += pt_s.head <3> ().norm () + pt_t.head <3> ().norm ();
00476   }
00477 
00478   // Inverse factor (do a multiplication instead of division later)
00479   const float factor         = 2.f * static_cast <float> (n) / accum;
00480   const float factor_squared = factor*factor;
00481 
00482   // Covariance matrix C
00483   Eigen::Matrix <float, 6, 6> C;
00484 
00485   // Right hand side vector b
00486   Eigen::Matrix <float, 6, 1> b;
00487 
00488   // For Eigen vectorization: use 4x4 submatrixes instead of 3x3 submatrixes
00489   // -> top left 3x3 matrix will form the final C
00490   // Same for b
00491   Eigen::Matrix4f C_tl    = Eigen::Matrix4f::Zero(); // top left corner
00492   Eigen::Matrix4f C_tr_bl = Eigen::Matrix4f::Zero(); // top right / bottom left
00493   Eigen::Matrix4f C_br    = Eigen::Matrix4f::Zero(); // bottom right
00494 
00495   Eigen::Vector4f b_t     = Eigen::Vector4f::Zero(); // top
00496   Eigen::Vector4f b_b     = Eigen::Vector4f::Zero(); // bottom
00497 
00498   Vec4Xf::const_iterator it_xyz_s = xyz_s.begin ();
00499   Vec4Xf::const_iterator it_xyz_t = xyz_t.begin ();
00500   Vec4Xf::const_iterator it_nor_t = nor_t.begin ();
00501 
00502   Eigen::Vector4f cross;
00503   float dot;
00504   for (; it_xyz_s!=xyz_s.end (); ++it_xyz_s, ++it_xyz_t, ++it_nor_t)
00505   {
00506     cross    = it_xyz_s->cross3 (*it_nor_t);
00507 
00508     C_tl    += cross     * cross.    transpose ();
00509     C_tr_bl += cross     * it_nor_t->transpose ();
00510     C_br    += *it_nor_t * it_nor_t->transpose ();
00511 
00512     dot      = (*it_xyz_t-*it_xyz_s).dot (*it_nor_t);
00513 
00514     b_t     += cross     * dot;
00515     b_b     += *it_nor_t * dot;
00516   }
00517 
00518   // Scale with the factor and copy the 3x3 submatrixes into C and b
00519   C_tl    *= factor_squared;
00520   C_tr_bl *= factor;
00521 
00522   C << C_tl.  topLeftCorner <3, 3> ()            , C_tr_bl.topLeftCorner <3, 3> (),
00523       C_tr_bl.topLeftCorner <3, 3> ().transpose(), C_br.   topLeftCorner <3, 3> ();
00524 
00525   b << b_t.head <3> () * factor_squared,
00526       b_b. head <3> () * factor;
00527 
00528   // Solve C * x = b with a Cholesky factorization with pivoting
00529   // x = [alpha; beta; gamma; trans_x; trans_y; trans_z]
00530   Eigen::Matrix <float, 6, 1> x = C.selfadjointView <Eigen::Lower> ().ldlt ().solve (b);
00531 
00532   // The calculated transformation in the scaled coordinate system
00533   const float
00534       sa = std::sin (x (0)),
00535       ca = std::cos (x (0)),
00536       sb = std::sin (x (1)),
00537       cb = std::cos (x (1)),
00538       sg = std::sin (x (2)),
00539       cg = std::cos (x (2)),
00540       tx = x (3),
00541       ty = x (4),
00542       tz = x (5);
00543 
00544   Eigen::Matrix4f TT;
00545   TT << cg*cb, -sg*ca+cg*sb*sa,  sg*sa+cg*sb*ca, tx,
00546       sg*cb  ,  cg*ca+sg*sb*sa, -cg*sa+sg*sb*ca, ty,
00547       -sb    ,  cb*sa         ,  cb*ca         , tz,
00548       0.f    ,  0.f           ,  0.f           , 1.f;
00549 
00550   // Transformation matrixes into the local coordinate systems of model/data
00551   Eigen::Matrix4f T_s, T_t;
00552 
00553   T_s << factor, 0.f   , 0.f   , -c_s.x () * factor,
00554       0.f      , factor, 0.f   , -c_s.y () * factor,
00555       0.f      , 0.f   , factor, -c_s.z () * factor,
00556       0.f      , 0.f   , 0.f   ,  1.f;
00557 
00558   T_t << factor, 0.f   , 0.f   , -c_t.x () * factor,
00559       0.f      , factor, 0.f   , -c_t.y () * factor,
00560       0.f      , 0.f   , factor, -c_t.z () * factor,
00561       0.f      , 0.f   , 0.f   ,  1.f;
00562 
00563   // Output transformation T
00564   T = T_t.inverse () * TT * T_s;
00565 
00566   return (true);
00567 }
00568 


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:24:53