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