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();
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]);
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)
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]);
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)
576 pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> ((*input_)[inliers[i]], projected_points[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]);
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)