From aab340f39390eb4fb6adbdac987ef23fca535101 Mon Sep 17 00:00:00 2001 From: Markus Vieth <39675748+mvieth@users.noreply.github.com> Date: Sat, 2 Sep 2023 07:53:35 +0200 Subject: [PATCH] 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 --- .../sample_consensus/impl/sac_model_cone.hpp | 21 +++++--- .../impl/sac_model_cylinder.hpp | 21 +++++--- .../impl/sac_model_sphere.hpp | 21 +++++--- .../pcl/sample_consensus/sac_model_cone.h | 52 ++----------------- .../pcl/sample_consensus/sac_model_cylinder.h | 40 ++------------ .../pcl/sample_consensus/sac_model_sphere.h | 38 ++------------ sample_consensus/src/sac_model_cone.cpp | 50 ++++++++++++++++++ sample_consensus/src/sac_model_cylinder.cpp | 49 +++++++++++++++++ sample_consensus/src/sac_model_sphere.cpp | 40 ++++++++++++++ 9 files changed, 190 insertions(+), 142 deletions(-) diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_cone.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_cone.hpp index 1003cd96861..2f7118e7058 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_cone.hpp +++ b/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 // for LevenbergMarquardt #include #include // for getAngle3D #include @@ -344,14 +343,20 @@ pcl::SampleConsensusModelCone::optimizeModelCoefficients ( return; } - OptimizationFunctor functor (this, inliers); - Eigen::NumericalDiff num_diff (functor); - Eigen::LevenbergMarquardt, 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]); diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_cylinder.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_cylinder.hpp index 284e95c447b..b4caa68609b 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_cylinder.hpp +++ b/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 // for LevenbergMarquardt #include #include // for getAngle3D #include @@ -299,14 +298,20 @@ pcl::SampleConsensusModelCylinder::optimizeModelCoefficients ( return; } - OptimizationFunctor functor (this, inliers); - Eigen::NumericalDiff num_diff (functor); - Eigen::LevenbergMarquardt, 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]); diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_sphere.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_sphere.hpp index 6834afb601e..2ff3e55210f 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_sphere.hpp +++ b/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 // for LevenbergMarquardt #include ////////////////////////////////////////////////////////////////////////// @@ -354,14 +353,20 @@ pcl::SampleConsensusModelSphere::optimizeModelCoefficients ( return; } - OptimizationFunctor functor (this, inliers); - Eigen::NumericalDiff num_diff (functor); - Eigen::LevenbergMarquardt, 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]); } ////////////////////////////////////////////////////////////////////////// diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_cone.h b/sample_consensus/include/pcl/sample_consensus/sac_model_cone.h index d96a0d764b8..6137c26126c 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_cone.h +++ b/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: *
    @@ -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 - { - /** Functor constructor - * \param[in] indices the indices of data points to evaluate - * \param[in] estimator pointer to the estimator object - */ - OptimizationFunctor (const pcl::SampleConsensusModelCone *model, const Indices& indices) : - pcl::Functor (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 (pcl::sqrPointToLineDistance (pt, apex, axis_dir) - actual_cone_radius * actual_cone_radius); - } - return (0); - } - - const pcl::SampleConsensusModelCone *model_; - const Indices &indices_; - }; }; } diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_cylinder.h b/sample_consensus/include/pcl/sample_consensus/sac_model_cylinder.h index 7a45f26ff66..95a6e80873b 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_cylinder.h +++ b/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 - { - /** Functor constructor - * \param[in] indices the indices of data points to evaluate - * \param[in] estimator pointer to the estimator object - */ - OptimizationFunctor (const pcl::SampleConsensusModelCylinder *model, const Indices& indices) : - pcl::Functor (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 (pcl::sqrPointToLineDistance (pt, line_pt, line_dir) - x[6]*x[6]); - } - return (0); - } - - const pcl::SampleConsensusModelCylinder *model_; - const Indices &indices_; - }; }; } diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_sphere.h b/sample_consensus/include/pcl/sample_consensus/sac_model_sphere.h index 44dca3591af..1e15149953c 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_sphere.h +++ b/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 - { - /** Functor constructor - * \param[in] indices the indices of data points to evaluate - * \param[in] estimator pointer to the estimator object - */ - OptimizationFunctor (const pcl::SampleConsensusModelSphere *model, const Indices& indices) : - pcl::Functor (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 *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 diff --git a/sample_consensus/src/sac_model_cone.cpp b/sample_consensus/src/sac_model_cone.cpp index a321c33bbc0..e2db0c33731 100644 --- a/sample_consensus/src/sac_model_cone.cpp +++ b/sample_consensus/src/sac_model_cone.cpp @@ -37,6 +37,56 @@ */ #include +#include // 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 + { + ConeOptimizationFunctor (const Eigen::ArrayXf& x, const Eigen::ArrayXf& y, const Eigen::ArrayXf& z) : + pcl::Functor(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 num_diff (functor); + Eigen::LevenbergMarquardt, 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 diff --git a/sample_consensus/src/sac_model_cylinder.cpp b/sample_consensus/src/sac_model_cylinder.cpp index 54929fab8cb..aae33d91c7e 100644 --- a/sample_consensus/src/sac_model_cylinder.cpp +++ b/sample_consensus/src/sac_model_cylinder.cpp @@ -37,6 +37,55 @@ */ #include +#include // 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 + { + CylinderOptimizationFunctor (const Eigen::ArrayXf& x, const Eigen::ArrayXf& y, const Eigen::ArrayXf& z) : + pcl::Functor(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 num_diff (functor); + Eigen::LevenbergMarquardt, 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 diff --git a/sample_consensus/src/sac_model_sphere.cpp b/sample_consensus/src/sac_model_sphere.cpp index 03d84652715..27fbf325760 100644 --- a/sample_consensus/src/sac_model_sphere.cpp +++ b/sample_consensus/src/sac_model_sphere.cpp @@ -37,6 +37,46 @@ */ #include +#include // for LevenbergMarquardt + +int pcl::internal::optimizeModelCoefficientsSphere (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::optimizeModelCoefficientsSphere] Sizes not equal!\n"); + return Eigen::LevenbergMarquardtSpace::ImproperInputParameters; + } + if(coeff.size() != 4) { + PCL_ERROR("[pcl::internal::optimizeModelCoefficientsSphere] Coefficients have wrong size\n"); + return Eigen::LevenbergMarquardtSpace::ImproperInputParameters; + } + struct SphereOptimizationFunctor : pcl::Functor + { + SphereOptimizationFunctor (const Eigen::ArrayXf& x, const Eigen::ArrayXf& y, const Eigen::ArrayXf& z) : + pcl::Functor(x.size()), pts_x(x), pts_y(y), pts_z(z) + {} + + int + operator() (const Eigen::VectorXf &x, Eigen::VectorXf &fvec) const + { + // Compute distance of all points to center, then subtract radius + fvec = ((Eigen::ArrayXf::Constant(pts_x.size(), x[0])-pts_x).square() + +(Eigen::ArrayXf::Constant(pts_x.size(), x[1])-pts_y).square() + +(Eigen::ArrayXf::Constant(pts_x.size(), x[2])-pts_z).square()).sqrt() + -Eigen::ArrayXf::Constant(pts_x.size(), x[3]); + return (0); + } + + const Eigen::ArrayXf& pts_x, pts_y, pts_z; + }; + + SphereOptimizationFunctor functor (pts_x, pts_y, pts_z); + Eigen::NumericalDiff num_diff (functor); + Eigen::LevenbergMarquardt, float> lm (num_diff); + const int info = lm.minimize (coeff); + PCL_DEBUG ("[pcl::internal::optimizeModelCoefficientsSphere] 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