Fix ICP misbehavior in the "failure after maximum iterations" mode

registration/include/pcl/registration/default_convergence_criteria.h

@@ -76,7 +76,8 @@ namespace pcl
           CONVERGENCE_CRITERIA_TRANSFORM,
           CONVERGENCE_CRITERIA_ABS_MSE,
           CONVERGENCE_CRITERIA_REL_MSE,
-          CONVERGENCE_CRITERIA_NO_CORRESPONDENCES
+          CONVERGENCE_CRITERIA_NO_CORRESPONDENCES,
+          CONVERGENCE_CRITERIA_FAILURE_AFTER_MAX_ITERATIONS
         };
 
         /** \brief Empty constructor.
@@ -111,14 +112,14 @@ namespace pcl
         /** \brief Empty destructor */
         ~DefaultConvergenceCriteria () {}
 
-        /** \brief Set the maximum number of iterations that the internal rotation, 
+        /** \brief Set the maximum number of consecutive iterations that the internal rotation, 
           * translation, and MSE differences are allowed to be similar. 
           * \param[in] nr_iterations the maximum number of iterations 
           */
         inline void
         setMaximumIterationsSimilarTransforms (const int nr_iterations) { max_iterations_similar_transforms_ = nr_iterations; }
 
-        /** \brief Get the maximum number of iterations that the internal rotation, 
+        /** \brief Get the maximum number of consecutive iterations that the internal rotation, 
           * translation, and MSE differences are allowed to be similar, as set by the user.
           */
         inline int

registration/include/pcl/registration/impl/default_convergence_criteria.hpp

@@ -46,17 +46,26 @@
 template <typename Scalar> bool
 pcl::registration::DefaultConvergenceCriteria<Scalar>::hasConverged ()
 {
-  convergence_state_ = CONVERGENCE_CRITERIA_NOT_CONVERGED;
+  if (convergence_state_ != CONVERGENCE_CRITERIA_NOT_CONVERGED)
+  {
+    //If it already converged or failed before, reset.
+    iterations_similar_transforms_ = 0;
+    convergence_state_ = CONVERGENCE_CRITERIA_NOT_CONVERGED;
+  }
+
+  bool is_similar = false;
 
   PCL_DEBUG ("[pcl::DefaultConvergenceCriteria::hasConverged] Iteration %d out of %d.\n", iterations_, max_iterations_);
   // 1. Number of iterations has reached the maximum user imposed number of iterations
   if (iterations_ >= max_iterations_)
   {
-    if (failure_after_max_iter_)
-      return (false);
-
-    convergence_state_ = CONVERGENCE_CRITERIA_ITERATIONS;
-    return (true);
+    if (!failure_after_max_iter_)
+    {
+      iterations_similar_transforms_ = 0;
+      convergence_state_ = CONVERGENCE_CRITERIA_ITERATIONS;
+      return (true);
+    }
+    convergence_state_ = CONVERGENCE_CRITERIA_FAILURE_AFTER_MAX_ITERATIONS;
   }
 
   // 2. The epsilon (difference) between the previous transformation and the current estimated transformation
@@ -68,18 +77,13 @@ pcl::registration::DefaultConvergenceCriteria<Scalar>::hasConverged ()
 
   if (cos_angle >= rotation_threshold_ && translation_sqr <= translation_threshold_)
   {
-    if (iterations_similar_transforms_ < max_iterations_similar_transforms_)
-    {
-      // Increment the number of transforms that the thresholds are allowed to be similar
-      ++iterations_similar_transforms_;
-      return (false);
-    }
-    else
+    if (iterations_similar_transforms_ >= max_iterations_similar_transforms_)
     {
       iterations_similar_transforms_ = 0;
       convergence_state_ = CONVERGENCE_CRITERIA_TRANSFORM;
       return (true);
     }
+    is_similar = true;
   }
 
   correspondences_cur_mse_ = calculateMSE (correspondences_);
@@ -89,35 +93,36 @@ pcl::registration::DefaultConvergenceCriteria<Scalar>::hasConverged ()
   // Absolute
   if (fabs (correspondences_cur_mse_ - correspondences_prev_mse_) < mse_threshold_absolute_)
   {
-    if (iterations_similar_transforms_ < max_iterations_similar_transforms_)
-    {
-      // Increment the number of transforms that the thresholds are allowed to be similar
-      ++iterations_similar_transforms_;
-      return (false);
-    }
-    else
+    if (iterations_similar_transforms_ >= max_iterations_similar_transforms_)
     {
       iterations_similar_transforms_ = 0;
       convergence_state_ = CONVERGENCE_CRITERIA_ABS_MSE;
       return (true);
     }
+    is_similar = true;
   }
 
   // Relative
   if (fabs (correspondences_cur_mse_ - correspondences_prev_mse_) / correspondences_prev_mse_ < mse_threshold_relative_)
   {
-    if (iterations_similar_transforms_ < max_iterations_similar_transforms_)
-    {
-      // Increment the number of transforms that the thresholds are allowed to be similar
-      ++iterations_similar_transforms_;
-      return (false);
-    }
-    else
+    if (iterations_similar_transforms_ >= max_iterations_similar_transforms_)
     {
       iterations_similar_transforms_ = 0;
       convergence_state_ = CONVERGENCE_CRITERIA_REL_MSE;
       return (true);
     }
+    is_similar = true;
+  }
+
+  if (is_similar)
+  {
+    // Increment the number of transforms that the thresholds are allowed to be similar
+    ++iterations_similar_transforms_;
+  }
+  else
+  {
+    // When the transform becomes large, reset.
+    iterations_similar_transforms_ = 0;
   }
 
   correspondences_prev_mse_ = correspondences_cur_mse_;

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

@@ -235,7 +235,7 @@ pcl::IterativeClosestPoint<PointSource, PointTarget, Scalar>::computeTransformat
 
     converged_ = static_cast<bool> ((*convergence_criteria_));
   }
-  while (!converged_);
+  while (convergence_criteria_->getConvergenceState() == pcl::registration::DefaultConvergenceCriteria<Scalar>::CONVERGENCE_CRITERIA_NOT_CONVERGED);
 
   // Transform the input cloud using the final transformation
   PCL_DEBUG ("Transformation is:\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n\t%5f\t%5f\t%5f\t%5f\n",