diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_ellipse3d.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_ellipse3d.hpp index 2110a2a85be..f1d4cf54981 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_ellipse3d.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_ellipse3d.hpp @@ -11,7 +11,6 @@ #include -#include // for LevenbergMarquardt #include #include @@ -225,7 +224,7 @@ pcl::SampleConsensusModelEllipse3D::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,18 @@ pcl::SampleConsensusModelEllipse3D::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_) { + // 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 + // 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 +299,9 @@ pcl::SampleConsensusModelEllipse3D::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 +344,10 @@ pcl::SampleConsensusModelEllipse3D::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 +358,13 @@ pcl::SampleConsensusModelEllipse3D::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 +403,10 @@ pcl::SampleConsensusModelEllipse3D::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 +417,7 @@ pcl::SampleConsensusModelEllipse3D::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 +444,29 @@ pcl::SampleConsensusModelEllipse3D::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 num_diff(functor); - Eigen::LevenbergMarquardt, double> lm(num_diff); - Eigen::VectorXd coeff = model_coefficients.cast(); - int info = lm.minimize(coeff); - optimized_coefficients = coeff.cast(); - - // 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 void pcl::SampleConsensusModelEllipse3D::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 +516,10 @@ pcl::SampleConsensusModelEllipse3D::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) + 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 +530,17 @@ pcl::SampleConsensusModelEllipse3D::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 (k[0]); - projected_points[i].y = static_cast (k[1]); - projected_points[i].z = static_cast (k[2]); + projected_points[inlier].x = static_cast (k[0]); + projected_points[inlier].y = static_cast (k[1]); + projected_points[inlier].z = static_cast (k[2]); } } else @@ -599,10 +578,11 @@ pcl::SampleConsensusModelEllipse3D::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 +593,17 @@ pcl::SampleConsensusModelEllipse3D::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(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 (k[0]); projected_points[i].y = static_cast (k[1]); - projected_points[i].z = static_cast (k[2]); + projected_points[i++].z = static_cast (k[2]); } } } @@ -679,7 +658,7 @@ pcl::SampleConsensusModelEllipse3D::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 +689,4 @@ pcl::SampleConsensusModelEllipse3D::isModelValid (const Eigen::VectorXf return (true); } - - -////////////////////////////////////////////////////////////////////////// -template -void inline pcl::SampleConsensusModelEllipse3D::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 -Eigen::Vector2f inline pcl::SampleConsensusModelEllipse3D::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 -float inline pcl::SampleConsensusModelEllipse3D::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; diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_ellipse3d.h b/sample_consensus/include/pcl/sample_consensus/sac_model_ellipse3d.h index 0b085869920..d56ed1b5fb6 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_ellipse3d.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_ellipse3d.h @@ -14,6 +14,143 @@ namespace pcl { + namespace internal + { + /** \brief Internal function to optimize ellipse coefficients. */ + PCL_EXPORTS int + optimizeModelCoefficientsEllipse3D (Eigen::VectorXf &coeff, + const Eigen::ArrayXf &pts_x, + const Eigen::ArrayXf &pts_y, + const Eigen::ArrayXf &pts_z); + + /** \brief Internal function to compute ellipse point from parametric coefficients and angle. + * \param[in] par the parametric coefficients (a, b, h, k, slant) + * \param[in] th the angle (in radians) + * \param[out] x the resultant X coordinate in local frame + * \param[out] y the resultant Y coordinate in local frame + */ + inline void + get_ellipse_point (const Eigen::VectorXf& par, float th, float& x, float& y) + { + 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); + } + + /** \brief Internal function to find the optimal angle using Golden Section Search. + * \param[in] par the ellipse coefficients (a, b, 0, 0, 0) + * \param[in] u point X coordinate in local frame + * \param[in] v point Y coordinate in local frame + * \param[in] th_min search interval lower bound + * \param[in] th_max search interval upper bound + * \param[in] epsilon search convergence tolerance + * \return the optimal angle (in radians) + */ + inline float + golden_section_search (const Eigen::VectorXf& par, float u, float v, float th_min, float th_max, float epsilon) + { + constexpr float phi (1.61803398874989484820f); + float tl (th_min), tu (th_max); + float ta = tl + (tu - tl) * (1.0f - 1.0f / phi); + float tb = tl + (tu - tl) * 1.0f / phi; + + while ((tu - tl) > epsilon) + { + float x_a (0.0f), y_a (0.0f); + 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); + + float x_b (0.0f), y_b (0.0f); + 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.0f - 1.0f / phi); + } + else if (squared_dist_ta > squared_dist_tb) + { + tl = ta; + ta = tb; + tb = tl + (tu - tl) * 1.0f / phi; + } + else + { + tl = ta; + tu = tb; + ta = tl + (tu - tl) * (1.0f - 1.0f / phi); + tb = tl + (tu - tl) * 1.0f / phi; + } + } + return (tl + tu) / 2.0f; + } + + /** \brief Internal function to compute the shortest distance vector from a point to an ellipse. + * \param[in] par the ellipse coefficients (a, b, 0, 0, 0) + * \param[in] u point X coordinate in local frame + * \param[in] v point Y coordinate in local frame + * \param[out] th_opt the resultant optimal angle on the ellipse + * \return the distance vector from the point to its projection on the ellipse + */ + inline Eigen::Vector2f + dvec2ellipse (const Eigen::VectorXf& par, float u, float v, float& th_opt) + { + 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; + + Eigen::Vector2f x_axis (0.0f, 0.0f); + get_ellipse_point (par, 0.0f, x_axis (0), x_axis (1)); + x_axis -= center; + + Eigen::Vector2f y_axis (0.0f, 0.0f); + get_ellipse_point (par, static_cast (M_PI / 2.0), y_axis (0), y_axis (1)); + y_axis -= center; + + const float x_proj = p.dot (x_axis) / x_axis.norm (); + const float y_proj = p.dot (y_axis) / y_axis.norm (); + + float th_min (0.0f), th_max (0.0f); + const float th = std::atan2 (y_proj, x_proj); + + if (th < -static_cast (M_PI / 2.0)) + { + th_min = -static_cast (M_PI); + th_max = -static_cast (M_PI / 2.0); + } + else if (th < 0.0f) + { + th_min = -static_cast (M_PI / 2.0); + th_max = 0.0f; + } + else if (th < static_cast (M_PI / 2.0)) + { + th_min = 0.0f; + th_max = static_cast (M_PI / 2.0); + } + else + { + th_min = static_cast (M_PI / 2.0); + th_max = static_cast (M_PI); + } + + th_opt = golden_section_search (par, u, v, th_min, th_max, 1.e-3f); + float x (0.0f), y (0.0f); + get_ellipse_point (par, th_opt, x, y); + return {u - x, v - y}; + } + } + /** \brief SampleConsensusModelEllipse3D defines a model for 3D ellipse segmentation. * * The model coefficients are defined as: @@ -52,8 +189,8 @@ namespace pcl using PointCloudPtr = typename SampleConsensusModel::PointCloudPtr; using PointCloudConstPtr = typename SampleConsensusModel::PointCloudConstPtr; - using Ptr = shared_ptr >; - using ConstPtr = shared_ptr >; + using Ptr = shared_ptr>; + using ConstPtr = shared_ptr>; /** \brief Constructor for base SampleConsensusModelEllipse3D. * \param[in] cloud the input point cloud dataset @@ -194,85 +331,6 @@ namespace pcl */ bool isSampleGood(const Indices &samples) const override; - - private: - /** \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::SampleConsensusModelEllipse3D *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::VectorXd &x, Eigen::VectorXd &fvec) const - { - // c : Ellipse Center - const Eigen::Vector3f c(x[0], x[1], x[2]); - // n : Ellipse (Plane) Normal - const Eigen::Vector3f n_axis(x[5], x[6], x[7]); - // x : Ellipse (Plane) X-Axis - const Eigen::Vector3f x_axis(x[8], x[9], x[10]); - // y : Ellipse (Plane) Y-Axis - const Eigen::Vector3f y_axis = n_axis.cross(x_axis).normalized(); - // a : Ellipse semi-major axis (X) length - const float par_a(x[3]); - // b : Ellipse semi-minor axis (Y) length - const float par_b(x[4]); - - // Compute the rotation matrix and its transpose - const Eigen::Matrix3f Rot = (Eigen::Matrix3f(3,3) - << x_axis(0), y_axis(0), n_axis(0), - x_axis(1), y_axis(1), n_axis(1), - x_axis(2), y_axis(2), n_axis(2)) - .finished(); - const Eigen::Matrix3f Rot_T = Rot.transpose(); - - for (int i = 0; i < values (); ++i) - { - // what i have: - // p : Sample Point - const Eigen::Vector3f p = (*model_->input_)[indices_[i]].getVector3fMap().template cast(); - - // Local coordinates of sample point p - const Eigen::Vector3f p_ = Rot_T * (p - c); - - // k : Point on Ellipse - // 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::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); - fvec[i] = distanceVector.norm(); - } - return (0); - } - - const pcl::SampleConsensusModelEllipse3D *model_; - const Indices &indices_; - }; - - static void - get_ellipse_point(const Eigen::VectorXf& par, float th, float& x, float& y); - - static Eigen::Vector2f - dvec2ellipse(const Eigen::VectorXf& par, float u, float v, float& th_opt); - - static float - golden_section_search( - const Eigen::VectorXf& par, - float u, - float v, - float th_min, - float th_max, - float epsilon); }; } diff --git a/sample_consensus/src/sac_model_ellipse3d.cpp b/sample_consensus/src/sac_model_ellipse3d.cpp index 4f47a8b0af9..09d0a67723c 100644 --- a/sample_consensus/src/sac_model_ellipse3d.cpp +++ b/sample_consensus/src/sac_model_ellipse3d.cpp @@ -8,10 +8,90 @@ */ #include +#include +#include + +namespace pcl +{ + namespace internal + { + int + optimizeModelCoefficientsEllipse3D (Eigen::VectorXf &coeff, const Eigen::ArrayXf &pts_x, const Eigen::ArrayXf &pts_y, const Eigen::ArrayXf &pts_z) + { + struct Ellipse3DOptimizationFunctor : pcl::Functor + { + Ellipse3DOptimizationFunctor (const Eigen::ArrayXf& x, const Eigen::ArrayXf& y, const Eigen::ArrayXf& z) : + pcl::Functor (static_cast(x.size ())), x_ (x), y_ (y), z_ (z) {} + + int operator() (const Eigen::VectorXd &x, Eigen::VectorXd &fvec) const + { + // c : Ellipse Center + const Eigen::Vector3f c (static_cast(x[0]), static_cast(x[1]), static_cast(x[2])); + // a : Ellipse semi-major axis (X) length + const float par_a (static_cast(x [3])); + // b : Ellipse semi-minor axis (Y) length + const float par_b (static_cast(x [4])); + // n : Ellipse (Plane) Normal + const Eigen::Vector3f n_axis = Eigen::Vector3f (static_cast(x[5]), static_cast(x[6]), static_cast(x[7])).normalized (); + // x : Ellipse (Plane) X-Axis + Eigen::Vector3f x_ax = Eigen::Vector3f (static_cast(x[8]), static_cast(x[9]), static_cast(x[10])); + x_ax = (x_ax - x_ax.dot (n_axis) * n_axis).normalized (); + // y : Ellipse (Plane) Y-Axis + const Eigen::Vector3f y_ax = n_axis.cross (x_ax).normalized (); + + // Compute the rotation matrix and its transpose + const Eigen::Matrix3f Rot = (Eigen::Matrix3f (3,3) + << x_ax (0), y_ax(0), n_axis(0), + x_ax (1), y_ax(1), n_axis(1), + x_ax (2), y_ax(2), n_axis(2)) + .finished (); + const Eigen::Matrix3f Rot_T = Rot.transpose (); + + const Eigen::VectorXf params = (Eigen::VectorXf (5) << par_a, par_b, 0.0f, 0.0f, 0.0f).finished (); + for (int i = 0; i < values (); ++i) + { + const Eigen::Vector3f p (x_ [i], y_ [i], z_ [i]); + const Eigen::Vector3f p_ = Rot_T * (p - c); + float th_opt; + // k : Point on Ellipse + // 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. + fvec [i] = static_cast(dvec2ellipse (params, p_ (0), p_ (1), th_opt).norm ()); + } + return (0); + } + + const Eigen::ArrayXf &x_, &y_, &z_; + }; + + Ellipse3DOptimizationFunctor functor (pts_x, pts_y, pts_z); + Eigen::NumericalDiff num_diff (functor); + Eigen::LevenbergMarquardt, double> lm (num_diff); + Eigen::VectorXd coeff_double = coeff.cast (); + const int info = lm.minimize (coeff_double); + coeff = coeff_double.cast (); + + const Eigen::Vector3f n_axis = Eigen::Vector3f (coeff[5], coeff[6], coeff[7]).normalized (); + coeff[5] = n_axis[0]; + coeff[6] = n_axis[1]; + coeff[7] = n_axis[2]; + + Eigen::Vector3f x_ax = Eigen::Vector3f (coeff[8], coeff[9], coeff[10]); + x_ax = (x_ax - x_ax.dot (n_axis) * n_axis).normalized (); + coeff[8] = x_ax[0]; + coeff[9] = x_ax[1]; + coeff[10] = x_ax[2]; + + return info; + } + } +} #ifndef PCL_NO_PRECOMPILE #include #include + // Instantiations of specific point types #ifdef PCL_ONLY_CORE_POINT_TYPES PCL_INSTANTIATE(SampleConsensusModelEllipse3D, (pcl::PointXYZ)(pcl::PointXYZI)(pcl::PointXYZRGBA)(pcl::PointXYZRGB)(pcl::PointXYZRGBNormal)) @@ -19,4 +99,3 @@ PCL_INSTANTIATE(SampleConsensusModelEllipse3D, PCL_XYZ_POINT_TYPES) #endif #endif // PCL_NO_PRECOMPILE -