Improve optimizeModelCoefficients for cone, cylinder, sphere models (#5790)

* Improve optimizeModelCoefficients for cone,cylinder,sphere models
This has two effects: making the functions faster and reducing PCL's compile time. The function that is optimized is exactly the same as before.
In my tests, optimizeModelCoefficients is now at least twice as fast, often even three times as fast as before. This is because the x, y, and z values are perfectly arranged for SSE and AVX instructions.
The time needed to compile PCL is also reduced: instantiating Eigen::LevenbergMarquardt takes a rather long time, and previously this was done for each point type (sphere model) or even each point type-normal type combination (cone and cylinder model). Now, Eigen::LevenbergMarquardt is only instantiated once for all cone models, once for all cylinder models, and once for all sphere models. In my tests, this leads to a compile time reduction of roughly 1 minute (building the sample consensus library took 14m35s before and 13m31s now, one compile job, PCL_ONLY_CORE_POINT_TYPES=OFF).

* Add more const

Author: mvieth

sample_consensus/include/pcl/sample_consensus/impl/sac_model_cone.hpp

@@ -39,7 +39,6 @@
 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CONE_H_
 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CONE_H_
 
-#include <unsupported/Eigen/NonLinearOptimization> // for LevenbergMarquardt
 #include <pcl/sample_consensus/sac_model_cone.h>
 #include <pcl/common/common.h> // for getAngle3D
 #include <pcl/common/concatenate.h>
@@ -344,14 +343,20 @@ pcl::SampleConsensusModelCone<PointT, PointNT>::optimizeModelCoefficients (
     return;
   }
 
-  OptimizationFunctor functor (this, inliers);
-  Eigen::NumericalDiff<OptimizationFunctor > num_diff (functor);
-  Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, float> lm (num_diff);
-  int info = lm.minimize (optimized_coefficients);
+  Eigen::ArrayXf pts_x(inliers.size());
+  Eigen::ArrayXf pts_y(inliers.size());
+  Eigen::ArrayXf pts_z(inliers.size());
+  std::size_t pos = 0;
+  for(const auto& index : inliers) {
+    pts_x[pos] = (*input_)[index].x;
+    pts_y[pos] = (*input_)[index].y;
+    pts_z[pos] = (*input_)[index].z;
+    ++pos;
+  }
+  pcl::internal::optimizeModelCoefficientsCone(optimized_coefficients, pts_x, pts_y, pts_z);
 
-  // Compute the L2 norm of the residuals
-  PCL_DEBUG ("[pcl::SampleConsensusModelCone::optimizeModelCoefficients] LM solver finished with exit code %i, having a residual norm of %g. \nInitial solution: %g %g %g %g %g %g %g \nFinal solution: %g %g %g %g %g %g %g\n",
-             info, lm.fvec.norm (), model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3],
+  PCL_DEBUG ("[pcl::SampleConsensusModelCone::optimizeModelCoefficients] Initial solution: %g %g %g %g %g %g %g \nFinal solution: %g %g %g %g %g %g %g\n",
+             model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3],
              model_coefficients[4], model_coefficients[5], model_coefficients[6], optimized_coefficients[0], optimized_coefficients[1], optimized_coefficients[2], optimized_coefficients[3], optimized_coefficients[4], optimized_coefficients[5], optimized_coefficients[6]);
 
   Eigen::Vector3f line_dir (optimized_coefficients[3], optimized_coefficients[4], optimized_coefficients[5]);

sample_consensus/include/pcl/sample_consensus/impl/sac_model_cylinder.hpp

@@ -41,7 +41,6 @@
 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CYLINDER_H_
 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CYLINDER_H_
 
-#include <unsupported/Eigen/NonLinearOptimization> // for LevenbergMarquardt
 #include <pcl/sample_consensus/sac_model_cylinder.h>
 #include <pcl/common/common.h> // for getAngle3D
 #include <pcl/common/concatenate.h>
@@ -299,14 +298,20 @@ pcl::SampleConsensusModelCylinder<PointT, PointNT>::optimizeModelCoefficients (
     return;
   }
 
-  OptimizationFunctor functor (this, inliers);
-  Eigen::NumericalDiff<OptimizationFunctor > num_diff (functor);
-  Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, float> lm (num_diff);
-  int info = lm.minimize (optimized_coefficients);
+  Eigen::ArrayXf pts_x(inliers.size());
+  Eigen::ArrayXf pts_y(inliers.size());
+  Eigen::ArrayXf pts_z(inliers.size());
+  std::size_t pos = 0;
+  for(const auto& index : inliers) {
+    pts_x[pos] = (*input_)[index].x;
+    pts_y[pos] = (*input_)[index].y;
+    pts_z[pos] = (*input_)[index].z;
+    ++pos;
+  }
+  pcl::internal::optimizeModelCoefficientsCylinder(optimized_coefficients, pts_x, pts_y, pts_z);
 
-  // Compute the L2 norm of the residuals
-  PCL_DEBUG ("[pcl::SampleConsensusModelCylinder::optimizeModelCoefficients] LM solver finished with exit code %i, having a residual norm of %g. \nInitial solution: %g %g %g %g %g %g %g \nFinal solution: %g %g %g %g %g %g %g\n",
-             info, lm.fvec.norm (), model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3],
+  PCL_DEBUG ("[pcl::SampleConsensusModelCylinder::optimizeModelCoefficients] Initial solution: %g %g %g %g %g %g %g \nFinal solution: %g %g %g %g %g %g %g\n",
+             model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3],
              model_coefficients[4], model_coefficients[5], model_coefficients[6], optimized_coefficients[0], optimized_coefficients[1], optimized_coefficients[2], optimized_coefficients[3], optimized_coefficients[4], optimized_coefficients[5], optimized_coefficients[6]);
 
   Eigen::Vector3f line_dir (optimized_coefficients[3], optimized_coefficients[4], optimized_coefficients[5]);

sample_consensus/include/pcl/sample_consensus/impl/sac_model_sphere.hpp

@@ -41,7 +41,6 @@
 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_SPHERE_H_
 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_SPHERE_H_
 
-#include <unsupported/Eigen/NonLinearOptimization> // for LevenbergMarquardt
 #include <pcl/sample_consensus/sac_model_sphere.h>
 
 //////////////////////////////////////////////////////////////////////////
@@ -354,14 +353,20 @@ pcl::SampleConsensusModelSphere<PointT>::optimizeModelCoefficients (
     return;
   }
 
-  OptimizationFunctor functor (this, inliers);
-  Eigen::NumericalDiff<OptimizationFunctor> num_diff (functor);
-  Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, float> lm (num_diff);
-  int info = lm.minimize (optimized_coefficients);
+  Eigen::ArrayXf pts_x(inliers.size());
+  Eigen::ArrayXf pts_y(inliers.size());
+  Eigen::ArrayXf pts_z(inliers.size());
+  std::size_t pos = 0;
+  for(const auto& index : inliers) {
+    pts_x[pos] = (*input_)[index].x;
+    pts_y[pos] = (*input_)[index].y;
+    pts_z[pos] = (*input_)[index].z;
+    ++pos;
+  }
+  pcl::internal::optimizeModelCoefficientsSphere(optimized_coefficients, pts_x, pts_y, pts_z);
 
-  // Compute the L2 norm of the residuals
-  PCL_DEBUG ("[pcl::SampleConsensusModelSphere::optimizeModelCoefficients] LM solver finished with exit code %i, having a residual norm of %g. \nInitial solution: %g %g %g %g \nFinal solution: %g %g %g %g\n",
-             info, lm.fvec.norm (), model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3], optimized_coefficients[0], optimized_coefficients[1], optimized_coefficients[2], optimized_coefficients[3]);
+  PCL_DEBUG ("[pcl::SampleConsensusModelSphere::optimizeModelCoefficients] Initial solution: %g %g %g %g \nFinal solution: %g %g %g %g\n",
+             model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3], optimized_coefficients[0], optimized_coefficients[1], optimized_coefficients[2], optimized_coefficients[3]);
 }
 
 //////////////////////////////////////////////////////////////////////////

sample_consensus/include/pcl/sample_consensus/sac_model_cone.h

@@ -44,6 +44,10 @@
 
 namespace pcl
 {
+  namespace internal {
+    int optimizeModelCoefficientsCone (Eigen::VectorXf& coeff, const Eigen::ArrayXf& pts_x, const Eigen::ArrayXf& pts_y, const Eigen::ArrayXf& pts_z);
+  } // namespace internal
+
   /** \brief @b SampleConsensusModelCone defines a model for 3D cone segmentation.
     * The model coefficients are defined as:
     * <ul>
@@ -299,54 +303,6 @@ namespace pcl
       /** \brief The minimum and maximum allowed opening angles of valid cone model. */
       double min_angle_;
       double max_angle_;
-
-      /** \brief Functor for the optimization function */
-      struct OptimizationFunctor : pcl::Functor<float>
-      {
-        /** Functor constructor
-          * \param[in] indices the indices of data points to evaluate
-          * \param[in] estimator pointer to the estimator object
-          */
-        OptimizationFunctor (const pcl::SampleConsensusModelCone<PointT, PointNT> *model, const Indices& indices) :
-          pcl::Functor<float> (indices.size ()), model_ (model), indices_ (indices) {}
-
-        /** Cost function to be minimized
-          * \param[in] x variables array
-          * \param[out] fvec resultant functions evaluations
-          * \return 0
-          */
-        int 
-        operator() (const Eigen::VectorXf &x, Eigen::VectorXf &fvec) const
-        {
-          Eigen::Vector4f apex  (x[0], x[1], x[2], 0);
-          Eigen::Vector4f axis_dir (x[3], x[4], x[5], 0);
-          float opening_angle = x[6];
-
-          float apexdotdir = apex.dot (axis_dir);
-          float dirdotdir = 1.0f / axis_dir.dot (axis_dir);
-
-          for (int i = 0; i < values (); ++i)
-          {
-            // dist = f - r
-            Eigen::Vector4f pt = (*model_->input_)[indices_[i]].getVector4fMap();
-            pt[3] = 0;
-
-            // Calculate the point's projection on the cone axis
-            float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
-            Eigen::Vector4f pt_proj = apex + k * axis_dir;
-
-            // Calculate the actual radius of the cone at the level of the projected point
-            Eigen::Vector4f height = apex-pt_proj;
-            float actual_cone_radius = tanf (opening_angle) * height.norm ();
-
-            fvec[i] = static_cast<float> (pcl::sqrPointToLineDistance (pt, apex, axis_dir) - actual_cone_radius * actual_cone_radius);
-          }
-          return (0);
-        }
-
-        const pcl::SampleConsensusModelCone<PointT, PointNT> *model_;
-        const Indices &indices_;
-      };
   };
 }
 

sample_consensus/include/pcl/sample_consensus/sac_model_cylinder.h

@@ -46,6 +46,10 @@
 
 namespace pcl
 {
+  namespace internal {
+    int optimizeModelCoefficientsCylinder (Eigen::VectorXf& coeff, const Eigen::ArrayXf& pts_x, const Eigen::ArrayXf& pts_y, const Eigen::ArrayXf& pts_z);
+  } // namespace internal
+
   /** \brief @b SampleConsensusModelCylinder defines a model for 3D cylinder segmentation.
     * The model coefficients are defined as:
     *   - \b point_on_axis.x  : the X coordinate of a point located on the cylinder axis
@@ -295,42 +299,6 @@ namespace pcl
 
       /** \brief The maximum allowed difference between the cylinder direction and the given axis. */
       double eps_angle_;
-
-      /** \brief Functor for the optimization function */
-      struct OptimizationFunctor : pcl::Functor<float>
-      {
-        /** Functor constructor
-          * \param[in] indices the indices of data points to evaluate
-          * \param[in] estimator pointer to the estimator object
-          */
-        OptimizationFunctor (const pcl::SampleConsensusModelCylinder<PointT, PointNT> *model, const Indices& indices) :
-          pcl::Functor<float> (indices.size ()), model_ (model), indices_ (indices) {}
-
-        /** Cost function to be minimized
-          * \param[in] x variables array
-          * \param[out] fvec resultant functions evaluations
-          * \return 0
-          */
-        int 
-        operator() (const Eigen::VectorXf &x, Eigen::VectorXf &fvec) const
-        {
-          Eigen::Vector4f line_pt  (x[0], x[1], x[2], 0);
-          Eigen::Vector4f line_dir (x[3], x[4], x[5], 0);
-          
-          for (int i = 0; i < values (); ++i)
-          {
-            // dist = f - r
-            Eigen::Vector4f pt = (*model_->input_)[indices_[i]].getVector4fMap();
-            pt[3] = 0;
-
-            fvec[i] = static_cast<float> (pcl::sqrPointToLineDistance (pt, line_pt, line_dir) - x[6]*x[6]);
-          }
-          return (0);
-        }
-
-        const pcl::SampleConsensusModelCylinder<PointT, PointNT> *model_;
-        const Indices &indices_;
-      };
   };
 }
 

sample_consensus/include/pcl/sample_consensus/sac_model_sphere.h

@@ -52,6 +52,10 @@
 
 namespace pcl
 {
+  namespace internal {
+    int optimizeModelCoefficientsSphere (Eigen::VectorXf& coeff, const Eigen::ArrayXf& pts_x, const Eigen::ArrayXf& pts_y, const Eigen::ArrayXf& pts_z);
+  } // namespace internal
+
   /** \brief SampleConsensusModelSphere defines a model for 3D sphere segmentation.
     * The model coefficients are defined as:
     *   - \b center.x : the X coordinate of the sphere's center
@@ -263,40 +267,6 @@ namespace pcl
 #endif
 
     private:
-      struct OptimizationFunctor : pcl::Functor<float>
-      {
-        /** Functor constructor
-          * \param[in] indices the indices of data points to evaluate
-          * \param[in] estimator pointer to the estimator object
-          */
-        OptimizationFunctor (const pcl::SampleConsensusModelSphere<PointT> *model, const Indices& indices) :
-          pcl::Functor<float> (indices.size ()), model_ (model), indices_ (indices) {}
-
-        /** Cost function to be minimized
-          * \param[in] x the variables array
-          * \param[out] fvec the resultant functions evaluations
-          * \return 0
-          */
-        int 
-        operator() (const Eigen::VectorXf &x, Eigen::VectorXf &fvec) const
-        {
-          Eigen::Vector4f cen_t;
-          cen_t[3] = 0;
-          for (int i = 0; i < values (); ++i)
-          {
-            // Compute the difference between the center of the sphere and the datapoint X_i
-            cen_t.head<3>() = (*model_->input_)[indices_[i]].getVector3fMap() - x.head<3>();
-
-            // g = sqrt ((x-a)^2 + (y-b)^2 + (z-c)^2) - R
-            fvec[i] = std::sqrt (cen_t.dot (cen_t)) - x[3];
-          }
-          return (0);
-        }
-
-        const pcl::SampleConsensusModelSphere<PointT> *model_;
-        const Indices &indices_;
-      };
-
 #ifdef __AVX__
       inline __m256 sqr_dist8 (const std::size_t i, const __m256 a_vec, const __m256 b_vec, const __m256 c_vec) const;
 #endif

sample_consensus/src/sac_model_cone.cpp

@@ -37,6 +37,56 @@
  */
 
 #include <pcl/sample_consensus/impl/sac_model_cone.hpp>
+#include <unsupported/Eigen/NonLinearOptimization> // for LevenbergMarquardt
+
+int pcl::internal::optimizeModelCoefficientsCone (Eigen::VectorXf& coeff, const Eigen::ArrayXf& pts_x, const Eigen::ArrayXf& pts_y, const Eigen::ArrayXf& pts_z)
+{
+  if(pts_x.size() != pts_y.size() || pts_y.size() != pts_z.size()) {
+    PCL_ERROR("[pcl::internal::optimizeModelCoefficientsCone] Sizes not equal!\n");
+    return Eigen::LevenbergMarquardtSpace::ImproperInputParameters;
+  }
+  if(coeff.size() != 7) {
+    PCL_ERROR("[pcl::internal::optimizeModelCoefficientsCone] Coefficients have wrong size\n");
+    return Eigen::LevenbergMarquardtSpace::ImproperInputParameters;
+  }
+  struct ConeOptimizationFunctor : pcl::Functor<float>
+  {
+    ConeOptimizationFunctor (const Eigen::ArrayXf& x, const Eigen::ArrayXf& y, const Eigen::ArrayXf& z) :
+      pcl::Functor<float>(x.size()), pts_x(x), pts_y(y), pts_z(z)
+      {}
+
+    int
+    operator() (const Eigen::VectorXf &x, Eigen::VectorXf &fvec) const
+    {
+      Eigen::Vector3f axis_dir(x[3], x[4], x[5]);
+      axis_dir.normalize();
+      const Eigen::ArrayXf axis_dir_x = Eigen::ArrayXf::Constant(pts_x.size(), axis_dir.x());
+      const Eigen::ArrayXf axis_dir_y = Eigen::ArrayXf::Constant(pts_x.size(), axis_dir.y());
+      const Eigen::ArrayXf axis_dir_z = Eigen::ArrayXf::Constant(pts_x.size(), axis_dir.z());
+      const Eigen::ArrayXf bx = Eigen::ArrayXf::Constant(pts_x.size(), x[0]) - pts_x;
+      const Eigen::ArrayXf by = Eigen::ArrayXf::Constant(pts_x.size(), x[1]) - pts_y;
+      const Eigen::ArrayXf bz = Eigen::ArrayXf::Constant(pts_x.size(), x[2]) - pts_z;
+      const Eigen::ArrayXf actual_cone_radius = std::tan(x[6]) *
+          (bx*axis_dir_x+by*axis_dir_y+bz*axis_dir_z);
+      // compute the squared distance of point b to the line (cross product), then subtract the actual cone radius (squared)
+      fvec = ((axis_dir_y * bz - axis_dir_z * by).square()
+             +(axis_dir_z * bx - axis_dir_x * bz).square()
+             +(axis_dir_x * by - axis_dir_y * bx).square())
+             -actual_cone_radius.square();
+      return (0);
+    }
+
+    const Eigen::ArrayXf& pts_x, pts_y, pts_z;
+  };
+
+  ConeOptimizationFunctor functor (pts_x, pts_y, pts_z);
+  Eigen::NumericalDiff<ConeOptimizationFunctor> num_diff (functor);
+  Eigen::LevenbergMarquardt<Eigen::NumericalDiff<ConeOptimizationFunctor>, float> lm (num_diff);
+  const int info = lm.minimize (coeff);
+  PCL_DEBUG ("[pcl::internal::optimizeModelCoefficientsCone] LM solver finished with exit code %i, having a residual norm of %g.\n",
+             info, lm.fvec.norm ());
+  return info;
+}
 
 #ifndef PCL_NO_PRECOMPILE
 #include <pcl/impl/instantiate.hpp>

sample_consensus/src/sac_model_cylinder.cpp

@@ -37,6 +37,55 @@
  */
 
 #include <pcl/sample_consensus/impl/sac_model_cylinder.hpp>
+#include <unsupported/Eigen/NonLinearOptimization> // for LevenbergMarquardt
+
+int pcl::internal::optimizeModelCoefficientsCylinder (Eigen::VectorXf& coeff, const Eigen::ArrayXf& pts_x, const Eigen::ArrayXf& pts_y, const Eigen::ArrayXf& pts_z)
+{
+  if(pts_x.size() != pts_y.size() || pts_y.size() != pts_z.size()) {
+    PCL_ERROR("[pcl::internal::optimizeModelCoefficientsCylinder] Sizes not equal!\n");
+    return Eigen::LevenbergMarquardtSpace::ImproperInputParameters;
+  }
+  if(coeff.size() != 7) {
+    PCL_ERROR("[pcl::internal::optimizeModelCoefficientsCylinder] Coefficients have wrong size\n");
+    return Eigen::LevenbergMarquardtSpace::ImproperInputParameters;
+  }
+  struct CylinderOptimizationFunctor : pcl::Functor<float>
+  {
+    CylinderOptimizationFunctor (const Eigen::ArrayXf& x, const Eigen::ArrayXf& y, const Eigen::ArrayXf& z) :
+      pcl::Functor<float>(x.size()), pts_x(x), pts_y(y), pts_z(z)
+      {}
+
+    int
+    operator() (const Eigen::VectorXf &x, Eigen::VectorXf &fvec) const
+    {
+      Eigen::Vector3f line_dir(x[3], x[4], x[5]);
+      line_dir.normalize();
+      const Eigen::ArrayXf line_dir_x = Eigen::ArrayXf::Constant(pts_x.size(), line_dir.x());
+      const Eigen::ArrayXf line_dir_y = Eigen::ArrayXf::Constant(pts_x.size(), line_dir.y());
+      const Eigen::ArrayXf line_dir_z = Eigen::ArrayXf::Constant(pts_x.size(), line_dir.z());
+      const Eigen::ArrayXf bx = Eigen::ArrayXf::Constant(pts_x.size(), x[0]) - pts_x;
+      const Eigen::ArrayXf by = Eigen::ArrayXf::Constant(pts_x.size(), x[1]) - pts_y;
+      const Eigen::ArrayXf bz = Eigen::ArrayXf::Constant(pts_x.size(), x[2]) - pts_z;
+      // compute the squared distance of point b to the line (cross product), then subtract the squared model radius
+      fvec = ((line_dir_y * bz - line_dir_z * by).square()
+             +(line_dir_z * bx - line_dir_x * bz).square()
+             +(line_dir_x * by - line_dir_y * bx).square())
+             -Eigen::ArrayXf::Constant(pts_x.size(), x[6]*x[6]);
+      return (0);
+    }
+
+    const Eigen::ArrayXf& pts_x, pts_y, pts_z;
+  };
+
+  CylinderOptimizationFunctor functor (pts_x, pts_y, pts_z);
+  Eigen::NumericalDiff<CylinderOptimizationFunctor> num_diff (functor);
+  Eigen::LevenbergMarquardt<Eigen::NumericalDiff<CylinderOptimizationFunctor>, float> lm (num_diff);
+  const int info = lm.minimize (coeff);
+  coeff[6] = std::abs(coeff[6]);
+  PCL_DEBUG ("[pcl::internal::optimizeModelCoefficientsCylinder] LM solver finished with exit code %i, having a residual norm of %g.\n",
+             info, lm.fvec.norm ());
+  return info;
+}
 
 #ifndef PCL_NO_PRECOMPILE
 #include <pcl/impl/instantiate.hpp>