Fix pclomp::GICP for humble

include/pclomp/gicp_omp.h

@@ -119,7 +119,9 @@ class GeneralizedIterativeClosestPoint : public pcl::IterativeClosestPoint<Point
     gicp_epsilon_(0.001),
     rotation_epsilon_(2e-3),
     mahalanobis_(0),
-    max_inner_iterations_(20)
+    max_inner_iterations_(20),
+    translation_gradient_tolerance_(1e-2),
+    rotation_gradient_tolerance_(1e-2)
   {
     min_number_correspondences_ = 4;
     reg_name_ = "GeneralizedIterativeClosestPoint";
@@ -245,6 +247,27 @@ class GeneralizedIterativeClosestPoint : public pcl::IterativeClosestPoint<Point
   ///\return maximum number of iterations at the optimization step
   int getMaximumOptimizerIterations() { return (max_inner_iterations_); }
 
+  /** \brief Set the minimal translation gradient threshold for early optimization stop
+   * \param[in] tolerance gradient threshold in meters
+   */
+  void setTranslationGradientTolerance(double tolerance)
+  {
+    translation_gradient_tolerance_ = tolerance;
+  }
+
+  /** \brief Return the minimal translation gradient threshold for early optimization stop
+   */
+  double getTranslationGradientTolerance() const { return translation_gradient_tolerance_; }
+
+  /** \brief Set the minimal rotation gradient threshold for early optimization stop
+   * \param[in] tolerance gradient threshold in radians
+   */
+  void setRotationGradientTolerance(double tolerance) { rotation_gradient_tolerance_ = tolerance; }
+
+  /** \brief Return the minimal rotation gradient threshold for early optimization stop
+   */
+  double getRotationGradientTolerance() const { return rotation_gradient_tolerance_; }
+
 protected:
   /** \brief The number of neighbors used for covariances computation.
    * default: 20
@@ -290,6 +313,12 @@ class GeneralizedIterativeClosestPoint : public pcl::IterativeClosestPoint<Point
   /** \brief maximum number of optimizations */
   int max_inner_iterations_;
 
+  /** \brief minimal translation gradient for early optimization stop */
+  double translation_gradient_tolerance_;
+
+  /** \brief minimal rotation gradient for early optimization stop */
+  double rotation_gradient_tolerance_;
+
   /** \brief compute points covariances matrices according to the K nearest
    * neighbors. K is set via setCorrespondenceRandomness() method.
    * \param cloud pointer to point cloud
@@ -347,6 +376,9 @@ class GeneralizedIterativeClosestPoint : public pcl::IterativeClosestPoint<Point
     double operator()(const Vector6d & x) override;
     void df(const Vector6d & x, Vector6d & df) override;
     void fdf(const Vector6d & x, double & f, Vector6d & df) override;
+#if PCL_VERSION_COMPARE(>=, 1, 11, 0)
+    BFGSSpace::Status checkGradient(const Vector6d & g) override;
+#endif
 
     const GeneralizedIterativeClosestPoint * gicp_;
   };

include/pclomp/gicp_omp_impl.hpp

@@ -233,7 +233,11 @@ void pclomp::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::
     if (result) {
       break;
     }
+#if PCL_VERSION_COMPARE(<, 1, 11, 0)
     result = bfgs.testGradient(gradient_tol);
+#else
+    result = bfgs.testGradient();
+#endif
   } while (result == BFGSSpace::Running && inner_iterations_ < max_inner_iterations_);
   if (
     result == BFGSSpace::NoProgress || result == BFGSSpace::Success ||
@@ -388,6 +392,31 @@ inline void pclomp::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::
   gicp_->computeRDerivative(x, R, g);
 }
 
+#if PCL_VERSION_COMPARE(>=, 1, 11, 0)
+////////////////////////////////////////////////////////////////////////////////////////
+template <typename PointSource, typename PointTarget>
+inline BFGSSpace::Status pclomp::GeneralizedIterativeClosestPoint<
+  PointSource, PointTarget>::OptimizationFunctorWithIndices::checkGradient(const Vector6d & g)
+{
+  auto translation_epsilon = gicp_->translation_gradient_tolerance_;
+  auto rotation_epsilon = gicp_->rotation_gradient_tolerance_;
+
+  if ((translation_epsilon < 0.) || (rotation_epsilon < 0.))
+    return BFGSSpace::NegativeGradientEpsilon;
+
+  // express translation gradient as norm of translation parameters
+  auto translation_grad = g.head<3>().norm();
+
+  // express rotation gradient as a norm of rotation parameters
+  auto rotation_grad = g.tail<3>().norm();
+
+  if ((translation_grad < translation_epsilon) && (rotation_grad < rotation_epsilon))
+    return BFGSSpace::Success;
+
+  return BFGSSpace::Running;
+}
+#endif
+
 ////////////////////////////////////////////////////////////////////////////////////////
 template <typename PointSource, typename PointTarget>
 inline void

include/pclomp/ndt_omp.h

@@ -51,6 +51,8 @@
 
 #include <pcl/registration/registration.h>
 
+#include <set>
+
 namespace pclomp
 {