Optimize SampleConsensusModelEllipse3D build performance

Move the heavy LevenbergMarquardt optimization logic into a non-templated
'pcl::internal' namespace in the source file. This reduces the number of
redundant Eigen::LevenbergMarquardt instantiations from 18+ to 1,
significantly improving build times while preserving runtime performance
by keeping hot-path helpers inline in the header.

Author: AlrIsmail

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

@@ -11,9 +11,10 @@
 
 #include <limits>
 
-#include <unsupported/Eigen/NonLinearOptimization> // for LevenbergMarquardt
 #include <pcl/sample_consensus/sac_model_ellipse3d.h>
 #include <pcl/common/concatenate.h>
+#include <pcl/common/io.h>
+#include <pcl/common/copy_point.h>
 
 #include <Eigen/Eigenvalues>
 
@@ -30,9 +31,9 @@ pcl::SampleConsensusModelEllipse3D<PointT>::isSampleGood (
   }
 
   // Use three points out of the 6 samples
-  const Eigen::Vector3d p0 ((*input_)[samples[0]].x, (*input_)[samples[0]].y, (*input_)[samples[0]].z);
-  const Eigen::Vector3d p1 ((*input_)[samples[1]].x, (*input_)[samples[1]].y, (*input_)[samples[1]].z);
-  const Eigen::Vector3d p2 ((*input_)[samples[2]].x, (*input_)[samples[2]].y, (*input_)[samples[2]].z);
+  const Eigen::Vector3f p0 = (*input_)[samples[0]].getVector3fMap();
+  const Eigen::Vector3f p1 = (*input_)[samples[1]].getVector3fMap();
+  const Eigen::Vector3f p2 = (*input_)[samples[2]].getVector3fMap();
 
   // Check if the squared norm of the cross-product is non-zero, otherwise
   // common_helper_vec, which plays an important role in computeModelCoefficients,
@@ -225,7 +226,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::getEllipsePoint(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();
@@ -298,7 +299,7 @@ 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();
   }
@@ -357,7 +358,7 @@ 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)
@@ -416,7 +417,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++;
@@ -447,40 +448,14 @@ pcl::SampleConsensusModelEllipse3D<PointT>::optimizeModelCoefficients (
     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 ());
+  for (std::size_t i = 0; i < inliers.size (); ++i)
+  {
+    const auto& pt = (*input_)[inliers[i]];
+    x[i] = pt.x; y[i] = pt.y; z[i] = pt.z;
+  }
+
+  internal::optimizeModelCoefficientsEllipse3D (x, y, z, model_coefficients, optimized_coefficients);
 }
 
 //////////////////////////////////////////////////////////////////////////
@@ -507,13 +482,7 @@ pcl::SampleConsensusModelEllipse3D<PointT>::projectPoints (
     projected_points.width    = input_->width;
     projected_points.height   = input_->height;
 
-    using FieldList = typename pcl::traits::fieldList<PointT>::type;
-    // Iterate over each point
-    for (std::size_t i = 0; i < projected_points.size(); ++i)
-    {
-      // Iterate over each dimension
-      pcl::for_each_type<FieldList>(NdConcatenateFunctor<PointT, PointT>((*input_)[i], projected_points[i]));
-    }
+    pcl::copyPointCloud (*input_, projected_points);
 
     // c : Ellipse Center
     const Eigen::Vector3f c(model_coefficients[0], model_coefficients[1], model_coefficients[2]);
@@ -540,7 +509,7 @@ pcl::SampleConsensusModelEllipse3D<PointT>::projectPoints (
     for (std::size_t i = 0; i < inliers.size (); ++i)
     {
       // p : Sample Point
-      const Eigen::Vector3f p((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z);
+      const Eigen::Vector3f p = (*input_)[inliers[i]].getVector3fMap();
 
       // Local coordinates of sample point p
       const Eigen::Vector3f p_ = Rot_T * (p - c);
@@ -551,17 +520,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::getEllipsePoint(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[inliers[i]].x = static_cast<float> (k[0]);
+      projected_points[inliers[i]].y = static_cast<float> (k[1]);
+      projected_points[inliers[i]].z = static_cast<float> (k[2]);
     }
   }
   else
@@ -571,11 +540,7 @@ pcl::SampleConsensusModelEllipse3D<PointT>::projectPoints (
     projected_points.width    = inliers.size ();
     projected_points.height   = 1;
 
-    using FieldList = typename pcl::traits::fieldList<PointT>::type;
-    // Iterate over each point
-    for (std::size_t i = 0; i < inliers.size (); ++i)
-      // Iterate over each dimension
-      pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> ((*input_)[inliers[i]], projected_points[i]));
+    pcl::copyPointCloud (*input_, inliers, projected_points);
 
     // c : Ellipse Center
     const Eigen::Vector3f c(model_coefficients[0], model_coefficients[1], model_coefficients[2]);
@@ -602,7 +567,7 @@ pcl::SampleConsensusModelEllipse3D<PointT>::projectPoints (
     for (std::size_t i = 0; i < inliers.size (); ++i)
     {
       // p : Sample Point
-      const Eigen::Vector3f p((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z);
+      const Eigen::Vector3f p = projected_points[i].getVector3fMap();
 
       // Local coordinates of sample point p
       const Eigen::Vector3f p_ = Rot_T * (p - c);
@@ -613,12 +578,11 @@ 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::getEllipsePoint(params, th_opt, k_[0], k_[1]);
 
       const Eigen::Vector3f k = c + Rot * k_;
 
@@ -679,7 +643,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);
@@ -711,151 +675,4 @@ pcl::SampleConsensusModelEllipse3D<PointT>::isModelValid (const Eigen::VectorXf
 }
 
 
-
-//////////////////////////////////////////////////////////////////////////
-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>;