14 #include <unsupported/Eigen/NonLinearOptimization>
15 #include <pcl/sample_consensus/sac_model_ellipse3d.h>
16 #include <pcl/common/concatenate.h>
18 #include <Eigen/Eigenvalues>
23 template <
typename Po
intT>
bool
27 if (samples.size () != sample_size_)
29 PCL_ERROR (
"[pcl::SampleConsensusModelEllipse3D::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n", samples.size (), sample_size_);
34 const Eigen::Vector3d p0 ((*input_)[samples[0]].x, (*input_)[samples[0]].y, (*input_)[samples[0]].z);
35 const Eigen::Vector3d p1 ((*input_)[samples[1]].x, (*input_)[samples[1]].y, (*input_)[samples[1]].z);
36 const Eigen::Vector3d p2 ((*input_)[samples[2]].x, (*input_)[samples[2]].y, (*input_)[samples[2]].z);
41 if ((p1 - p0).cross(p1 - p2).squaredNorm() < Eigen::NumTraits<float>::dummy_precision ())
43 PCL_ERROR (
"[pcl::SampleConsensusModelEllipse3D::isSampleGood] Sample points too similar or collinear!\n");
51 template <
typename Po
intT>
bool
55 if (samples.size () != sample_size_)
57 PCL_ERROR (
"[pcl::SampleConsensusModelEllipse3D::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
61 model_coefficients.resize (model_size_);
63 const Eigen::Vector3f p0((*input_)[samples[0]].x, (*input_)[samples[0]].y, (*input_)[samples[0]].z);
64 const Eigen::Vector3f p1((*input_)[samples[1]].x, (*input_)[samples[1]].y, (*input_)[samples[1]].z);
65 const Eigen::Vector3f p2((*input_)[samples[2]].x, (*input_)[samples[2]].y, (*input_)[samples[2]].z);
66 const Eigen::Vector3f p3((*input_)[samples[3]].x, (*input_)[samples[3]].y, (*input_)[samples[3]].z);
67 const Eigen::Vector3f p4((*input_)[samples[4]].x, (*input_)[samples[4]].y, (*input_)[samples[4]].z);
68 const Eigen::Vector3f p5((*input_)[samples[5]].x, (*input_)[samples[5]].y, (*input_)[samples[5]].z);
70 const Eigen::Vector3f common_helper_vec = (p1 - p0).cross(p1 - p2);
71 const Eigen::Vector3f ellipse_normal = common_helper_vec.normalized();
75 if (common_helper_vec.squaredNorm() < Eigen::NumTraits<float>::dummy_precision ())
77 PCL_ERROR (
"[pcl::SampleConsensusModelEllipse3D::computeModelCoefficients] Sample points too similar or collinear!\n");
82 Eigen::Vector3f x_axis = (p1 - p0).normalized();
83 const Eigen::Vector3f z_axis = ellipse_normal.normalized();
84 const Eigen::Vector3f y_axis = z_axis.cross(x_axis).normalized();
87 const Eigen::Matrix3f Rot = (Eigen::Matrix3f(3,3)
88 << x_axis(0), y_axis(0), z_axis(0),
89 x_axis(1), y_axis(1), z_axis(1),
90 x_axis(2), y_axis(2), z_axis(2))
92 const Eigen::Matrix3f Rot_T = Rot.transpose();
95 const Eigen::Vector3f p0_ = Rot_T * (p0 - p0);
96 const Eigen::Vector3f p1_ = Rot_T * (p1 - p0);
97 const Eigen::Vector3f p2_ = Rot_T * (p2 - p0);
98 const Eigen::Vector3f p3_ = Rot_T * (p3 - p0);
99 const Eigen::Vector3f p4_ = Rot_T * (p4 - p0);
100 const Eigen::Vector3f p5_ = Rot_T * (p5 - p0);
108 const Eigen::VectorXf X = (Eigen::VectorXf(6) << p0_(0), p1_(0), p2_(0), p3_(0), p4_(0), p5_(0)).finished();
109 const Eigen::VectorXf Y = (Eigen::VectorXf(6) << p0_(1), p1_(1), p2_(1), p3_(1), p4_(1), p5_(1)).finished();
112 const Eigen::MatrixXf D = (Eigen::MatrixXf(6,6)
113 << X(0) * X(0), X(0) * Y(0), Y(0) * Y(0), X(0), Y(0), 1.0,
114 X(1) * X(1), X(1) * Y(1), Y(1) * Y(1), X(1), Y(1), 1.0,
115 X(2) * X(2), X(2) * Y(2), Y(2) * Y(2), X(2), Y(2), 1.0,
116 X(3) * X(3), X(3) * Y(3), Y(3) * Y(3), X(3), Y(3), 1.0,
117 X(4) * X(4), X(4) * Y(4), Y(4) * Y(4), X(4), Y(4), 1.0,
118 X(5) * X(5), X(5) * Y(5), Y(5) * Y(5), X(5), Y(5), 1.0)
122 const Eigen::MatrixXf S = D.transpose() * D;
125 const Eigen::MatrixXf C = (Eigen::MatrixXf(6,6)
126 << 0.0, 0.0, -2.0, 0.0, 0.0, 0.0,
127 0.0, 1.0, 0.0, 0.0, 0.0, 0.0,
128 -2.0, 0.0, 0.0, 0.0, 0.0, 0.0,
129 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
130 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
131 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
135 Eigen::GeneralizedEigenSolver<Eigen::MatrixXf> solver;
136 solver.compute(S, C);
137 const Eigen::VectorXf eigvals = solver.eigenvalues().real();
142 for (
size_t i(0); i < static_cast<size_t>(eigvals.size()); ++i) {
143 if (eigvals(i) < absmin && !std::isinf(eigvals(i))) {
149 PCL_DEBUG(
"[pcl::SampleConsensusModelEllipse3D::computeModelCoefficients] Failed to find the negative eigenvalue in the GES.\n");
152 const Eigen::VectorXf neigvec = solver.eigenvectors().real().col(idx).normalized();
158 const float con_A(neigvec(0));
159 const float con_B(neigvec(1));
160 const float con_C(neigvec(2));
161 const float con_D(neigvec(3));
162 const float con_E(neigvec(4));
163 const float con_F(neigvec(5));
166 const Eigen::MatrixXf M0 = (Eigen::MatrixXf(3, 3)
167 << con_F, con_D/2.0, con_E/2.0,
168 con_D/2.0, con_A, con_B/2.0,
169 con_E/2.0, con_B/2.0, con_C)
173 const Eigen::MatrixXf M = (Eigen::MatrixXf(2, 2)
179 Eigen::EigenSolver<Eigen::MatrixXf> solver_M(M);
181 Eigen::VectorXf eigvals_M = solver_M.eigenvalues().real();
184 float aux_eigval(0.0);
185 if (std::abs(eigvals_M(0) - con_A) > std::abs(eigvals_M(0) - con_C)) {
186 aux_eigval = eigvals_M(0);
187 eigvals_M(0) = eigvals_M(1);
188 eigvals_M(1) = aux_eigval;
192 float par_a = std::sqrt(-M0.determinant() / (M.determinant() * eigvals_M(0)));
193 float par_b = std::sqrt(-M0.determinant() / (M.determinant() * eigvals_M(1)));
194 const float par_h = (con_B * con_E - 2.0 * con_C * con_D) / (4.0 * con_A * con_C - std::pow(con_B, 2));
195 const float par_k = (con_B * con_D - 2.0 * con_A * con_E) / (4.0 * con_A * con_C - std::pow(con_B, 2));
196 const float par_t = (
M_PI / 2.0 - std::atan((con_A - con_C) / con_B)) / 2.0;
200 Eigen::Vector3f p_ctr;
203 p_ctr = p0 + Rot * Eigen::Vector3f(par_h, par_k, 0.0);
208 p_ctr = p0 + Rot * Eigen::Vector3f(par_k, par_h, 0.0);
212 model_coefficients[0] =
static_cast<float>(p_ctr(0));
213 model_coefficients[1] =
static_cast<float>(p_ctr(1));
214 model_coefficients[2] =
static_cast<float>(p_ctr(2));
217 model_coefficients[3] =
static_cast<float>(par_a);
219 model_coefficients[4] =
static_cast<float>(par_b);
222 model_coefficients[5] =
static_cast<float>(ellipse_normal[0]);
223 model_coefficients[6] =
static_cast<float>(ellipse_normal[1]);
224 model_coefficients[7] =
static_cast<float>(ellipse_normal[2]);
227 const Eigen::VectorXf params = (Eigen::VectorXf(5) << par_a, par_b, par_h, par_k, par_t).finished();
228 Eigen::Vector3f p_th_(0.0, 0.0, 0.0);
229 get_ellipse_point(params, par_t, p_th_(0), p_th_(1));
232 x_axis = (Rot * p_th_).normalized();
233 model_coefficients[8] =
static_cast<float>(x_axis[0]);
234 model_coefficients[9] =
static_cast<float>(x_axis[1]);
235 model_coefficients[10] =
static_cast<float>(x_axis[2]);
238 PCL_DEBUG (
"[pcl::SampleConsensusModelEllipse3D::computeModelCoefficients] Model is (%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g,%g).\n",
239 model_coefficients[0], model_coefficients[1], model_coefficients[2], model_coefficients[3],
240 model_coefficients[4], model_coefficients[5], model_coefficients[6], model_coefficients[7],
241 model_coefficients[8], model_coefficients[9], model_coefficients[10]);
247 template <
typename Po
intT>
void
251 if (!isModelValid (model_coefficients))
256 distances.resize (indices_->size ());
259 const Eigen::Vector3f c(model_coefficients[0], model_coefficients[1], model_coefficients[2]);
261 const Eigen::Vector3f n_axis(model_coefficients[5], model_coefficients[6], model_coefficients[7]);
263 const Eigen::Vector3f x_axis(model_coefficients[8], model_coefficients[9], model_coefficients[10]);
265 const Eigen::Vector3f y_axis = n_axis.cross(x_axis).normalized();
267 const float par_a(model_coefficients[3]);
269 const float par_b(model_coefficients[4]);
272 const Eigen::Matrix3f Rot = (Eigen::Matrix3f(3,3)
273 << x_axis(0), y_axis(0), n_axis(0),
274 x_axis(1), y_axis(1), n_axis(1),
275 x_axis(2), y_axis(2), n_axis(2))
277 const Eigen::Matrix3f Rot_T = Rot.transpose();
280 const Eigen::VectorXf params = (Eigen::VectorXf(5) << par_a, par_b, 0.0, 0.0, 0.0).finished();
284 for (std::size_t i = 0; i < indices_->size (); ++i)
293 const Eigen::Vector3f p((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z);
296 const Eigen::Vector3f p_ = Rot_T * (p - c);
302 const Eigen::Vector2f distanceVector = dvec2ellipse(params, p_(0), p_(1), th_opt);
304 distances[i] = distanceVector.norm();
309 template <
typename Po
intT>
void
311 const Eigen::VectorXf &model_coefficients,
const double threshold,
316 if (!isModelValid (model_coefficients))
320 inliers.reserve (indices_->size ());
323 const Eigen::Vector3f c(model_coefficients[0], model_coefficients[1], model_coefficients[2]);
325 const Eigen::Vector3f n_axis(model_coefficients[5], model_coefficients[6], model_coefficients[7]);
327 const Eigen::Vector3f x_axis(model_coefficients[8], model_coefficients[9], model_coefficients[10]);
329 const Eigen::Vector3f y_axis = n_axis.cross(x_axis).normalized();
331 const float par_a(model_coefficients[3]);
333 const float par_b(model_coefficients[4]);
336 const Eigen::Matrix3f Rot = (Eigen::Matrix3f(3,3)
337 << x_axis(0), y_axis(0), n_axis(0),
338 x_axis(1), y_axis(1), n_axis(1),
339 x_axis(2), y_axis(2), n_axis(2))
341 const Eigen::Matrix3f Rot_T = Rot.transpose();
343 const auto squared_threshold = threshold * threshold;
345 for (std::size_t i = 0; i < indices_->size (); ++i)
348 const Eigen::Vector3f p((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z);
351 const Eigen::Vector3f p_ = Rot_T * (p - c);
357 const Eigen::VectorXf params = (Eigen::VectorXf(5) << par_a, par_b, 0.0, 0.0, 0.0).finished();
359 const Eigen::Vector2f distanceVector = dvec2ellipse(params, p_(0), p_(1), th_opt);
361 if (distanceVector.squaredNorm() < squared_threshold)
364 inliers.push_back ((*indices_)[i]);
370 template <
typename Po
intT> std::size_t
372 const Eigen::VectorXf &model_coefficients,
const double threshold)
const
375 if (!isModelValid (model_coefficients))
377 std::size_t nr_p = 0;
380 const Eigen::Vector3f c(model_coefficients[0], model_coefficients[1], model_coefficients[2]);
382 const Eigen::Vector3f n_axis(model_coefficients[5], model_coefficients[6], model_coefficients[7]);
384 const Eigen::Vector3f x_axis(model_coefficients[8], model_coefficients[9], model_coefficients[10]);
386 const Eigen::Vector3f y_axis = n_axis.cross(x_axis).normalized();
388 const float par_a(model_coefficients[3]);
390 const float par_b(model_coefficients[4]);
393 const Eigen::Matrix3f Rot = (Eigen::Matrix3f(3,3)
394 << x_axis(0), y_axis(0), n_axis(0),
395 x_axis(1), y_axis(1), n_axis(1),
396 x_axis(2), y_axis(2), n_axis(2))
398 const Eigen::Matrix3f Rot_T = Rot.transpose();
400 const auto squared_threshold = threshold * threshold;
402 for (std::size_t i = 0; i < indices_->size (); ++i)
405 const Eigen::Vector3f p((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z);
408 const Eigen::Vector3f p_ = Rot_T * (p - c);
414 const Eigen::VectorXf params = (Eigen::VectorXf(5) << par_a, par_b, 0.0, 0.0, 0.0).finished();
416 const Eigen::Vector2f distanceVector = dvec2ellipse(params, p_(0), p_(1), th_opt);
418 if (distanceVector.squaredNorm() < squared_threshold)
425 template <
typename Po
intT>
void
428 const Eigen::VectorXf &model_coefficients,
429 Eigen::VectorXf &optimized_coefficients)
const
431 optimized_coefficients = model_coefficients;
434 if (!isModelValid (model_coefficients))
436 PCL_ERROR (
"[pcl::SampleConsensusModelEllipse3D::optimizeModelCoefficients] Given model is invalid!\n");
441 if (inliers.size () <= sample_size_)
443 PCL_ERROR (
"[pcl::SampleConsensusModelEllipse3D::optimizeModelCoefficients] Not enough inliers to refine/optimize the model's coefficients (%lu)! Returning the same coefficients.\n", inliers.size ());
447 OptimizationFunctor functor(
this, inliers);
448 Eigen::NumericalDiff<OptimizationFunctor> num_diff(functor);
449 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>,
double> lm(num_diff);
450 Eigen::VectorXd coeff;
451 int info = lm.minimize(coeff);
452 for (Eigen::Index i = 0; i < coeff.size (); ++i)
453 optimized_coefficients[i] =
static_cast<float> (coeff[i]);
456 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 %g %g \nFinal solution: %g %g %g %g %g %g %g %g %g %g %g %g %g\n",
457 info, lm.fvec.norm (),
459 model_coefficients[0],
460 model_coefficients[1],
461 model_coefficients[2],
462 model_coefficients[3],
463 model_coefficients[4],
464 model_coefficients[5],
465 model_coefficients[6],
466 model_coefficients[7],
467 model_coefficients[8],
468 model_coefficients[9],
469 model_coefficients[10],
471 optimized_coefficients[0],
472 optimized_coefficients[1],
473 optimized_coefficients[2],
474 optimized_coefficients[3],
475 optimized_coefficients[4],
476 optimized_coefficients[5],
477 optimized_coefficients[6],
478 optimized_coefficients[7],
479 optimized_coefficients[8],
480 optimized_coefficients[9],
481 optimized_coefficients[10]);
485 template <
typename Po
intT>
void
487 const Indices &inliers,
const Eigen::VectorXf &model_coefficients,
488 PointCloud &projected_points,
bool copy_data_fields)
const
491 if (!isModelValid (model_coefficients))
493 PCL_ERROR (
"[pcl::SampleConsensusModelEllipse3D::projectPoints] Given model is invalid!\n");
497 projected_points.
header = input_->header;
498 projected_points.
is_dense = input_->is_dense;
501 if (copy_data_fields)
504 projected_points.
resize (input_->size ());
505 projected_points.
width = input_->width;
506 projected_points.
height = input_->height;
508 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
510 for (std::size_t i = 0; i < projected_points.
size(); ++i)
517 const Eigen::Vector3f c(model_coefficients[0], model_coefficients[1], model_coefficients[2]);
519 const Eigen::Vector3f n_axis(model_coefficients[5], model_coefficients[6], model_coefficients[7]);
521 const Eigen::Vector3f x_axis(model_coefficients[8], model_coefficients[9], model_coefficients[10]);
523 const Eigen::Vector3f y_axis = n_axis.cross(x_axis).normalized();
525 const float par_a(model_coefficients[3]);
527 const float par_b(model_coefficients[4]);
530 const Eigen::Matrix3f Rot = (Eigen::Matrix3f(3,3)
531 << x_axis(0), y_axis(0), n_axis(0),
532 x_axis(1), y_axis(1), n_axis(1),
533 x_axis(2), y_axis(2), n_axis(2))
535 const Eigen::Matrix3f Rot_T = Rot.transpose();
538 for (std::size_t i = 0; i < inliers.size (); ++i)
541 const Eigen::Vector3f p((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z);
544 const Eigen::Vector3f p_ = Rot_T * (p - c);
550 const Eigen::VectorXf params = (Eigen::VectorXf(5) << par_a, par_b, 0.0, 0.0, 0.0).finished();
552 dvec2ellipse(params, p_(0), p_(1), th_opt);
555 Eigen::Vector3f k_(0.0, 0.0, 0.0);
556 get_ellipse_point(params, th_opt, k_[0], k_[1]);
558 const Eigen::Vector3f k = c + Rot * k_;
560 projected_points[i].x =
static_cast<float> (k[0]);
561 projected_points[i].y =
static_cast<float> (k[1]);
562 projected_points[i].z =
static_cast<float> (k[2]);
568 projected_points.
resize (inliers.size ());
569 projected_points.
width = inliers.size ();
570 projected_points.
height = 1;
572 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
574 for (std::size_t i = 0; i < inliers.size (); ++i)
579 const Eigen::Vector3f c(model_coefficients[0], model_coefficients[1], model_coefficients[2]);
581 const Eigen::Vector3f n_axis(model_coefficients[5], model_coefficients[6], model_coefficients[7]);
583 const Eigen::Vector3f x_axis(model_coefficients[8], model_coefficients[9], model_coefficients[10]);
585 const Eigen::Vector3f y_axis = n_axis.cross(x_axis).normalized();
587 const float par_a(model_coefficients[3]);
589 const float par_b(model_coefficients[4]);
592 const Eigen::Matrix3f Rot = (Eigen::Matrix3f(3,3)
593 << x_axis(0), y_axis(0), n_axis(0),
594 x_axis(1), y_axis(1), n_axis(1),
595 x_axis(2), y_axis(2), n_axis(2))
597 const Eigen::Matrix3f Rot_T = Rot.transpose();
600 for (std::size_t i = 0; i < inliers.size (); ++i)
603 const Eigen::Vector3f p((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z);
606 const Eigen::Vector3f p_ = Rot_T * (p - c);
612 const Eigen::VectorXf params = (Eigen::VectorXf(5) << par_a, par_b, 0.0, 0.0, 0.0).finished();
614 dvec2ellipse(params, p_(0), p_(1), th_opt);
618 Eigen::Vector3f k_(0.0, 0.0, 0.0);
619 get_ellipse_point(params, th_opt, k_[0], k_[1]);
621 const Eigen::Vector3f k = c + Rot * k_;
623 projected_points[i].x =
static_cast<float> (k[0]);
624 projected_points[i].y =
static_cast<float> (k[1]);
625 projected_points[i].z =
static_cast<float> (k[2]);
631 template <
typename Po
intT>
bool
633 const std::set<index_t> &indices,
634 const Eigen::VectorXf &model_coefficients,
635 const double threshold)
const
638 if (!isModelValid (model_coefficients))
640 PCL_ERROR (
"[pcl::SampleConsensusModelEllipse3D::doSamplesVerifyModel] Given model is invalid!\n");
645 const Eigen::Vector3f c(model_coefficients[0], model_coefficients[1], model_coefficients[2]);
647 const Eigen::Vector3f n_axis(model_coefficients[5], model_coefficients[6], model_coefficients[7]);
649 const Eigen::Vector3f x_axis(model_coefficients[8], model_coefficients[9], model_coefficients[10]);
651 const Eigen::Vector3f y_axis = n_axis.cross(x_axis).normalized();
653 const float par_a(model_coefficients[3]);
655 const float par_b(model_coefficients[4]);
658 const Eigen::Matrix3f Rot = (Eigen::Matrix3f(3,3)
659 << x_axis(0), y_axis(0), n_axis(0),
660 x_axis(1), y_axis(1), n_axis(1),
661 x_axis(2), y_axis(2), n_axis(2))
663 const Eigen::Matrix3f Rot_T = Rot.transpose();
665 const auto squared_threshold = threshold * threshold;
666 for (
const auto &index : indices)
669 const Eigen::Vector3f p((*input_)[index].x, (*input_)[index].y, (*input_)[index].z);
672 const Eigen::Vector3f p_ = Rot_T * (p - c);
678 const Eigen::VectorXf params = (Eigen::VectorXf(5) << par_a, par_b, 0.0, 0.0, 0.0).finished();
680 const Eigen::Vector2f distanceVector = dvec2ellipse(params, p_(0), p_(1), th_opt);
682 if (distanceVector.squaredNorm() > squared_threshold)
689 template <
typename Po
intT>
bool
695 if (radius_min_ != std::numeric_limits<double>::lowest() && (model_coefficients[3] < radius_min_ || model_coefficients[4] < radius_min_))
697 PCL_DEBUG (
"[pcl::SampleConsensusModelEllipse3D::isModelValid] Semi-minor axis OR semi-major axis (radii) of ellipse is/are too small: should be larger than %g, but are {%g, %g}.\n",
698 radius_min_, model_coefficients[3], model_coefficients[4]);
701 if (radius_max_ != std::numeric_limits<double>::max() && (model_coefficients[3] > radius_max_ || model_coefficients[4] > radius_max_))
703 PCL_DEBUG (
"[pcl::SampleConsensusModelEllipse3D::isModelValid] Semi-minor axis OR semi-major axis (radii) of ellipse is/are too big: should be smaller than %g, but are {%g, %g}.\n",
704 radius_max_, model_coefficients[3], model_coefficients[4]);
714 template <
typename Po
intT>
716 const Eigen::VectorXf& par,
float th,
float& x,
float& y)
723 const float par_a(par[0]);
724 const float par_b(par[1]);
725 const float par_h(par[2]);
726 const float par_k(par[3]);
727 const float par_t(par[4]);
729 x = par_h + std::cos(par_t) * par_a * std::cos(th) -
730 std::sin(par_t) * par_b * std::sin(th);
731 y = par_k + std::sin(par_t) * par_a * std::cos(th) +
732 std::cos(par_t) * par_b * std::sin(th);
738 template <
typename Po
intT>
740 const Eigen::VectorXf& par,
float u,
float v,
float& th_opt)
748 const float par_h = par[2];
749 const float par_k = par[3];
751 const Eigen::Vector2f center(par_h, par_k);
752 Eigen::Vector2f p(u, v);
756 Eigen::Vector2f x_axis(0.0, 0.0);
757 get_ellipse_point(par, 0.0, x_axis(0), x_axis(1));
761 Eigen::Vector2f y_axis(0.0, 0.0);
762 get_ellipse_point(par,
M_PI / 2.0, y_axis(0), y_axis(1));
766 const float x_proj = p.dot(x_axis) / x_axis.norm();
767 const float y_proj = p.dot(y_axis) / y_axis.norm();
771 float th_min(0.0), th_max(0.0);
772 const float th = std::atan2(y_proj, x_proj);
774 if (-
M_PI <= th && th < -
M_PI / 2.0) {
777 th_max = -
M_PI / 2.0;
779 if (-
M_PI / 2.0 <= th && th < 0.0) {
781 th_min = -
M_PI / 2.0;
784 if (0.0 <= th && th <
M_PI / 2.0) {
789 if (
M_PI / 2.0 <= th && th <=
M_PI) {
796 th_opt = golden_section_search(par, u, v, th_min, th_max, 1.e-3);
799 float x(0.0), y(0.0);
800 get_ellipse_point(par, th_opt, x, y);
801 Eigen::Vector2f distanceVector(u - x, v - y);
802 return distanceVector;
806 template <
typename Po
intT>
808 const Eigen::VectorXf& par,
819 constexpr
float phi(1.61803398874989484820f);
822 float tl(th_min), tu(th_max);
823 float ta = tl + (tu - tl) * (1 - 1 / phi);
824 float tb = tl + (tu - tl) * 1 / phi;
826 while ((tu - tl) > epsilon) {
829 float x_a(0.0), y_a(0.0);
830 get_ellipse_point(par, ta, x_a, y_a);
831 float squared_dist_ta = (u - x_a) * (u - x_a) + (v - y_a) * (v - y_a);
834 float x_b(0.0), y_b(0.0);
835 get_ellipse_point(par, tb, x_b, y_b);
836 float squared_dist_tb = (u - x_b) * (u - x_b) + (v - y_b) * (v - y_b);
838 if (squared_dist_ta < squared_dist_tb) {
841 ta = tl + (tu - tl) * (1 - 1 / phi);
843 else if (squared_dist_ta > squared_dist_tb) {
846 tb = tl + (tu - tl) * 1 / phi;
851 ta = tl + (tu - tl) * (1 - 1 / phi);
852 tb = tl + (tu - tl) * 1 / phi;
855 return (tl + tu) / 2.0;
859 #define PCL_INSTANTIATE_SampleConsensusModelEllipse3D(T) template class PCL_EXPORTS pcl::SampleConsensusModelEllipse3D<T>;
PointCloud represents the base class in PCL for storing collections of 3D points.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
void resize(std::size_t count)
Resizes the container to contain count elements.
std::uint32_t width
The point cloud width (if organized as an image-structure).
pcl::PCLHeader header
The point cloud header.
std::uint32_t height
The point cloud height (if organized as an image-structure).
SampleConsensusModelEllipse3D defines a model for 3D ellipse segmentation.
void optimizeModelCoefficients(const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const override
Recompute the 3d ellipse coefficients using the given inlier set and return them to the user.
std::size_t countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold) const override
Count all the points which respect the given model coefficients as inliers.
void projectPoints(const Indices &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields=true) const override
Create a new point cloud with inliers projected onto the 3d ellipse model.
bool computeModelCoefficients(const Indices &samples, Eigen::VectorXf &model_coefficients) const override
Check whether the given index samples can form a valid 3D ellipse model, compute the model coefficien...
bool doSamplesVerifyModel(const std::set< index_t > &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const override
Verify whether a subset of indices verifies the given 3d ellipse model coefficients.
void selectWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers) override
Compute all distances from the cloud data to a given 3D ellipse model.
bool isSampleGood(const Indices &samples) const override
Check if a sample of indices results in a good sample of points indices.
bool isModelValid(const Eigen::VectorXf &model_coefficients) const override
Check whether a model is valid given the user constraints.
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const override
Compute all distances from the cloud data to a given 3D ellipse model.
SampleConsensusModel represents the base model class.
IndicesAllocator<> Indices
Type used for indices in PCL.
Helper functor structure for concatenate.