Optimize SampleConsensusModelEllipse3D Levenberg-Marquardt instantiations

Move the heavy Levenberg-Marquardt optimization logic into a non-templated
'pcl::internal' namespace in the source file. This reduces redundant
instantiations from 61 down to 1, resulting in a ~24% reduction in
rebuild time for the sample_consensus library.

- Consolidation of LevenbergMarquardt into internal::optimizeModelCoefficientsEllipse3D
- Moving math helpers to pcl::internal for shared access
- Correcting template instantiations for core and non-core point types
- Matching parameter signature with other optimized internal models
- Fixing lint errors for modernize-return-braced-init-list

Author: AlrIsmail

sample_consensus/include/pcl/sample_consensus/impl/sac_model_ellipse3d.hpp

@@ -11,7 +11,6 @@
 
 #include <limits>
 
-#include <unsupported/Eigen/NonLinearOptimization> // for LevenbergMarquardt
 #include <pcl/sample_consensus/sac_model_ellipse3d.h>
 #include <pcl/common/concatenate.h>
 
@@ -225,7 +224,7 @@ pcl::SampleConsensusModelEllipse3D<PointT>::computeModelCoefficients (const Indi
   // Retrieve the ellipse point at the tilt angle t (par_t), along the local x-axis
   const Eigen::VectorXf params = (Eigen::VectorXf(5) << par_a, par_b, par_h, par_k, par_t).finished();
   Eigen::Vector3f p_th_(0.0, 0.0, 0.0);
-  get_ellipse_point(params, par_t, p_th_(0), p_th_(1));
+  internal::get_ellipse_point(params, par_t, p_th_(0), p_th_(1));
 
   // Redefinition of the x-axis of the ellipse's local reference frame
   x_axis = (Rot * p_th_).normalized();
@@ -280,16 +279,11 @@ pcl::SampleConsensusModelEllipse3D<PointT>::getDistancesToModel (const Eigen::Ve
   float th_opt;
 
   // Iterate through the 3D points and calculate the distances from them to the ellipse
-  for (std::size_t i = 0; i < indices_->size (); ++i)
-  // Calculate the distance from the point to the ellipse:
-  // 1.   calculate intersection point of the plane in which the ellipse lies and the
-  //      line from the sample point with the direction of the plane normal (projected point)
-  // 2.   calculate the intersection point of the line from the ellipse center to the projected point
-  //      with the ellipse
-  // 3.   calculate distance from corresponding point on the ellipse to the sample point
+  std::size_t i = 0;
+  for (const auto& index : *indices_)
   {
     // p : Sample Point
-    const Eigen::Vector3f p((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z);
+    const Eigen::Vector3f p((*input_)[index].x, (*input_)[index].y, (*input_)[index].z);
 
     // Local coordinates of sample point p
     const Eigen::Vector3f p_ = Rot_T * (p - c);
@@ -298,9 +292,9 @@ pcl::SampleConsensusModelEllipse3D<PointT>::getDistancesToModel (const Eigen::Ve
     // Calculate the shortest distance from the point to the ellipse which is given by
     // the norm of a vector that is normal to the ellipse tangent calculated at the
     // point it intersects the tangent.
-    const Eigen::Vector2f distanceVector = dvec2ellipse(params, p_(0), p_(1), th_opt);
+    const Eigen::Vector2f distanceVector = internal::dvec2ellipse(params, p_(0), p_(1), th_opt);
 
-    distances[i] = distanceVector.norm();
+    distances[i++] = distanceVector.norm();
   }
 }
 
@@ -343,10 +337,10 @@ pcl::SampleConsensusModelEllipse3D<PointT>::selectWithinDistance (
 
   const auto squared_threshold = threshold * threshold;
   // Iterate through the 3d points and calculate the distances from them to the ellipse
-  for (std::size_t i = 0; i < indices_->size (); ++i)
+  for (const auto& index : *indices_)
   {
     // p : Sample Point
-    const Eigen::Vector3f p((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z);
+    const Eigen::Vector3f p((*input_)[index].x, (*input_)[index].y, (*input_)[index].z);
 
     // Local coordinates of sample point p
     const Eigen::Vector3f p_ = Rot_T * (p - c);
@@ -357,13 +351,13 @@ pcl::SampleConsensusModelEllipse3D<PointT>::selectWithinDistance (
     // point it intersects the tangent.
     const Eigen::VectorXf params = (Eigen::VectorXf(5) << par_a, par_b, 0.0, 0.0, 0.0).finished();
     float th_opt;
-    const Eigen::Vector2f distanceVector = dvec2ellipse(params, p_(0), p_(1), th_opt);
+    const Eigen::Vector2f distanceVector = internal::dvec2ellipse(params, p_(0), p_(1), th_opt);
 
     const double sqr_dist = distanceVector.squaredNorm();
     if (sqr_dist < squared_threshold)
     {
       // Returns the indices of the points whose distances are smaller than the threshold
-      inliers.push_back ((*indices_)[i]);
+      inliers.push_back (index);
       error_sqr_dists_.push_back (sqr_dist);
     }
   }
@@ -402,10 +396,10 @@ pcl::SampleConsensusModelEllipse3D<PointT>::countWithinDistance (
 
   const auto squared_threshold = threshold * threshold;
   // Iterate through the 3d points and calculate the distances from them to the ellipse
-  for (std::size_t i = 0; i < indices_->size (); ++i)
+  for (const auto& index : *indices_)
   {
     // p : Sample Point
-    const Eigen::Vector3f p((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z);
+    const Eigen::Vector3f p((*input_)[index].x, (*input_)[index].y, (*input_)[index].z);
 
     // Local coordinates of sample point p
     const Eigen::Vector3f p_ = Rot_T * (p - c);
@@ -416,7 +410,7 @@ pcl::SampleConsensusModelEllipse3D<PointT>::countWithinDistance (
     // point it intersects the tangent.
     const Eigen::VectorXf params = (Eigen::VectorXf(5) << par_a, par_b, 0.0, 0.0, 0.0).finished();
     float th_opt;
-    const Eigen::Vector2f distanceVector = dvec2ellipse(params, p_(0), p_(1), th_opt);
+    const Eigen::Vector2f distanceVector = internal::dvec2ellipse(params, p_(0), p_(1), th_opt);
 
     if (distanceVector.squaredNorm() < squared_threshold)
       nr_p++;
@@ -443,51 +437,29 @@ pcl::SampleConsensusModelEllipse3D<PointT>::optimizeModelCoefficients (
   // Need more than the minimum sample size to make a difference
   if (inliers.size () <= sample_size_)
   {
-    PCL_ERROR ("[pcl::SampleConsensusModelEllipse3D::optimizeModelCoefficients] Not enough inliers to refine/optimize the model's coefficients (%lu)! Returning the same coefficients.\n", inliers.size ());
+    PCL_ERROR ("[pcl::SampleConsensusModelEllipse3D::optimizeModelCoefficients] Not enough inliers found to optimize model coefficients (%lu)! Returning the same coefficients.\n", inliers.size ());
     return;
   }
 
-  OptimizationFunctor functor(this, inliers);
-  Eigen::NumericalDiff<OptimizationFunctor> num_diff(functor);
-  Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, double> lm(num_diff);
-  Eigen::VectorXd coeff = model_coefficients.cast<double>();
-  int info = lm.minimize(coeff);
-  optimized_coefficients = coeff.cast<float>();
-
-  // Compute the L2 norm of the residuals
-  PCL_DEBUG ("[pcl::SampleConsensusModelEllipse3D::optimizeModelCoefficients] LM solver finished with exit code %i, having a residual norm of %g. \nInitial solution: %g %g %g %g %g %g %g %g %g %g %g\nFinal solution: %g %g %g %g %g %g %g %g %g %g %g\n",
-            info, lm.fvec.norm (),
-
-            model_coefficients[0],
-            model_coefficients[1],
-            model_coefficients[2],
-            model_coefficients[3],
-            model_coefficients[4],
-            model_coefficients[5],
-            model_coefficients[6],
-            model_coefficients[7],
-            model_coefficients[8],
-            model_coefficients[9],
-            model_coefficients[10],
-
-            optimized_coefficients[0],
-            optimized_coefficients[1],
-            optimized_coefficients[2],
-            optimized_coefficients[3],
-            optimized_coefficients[4],
-            optimized_coefficients[5],
-            optimized_coefficients[6],
-            optimized_coefficients[7],
-            optimized_coefficients[8],
-            optimized_coefficients[9],
-            optimized_coefficients[10]);
+  Eigen::ArrayXf x (inliers.size ()), y (inliers.size ()), z (inliers.size ());
+  std::size_t i = 0;
+  for (const auto& inlier : inliers)
+  {
+    const auto& pt = (*input_)[inlier];
+    x[i] = pt.x; y[i] = pt.y; z[i] = pt.z;
+    ++i;
+  }
+
+  internal::optimizeModelCoefficientsEllipse3D (optimized_coefficients, x, y, z);
 }
 
 //////////////////////////////////////////////////////////////////////////
 template <typename PointT> void
 pcl::SampleConsensusModelEllipse3D<PointT>::projectPoints (
-      const Indices &inliers, const Eigen::VectorXf &model_coefficients,
-      PointCloud &projected_points, bool copy_data_fields) const
+      const Indices &inliers,
+      const Eigen::VectorXf &model_coefficients,
+      PointCloud &projected_points,
+      bool copy_data_fields) const
 {
   // Needs a valid set of model coefficients
   if (!isModelValid (model_coefficients))
@@ -537,10 +509,11 @@ pcl::SampleConsensusModelEllipse3D<PointT>::projectPoints (
     const Eigen::Matrix3f Rot_T = Rot.transpose();
 
     // Iterate through the 3d points and calculate the distances from them to the plane
-    for (std::size_t i = 0; i < inliers.size (); ++i)
+    std::size_t i = 0;
+    for (const auto& inlier : inliers)
     {
       // p : Sample Point
-      const Eigen::Vector3f p((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z);
+      const Eigen::Vector3f p((*input_)[inlier].x, (*input_)[inlier].y, (*input_)[inlier].z);
 
       // Local coordinates of sample point p
       const Eigen::Vector3f p_ = Rot_T * (p - c);
@@ -551,17 +524,17 @@ pcl::SampleConsensusModelEllipse3D<PointT>::projectPoints (
       // point it intersects the tangent.
       const Eigen::VectorXf params = (Eigen::VectorXf(5) << par_a, par_b, 0.0, 0.0, 0.0).finished();
       float th_opt;
-      dvec2ellipse(params, p_(0), p_(1), th_opt);
+      internal::dvec2ellipse(params, p_(0), p_(1), th_opt);
 
       // Retrieve the ellipse point at the tilt angle t, along the local x-axis
       Eigen::Vector3f k_(0.0, 0.0, 0.0);
-      get_ellipse_point(params, th_opt, k_[0], k_[1]);
+      internal::get_ellipse_point(params, th_opt, k_[0], k_[1]);
 
       const Eigen::Vector3f k = c + Rot * k_;
 
       projected_points[i].x = static_cast<float> (k[0]);
       projected_points[i].y = static_cast<float> (k[1]);
-      projected_points[i].z = static_cast<float> (k[2]);
+      projected_points[i++].z = static_cast<float> (k[2]);
     }
   }
   else
@@ -599,10 +572,11 @@ pcl::SampleConsensusModelEllipse3D<PointT>::projectPoints (
     const Eigen::Matrix3f Rot_T = Rot.transpose();
 
     // Iterate through the 3d points and calculate the distances from them to the plane
-    for (std::size_t i = 0; i < inliers.size (); ++i)
+    std::size_t i = 0;
+    for (const auto& inlier : inliers)
     {
       // p : Sample Point
-      const Eigen::Vector3f p((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z);
+      const Eigen::Vector3f p((*input_)[inlier].x, (*input_)[inlier].y, (*input_)[inlier].z);
 
       // Local coordinates of sample point p
       const Eigen::Vector3f p_ = Rot_T * (p - c);
@@ -613,18 +587,18 @@ pcl::SampleConsensusModelEllipse3D<PointT>::projectPoints (
       // point it intersects the tangent.
       const Eigen::VectorXf params = (Eigen::VectorXf(5) << par_a, par_b, 0.0, 0.0, 0.0).finished();
       float th_opt;
-      dvec2ellipse(params, p_(0), p_(1), th_opt);
+      internal::dvec2ellipse(params, p_(0), p_(1), th_opt);
 
       // Retrieve the ellipse point at the tilt angle t, along the local x-axis
       //// model_coefficients[5] = static_cast<float>(par_t);
       Eigen::Vector3f k_(0.0, 0.0, 0.0);
-      get_ellipse_point(params, th_opt, k_[0], k_[1]);
+      internal::get_ellipse_point(params, th_opt, k_[0], k_[1]);
 
       const Eigen::Vector3f k = c + Rot * k_;
 
       projected_points[i].x = static_cast<float> (k[0]);
       projected_points[i].y = static_cast<float> (k[1]);
-      projected_points[i].z = static_cast<float> (k[2]);
+      projected_points[i++].z = static_cast<float> (k[2]);
     }
   }
 }
@@ -679,7 +653,7 @@ pcl::SampleConsensusModelEllipse3D<PointT>::doSamplesVerifyModel (
     // point it intersects the tangent.
     const Eigen::VectorXf params = (Eigen::VectorXf(5) << par_a, par_b, 0.0, 0.0, 0.0).finished();
     float th_opt;
-    const Eigen::Vector2f distanceVector = dvec2ellipse(params, p_(0), p_(1), th_opt);
+    const Eigen::Vector2f distanceVector = internal::dvec2ellipse(params, p_(0), p_(1), th_opt);
 
     if (distanceVector.squaredNorm() > squared_threshold)
       return (false);
@@ -710,152 +684,4 @@ pcl::SampleConsensusModelEllipse3D<PointT>::isModelValid (const Eigen::VectorXf
   return (true);
 }
 
-
-
-//////////////////////////////////////////////////////////////////////////
-template <typename PointT>
-void inline pcl::SampleConsensusModelEllipse3D<PointT>::get_ellipse_point(
-    const Eigen::VectorXf& par, float th, float& x, float& y)
-{
-  /*
-   * Calculates a point on the ellipse model 'par' using the angle 'th'.
-   */
-
-  // Parametric eq.params
-  const float par_a(par[0]);
-  const float par_b(par[1]);
-  const float par_h(par[2]);
-  const float par_k(par[3]);
-  const float par_t(par[4]);
-
-  x = par_h + std::cos(par_t) * par_a * std::cos(th) -
-      std::sin(par_t) * par_b * std::sin(th);
-  y = par_k + std::sin(par_t) * par_a * std::cos(th) +
-      std::cos(par_t) * par_b * std::sin(th);
-
-  return;
-}
-
-//////////////////////////////////////////////////////////////////////////
-template <typename PointT>
-Eigen::Vector2f inline pcl::SampleConsensusModelEllipse3D<PointT>::dvec2ellipse(
-    const Eigen::VectorXf& par, float u, float v, float& th_opt)
-{
-  /*
-   * Minimum distance vector from point p=(u,v) to the ellipse model 'par'.
-   */
-
-  // Parametric eq.params
-  // (par_a, par_b, and par_t do not need to be declared)
-  const float par_h = par[2];
-  const float par_k = par[3];
-
-  const Eigen::Vector2f center(par_h, par_k);
-  Eigen::Vector2f p(u, v);
-  p -= center;
-
-  // Local x-axis of the ellipse
-  Eigen::Vector2f x_axis(0.0, 0.0);
-  get_ellipse_point(par, 0.0, x_axis(0), x_axis(1));
-  x_axis -= center;
-
-  // Local y-axis of the ellipse
-  Eigen::Vector2f y_axis(0.0, 0.0);
-  get_ellipse_point(par, M_PI / 2.0, y_axis(0), y_axis(1));
-  y_axis -= center;
-
-  // Convert the point p=(u,v) to local ellipse coordinates
-  const float x_proj = p.dot(x_axis) / x_axis.norm();
-  const float y_proj = p.dot(y_axis) / y_axis.norm();
-
-  // Find the ellipse quandrant to where the point p=(u,v) belongs,
-  // and limit the search interval to 'th_min' and 'th_max'.
-  float th_min(0.0), th_max(0.0);
-  const float th = std::atan2(y_proj, x_proj);
-
-  if (-M_PI <= th && th < -M_PI / 2.0) {
-    // Q3
-    th_min = -M_PI;
-    th_max = -M_PI / 2.0;
-  }
-  if (-M_PI / 2.0 <= th && th < 0.0) {
-    // Q4
-    th_min = -M_PI / 2.0;
-    th_max = 0.0;
-  }
-  if (0.0 <= th && th < M_PI / 2.0) {
-    // Q1
-    th_min = 0.0;
-    th_max = M_PI / 2.0;
-  }
-  if (M_PI / 2.0 <= th && th <= M_PI) {
-    // Q2
-    th_min = M_PI / 2.0;
-    th_max = M_PI;
-  }
-
-  // Use an unconstrained line search optimizer to find the optimal th_opt
-  th_opt = golden_section_search(par, u, v, th_min, th_max, 1.e-3);
-
-  // Distance vector from a point (u,v) to a given point in the ellipse model 'par' at an angle 'th_opt'.
-  float x(0.0), y(0.0);
-  get_ellipse_point(par, th_opt, x, y);
-  Eigen::Vector2f distanceVector(u - x, v - y);
-  return distanceVector;
-}
-
-//////////////////////////////////////////////////////////////////////////
-template <typename PointT>
-float inline pcl::SampleConsensusModelEllipse3D<PointT>::golden_section_search(
-    const Eigen::VectorXf& par,
-    float u,
-    float v,
-    float th_min,
-    float th_max,
-    float epsilon)
-{
-  /*
-   * Golden section search
-   */
-
-  constexpr float phi(1.61803398874989484820f); // Golden ratio
-
-  // tl (theta lower bound), tu (theta upper bound)
-  float tl(th_min), tu(th_max);
-  float ta = tl + (tu - tl) * (1 - 1 / phi);
-  float tb = tl + (tu - tl) * 1 / phi;
-
-  while ((tu - tl) > epsilon) {
-
-    // theta a
-    float x_a(0.0), y_a(0.0);
-    get_ellipse_point(par, ta, x_a, y_a);
-    float squared_dist_ta = (u - x_a) * (u - x_a) + (v - y_a) * (v - y_a);
-
-    // theta b
-    float x_b(0.0), y_b(0.0);
-    get_ellipse_point(par, tb, x_b, y_b);
-    float squared_dist_tb = (u - x_b) * (u - x_b) + (v - y_b) * (v - y_b);
-
-    if (squared_dist_ta < squared_dist_tb) {
-      tu = tb;
-      tb = ta;
-      ta = tl + (tu - tl) * (1 - 1 / phi);
-    }
-    else if (squared_dist_ta > squared_dist_tb) {
-      tl = ta;
-      ta = tb;
-      tb = tl + (tu - tl) * 1 / phi;
-    }
-    else {
-      tl = ta;
-      tu = tb;
-      ta = tl + (tu - tl) * (1 - 1 / phi);
-      tb = tl + (tu - tl) * 1 / phi;
-    }
-  }
-  return (tl + tu) / 2.0;
-}
-
-
 #define PCL_INSTANTIATE_SampleConsensusModelEllipse3D(T) template class PCL_EXPORTS pcl::SampleConsensusModelEllipse3D<T>;