Merge pull request #1724 from carlosmccosta/improved_icp_ndt_converge…

…nce_metrics

Added option to specify translation and rotation convergence deltas in ICP and NDT algorithms.

registration/include/pcl/registration/icp.h

@@ -119,6 +119,7 @@ namespace pcl
       using Registration<PointSource, PointTarget, Scalar>::final_transformation_;
       using Registration<PointSource, PointTarget, Scalar>::transformation_;
       using Registration<PointSource, PointTarget, Scalar>::transformation_epsilon_;
+      using Registration<PointSource, PointTarget, Scalar>::transformation_rotation_epsilon_;
       using Registration<PointSource, PointTarget, Scalar>::converged_;
       using Registration<PointSource, PointTarget, Scalar>::corr_dist_threshold_;
       using Registration<PointSource, PointTarget, Scalar>::inlier_threshold_;

registration/include/pcl/registration/impl/icp.hpp

@@ -163,7 +163,10 @@ pcl::IterativeClosestPoint<PointSource, PointTarget, Scalar>::computeTransformat
   convergence_criteria_->setMaximumIterations (max_iterations_);
   convergence_criteria_->setRelativeMSE (euclidean_fitness_epsilon_);
   convergence_criteria_->setTranslationThreshold (transformation_epsilon_);
-  convergence_criteria_->setRotationThreshold (1.0 - transformation_epsilon_);
+  if (transformation_rotation_epsilon_ > 0)
+    convergence_criteria_->setRotationThreshold (transformation_rotation_epsilon_);
+  else
+    convergence_criteria_->setRotationThreshold (1.0 - transformation_epsilon_);
 
   // Repeat until convergence
   do

registration/include/pcl/registration/impl/ndt.hpp

@@ -156,14 +156,20 @@ pcl::NormalDistributionsTransform<PointSource, PointTarget>::computeTransformati
     if (update_visualizer_ != 0)
       update_visualizer_ (output, std::vector<int>(), *target_, std::vector<int>() );
 
-    if (nr_iterations_ > max_iterations_ ||
-        (nr_iterations_ && (std::fabs (delta_p_norm) < transformation_epsilon_)))
-    {
-      converged_ = true;
-    }
+    double cos_angle = 0.5 * (transformation_.coeff (0, 0) + transformation_.coeff (1, 1) + transformation_.coeff (2, 2) - 1);
+    double translation_sqr = transformation_.coeff (0, 3) * transformation_.coeff (0, 3) +
+                             transformation_.coeff (1, 3) * transformation_.coeff (1, 3) +
+                             transformation_.coeff (2, 3) * transformation_.coeff (2, 3);
 
     nr_iterations_++;
 
+    if (nr_iterations_ >= max_iterations_ ||
+        ((transformation_epsilon_ > 0 && translation_sqr <= transformation_epsilon_) && (transformation_rotation_epsilon_ > 0 && cos_angle >= transformation_rotation_epsilon_)) ||
+        ((transformation_epsilon_ <= 0)                                             && (transformation_rotation_epsilon_ > 0 && cos_angle >= transformation_rotation_epsilon_)) ||
+        ((transformation_epsilon_ > 0 && translation_sqr <= transformation_epsilon_) && (transformation_rotation_epsilon_ <= 0)))
+    {
+      converged_ = true;
+    }
   }
 
   // Store transformation probability.  The realtive differences within each scan registration are accurate

registration/include/pcl/registration/impl/ndt_2d.hpp

@@ -475,9 +475,19 @@ pcl::NormalDistributionsTransform2D<PointSource, PointTarget>::computeTransforma
 
     //std::cout << "eps=" << fabs ((transformation - previous_transformation_).sum ()) << std::endl;
 
-    if (nr_iterations_ > max_iterations_ ||
-       (transformation - previous_transformation_).array ().abs ().sum () < transformation_epsilon_)
+    Eigen::Matrix4f transformation_delta = transformation.inverse() * previous_transformation_;
+    double cos_angle = 0.5 * (transformation_delta.coeff (0, 0) + transformation_delta.coeff (1, 1) + transformation_delta.coeff (2, 2) - 1);
+    double translation_sqr = transformation_delta.coeff (0, 3) * transformation_delta.coeff (0, 3) +
+                               transformation_delta.coeff (1, 3) * transformation_delta.coeff (1, 3) +
+                               transformation_delta.coeff (2, 3) * transformation_delta.coeff (2, 3);
+
+    if (nr_iterations_ >= max_iterations_ ||
+        ((transformation_epsilon_ > 0 && translation_sqr <= transformation_epsilon_) && (transformation_rotation_epsilon_ > 0 && cos_angle >= transformation_rotation_epsilon_)) ||
+        ((transformation_epsilon_ <= 0)                                             && (transformation_rotation_epsilon_ > 0 && cos_angle >= transformation_rotation_epsilon_)) ||
+        ((transformation_epsilon_ > 0 && translation_sqr <= transformation_epsilon_) && (transformation_rotation_epsilon_ <= 0)))
+    {
       converged_ = true;
+    }
   }
   final_transformation_ = transformation;
   output = intm_cloud;

registration/include/pcl/registration/ndt.h

@@ -225,6 +225,7 @@ namespace pcl
       using Registration<PointSource, PointTarget>::final_transformation_;
       using Registration<PointSource, PointTarget>::transformation_;
       using Registration<PointSource, PointTarget>::transformation_epsilon_;
+      using Registration<PointSource, PointTarget>::transformation_rotation_epsilon_;
       using Registration<PointSource, PointTarget>::converged_;
       using Registration<PointSource, PointTarget>::corr_dist_threshold_;
       using Registration<PointSource, PointTarget>::inlier_threshold_;

registration/include/pcl/registration/ndt_2d.h

@@ -135,8 +135,9 @@ namespace pcl
       using Registration<PointSource, PointTarget>::nr_iterations_;
       using Registration<PointSource, PointTarget>::max_iterations_;
       using Registration<PointSource, PointTarget>::transformation_epsilon_;
+      using Registration<PointSource, PointTarget>::transformation_rotation_epsilon_;
       using Registration<PointSource, PointTarget>::transformation_;
-      using Registration<PointSource, PointTarget>::previous_transformation_;      
+      using Registration<PointSource, PointTarget>::previous_transformation_;
       using Registration<PointSource, PointTarget>::final_transformation_;
       using Registration<PointSource, PointTarget>::update_visualizer_;
       using Registration<PointSource, PointTarget>::indices_;

registration/include/pcl/registration/registration.h

@@ -110,6 +110,7 @@ namespace pcl
         , transformation_ (Matrix4::Identity ())
         , previous_transformation_ (Matrix4::Identity ())
         , transformation_epsilon_ (0.0)
+        , transformation_rotation_epsilon_(0.0)
         , euclidean_fitness_epsilon_ (-std::numeric_limits<double>::max ())
         , corr_dist_threshold_ (std::sqrt (std::numeric_limits<double>::max ()))
         , inlier_threshold_ (0.05)
@@ -326,7 +327,7 @@ namespace pcl
       inline double 
       getMaxCorrespondenceDistance () { return (corr_dist_threshold_); }
 
-      /** \brief Set the transformation epsilon (maximum allowable difference between two consecutive 
+      /** \brief Set the transformation epsilon (maximum allowable translation squared difference between two consecutive
         * transformations) in order for an optimization to be considered as having converged to the final 
         * solution.
         * \param[in] epsilon the transformation epsilon in order for an optimization to be considered as having 
@@ -335,20 +336,34 @@ namespace pcl
       inline void 
       setTransformationEpsilon (double epsilon) { transformation_epsilon_ = epsilon; }
 
-      /** \brief Get the transformation epsilon (maximum allowable difference between two consecutive 
+      /** \brief Get the transformation epsilon (maximum allowable translation squared difference between two consecutive
         * transformations) as set by the user.
         */
       inline double 
       getTransformationEpsilon () { return (transformation_epsilon_); }
 
+      /** \brief Set the transformation rotation epsilon (maximum allowable rotation difference between two consecutive
+        * transformations) in order for an optimization to be considered as having converged to the final
+        * solution.
+        * \param[in] epsilon the transformation rotation epsilon in order for an optimization to be considered as having
+        * converged to the final solution (epsilon is the cos(angle) in a axis-angle representation).
+        */
+      inline void
+      setTransformationRotationEpsilon (double epsilon) { transformation_rotation_epsilon_ = epsilon; }
+
+      /** \brief Get the transformation rotation epsilon (maximum allowable difference between two consecutive
+        * transformations) as set by the user (epsilon is the cos(angle) in a axis-angle representation).
+        */
+      inline double
+      getTransformationRotationEpsilon () { return (transformation_rotation_epsilon_); }
+
       /** \brief Set the maximum allowed Euclidean error between two consecutive steps in the ICP loop, before 
         * the algorithm is considered to have converged. 
         * The error is estimated as the sum of the differences between correspondences in an Euclidean sense, 
         * divided by the number of correspondences.
         * \param[in] epsilon the maximum allowed distance error before the algorithm will be considered to have
         * converged
         */
-
       inline void 
       setEuclideanFitnessEpsilon (double epsilon) { euclidean_fitness_epsilon_ = epsilon; }
 
@@ -515,6 +530,11 @@ namespace pcl
         */
       double transformation_epsilon_;
 
+      /** \brief The maximum rotation difference between two consecutive transformations in order to consider convergence
+        * (user defined).
+        */
+      double transformation_rotation_epsilon_;
+
       /** \brief The maximum allowed Euclidean error between two consecutive steps in the ICP loop, before the 
         * algorithm is considered to have converged. The error is estimated as the sum of the differences between 
         * correspondences in an Euclidean sense, divided by the number of correspondences.