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,
315 error_sqr_dists_.clear();
317 if (!isModelValid (model_coefficients))
321 inliers.reserve (indices_->size ());
322 error_sqr_dists_.reserve (indices_->size ());
325 const Eigen::Vector3f c(model_coefficients[0], model_coefficients[1], model_coefficients[2]);
327 const Eigen::Vector3f n_axis(model_coefficients[5], model_coefficients[6], model_coefficients[7]);
329 const Eigen::Vector3f x_axis(model_coefficients[8], model_coefficients[9], model_coefficients[10]);
331 const Eigen::Vector3f y_axis = n_axis.cross(x_axis).normalized();
333 const float par_a(model_coefficients[3]);
335 const float par_b(model_coefficients[4]);
338 const Eigen::Matrix3f Rot = (Eigen::Matrix3f(3,3)
339 << x_axis(0), y_axis(0), n_axis(0),
340 x_axis(1), y_axis(1), n_axis(1),
341 x_axis(2), y_axis(2), n_axis(2))
343 const Eigen::Matrix3f Rot_T = Rot.transpose();
345 const auto squared_threshold = threshold * threshold;
347 for (std::size_t i = 0; i < indices_->size (); ++i)
350 const Eigen::Vector3f p((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z);
353 const Eigen::Vector3f p_ = Rot_T * (p - c);
359 const Eigen::VectorXf params = (Eigen::VectorXf(5) << par_a, par_b, 0.0, 0.0, 0.0).finished();
361 const Eigen::Vector2f distanceVector = dvec2ellipse(params, p_(0), p_(1), th_opt);
363 const double sqr_dist = distanceVector.squaredNorm();
364 if (sqr_dist < squared_threshold)
367 inliers.push_back ((*indices_)[i]);
368 error_sqr_dists_.push_back (sqr_dist);
376 const Eigen::VectorXf &model_coefficients,
const double threshold)
const
379 if (!isModelValid (model_coefficients))
381 std::size_t nr_p = 0;
384 const Eigen::Vector3f c(model_coefficients[0], model_coefficients[1], model_coefficients[2]);
386 const Eigen::Vector3f n_axis(model_coefficients[5], model_coefficients[6], model_coefficients[7]);
388 const Eigen::Vector3f x_axis(model_coefficients[8], model_coefficients[9], model_coefficients[10]);
390 const Eigen::Vector3f y_axis = n_axis.cross(x_axis).normalized();
392 const float par_a(model_coefficients[3]);
394 const float par_b(model_coefficients[4]);
397 const Eigen::Matrix3f Rot = (Eigen::Matrix3f(3,3)
398 << x_axis(0), y_axis(0), n_axis(0),
399 x_axis(1), y_axis(1), n_axis(1),
400 x_axis(2), y_axis(2), n_axis(2))
402 const Eigen::Matrix3f Rot_T = Rot.transpose();
404 const auto squared_threshold = threshold * threshold;
406 for (std::size_t i = 0; i < indices_->size (); ++i)
409 const Eigen::Vector3f p((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z);
412 const Eigen::Vector3f p_ = Rot_T * (p - c);
418 const Eigen::VectorXf params = (Eigen::VectorXf(5) << par_a, par_b, 0.0, 0.0, 0.0).finished();
420 const Eigen::Vector2f distanceVector = dvec2ellipse(params, p_(0), p_(1), th_opt);
422 if (distanceVector.squaredNorm() < squared_threshold)
432 const Eigen::VectorXf &model_coefficients,
433 Eigen::VectorXf &optimized_coefficients)
const
435 optimized_coefficients = model_coefficients;
438 if (!isModelValid (model_coefficients))
440 PCL_ERROR (
"[pcl::SampleConsensusModelEllipse3D::optimizeModelCoefficients] Given model is invalid!\n");
445 if (inliers.size () <= sample_size_)
447 PCL_ERROR (
"[pcl::SampleConsensusModelEllipse3D::optimizeModelCoefficients] Not enough inliers to refine/optimize the model's coefficients (%lu)! Returning the same coefficients.\n", inliers.size ());
451 OptimizationFunctor functor(
this, inliers);
452 Eigen::NumericalDiff<OptimizationFunctor> num_diff(functor);
453 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>,
double> lm(num_diff);
454 Eigen::VectorXd coeff = model_coefficients.cast<
double>();
455 int info = lm.minimize(coeff);
456 optimized_coefficients = coeff.cast<
float>();
459 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",
460 info, lm.fvec.norm (),
462 model_coefficients[0],
463 model_coefficients[1],
464 model_coefficients[2],
465 model_coefficients[3],
466 model_coefficients[4],
467 model_coefficients[5],
468 model_coefficients[6],
469 model_coefficients[7],
470 model_coefficients[8],
471 model_coefficients[9],
472 model_coefficients[10],
474 optimized_coefficients[0],
475 optimized_coefficients[1],
476 optimized_coefficients[2],
477 optimized_coefficients[3],
478 optimized_coefficients[4],
479 optimized_coefficients[5],
480 optimized_coefficients[6],
481 optimized_coefficients[7],
482 optimized_coefficients[8],
483 optimized_coefficients[9],
484 optimized_coefficients[10]);
490 const Indices &inliers,
const Eigen::VectorXf &model_coefficients,
491 PointCloud &projected_points,
bool copy_data_fields)
const
494 if (!isModelValid (model_coefficients))
496 PCL_ERROR (
"[pcl::SampleConsensusModelEllipse3D::projectPoints] Given model is invalid!\n");
500 projected_points.header = input_->header;
501 projected_points.is_dense = input_->is_dense;
504 if (copy_data_fields)
507 projected_points.resize (input_->size ());
508 projected_points.width = input_->width;
509 projected_points.height = input_->height;
511 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
513 for (std::size_t i = 0; i < projected_points.size(); ++i)
520 const Eigen::Vector3f c(model_coefficients[0], model_coefficients[1], model_coefficients[2]);
522 const Eigen::Vector3f n_axis(model_coefficients[5], model_coefficients[6], model_coefficients[7]);
524 const Eigen::Vector3f x_axis(model_coefficients[8], model_coefficients[9], model_coefficients[10]);
526 const Eigen::Vector3f y_axis = n_axis.cross(x_axis).normalized();
528 const float par_a(model_coefficients[3]);
530 const float par_b(model_coefficients[4]);
533 const Eigen::Matrix3f Rot = (Eigen::Matrix3f(3,3)
534 << x_axis(0), y_axis(0), n_axis(0),
535 x_axis(1), y_axis(1), n_axis(1),
536 x_axis(2), y_axis(2), n_axis(2))
538 const Eigen::Matrix3f Rot_T = Rot.transpose();
541 for (std::size_t i = 0; i < inliers.size (); ++i)
544 const Eigen::Vector3f p((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z);
547 const Eigen::Vector3f p_ = Rot_T * (p - c);
553 const Eigen::VectorXf params = (Eigen::VectorXf(5) << par_a, par_b, 0.0, 0.0, 0.0).finished();
555 dvec2ellipse(params, p_(0), p_(1), th_opt);
558 Eigen::Vector3f k_(0.0, 0.0, 0.0);
559 get_ellipse_point(params, th_opt, k_[0], k_[1]);
561 const Eigen::Vector3f k = c + Rot * k_;
563 projected_points[i].x =
static_cast<float> (k[0]);
564 projected_points[i].y =
static_cast<float> (k[1]);
565 projected_points[i].z =
static_cast<float> (k[2]);
571 projected_points.resize (inliers.size ());
572 projected_points.width = inliers.size ();
573 projected_points.height = 1;
575 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
577 for (std::size_t i = 0; i < inliers.size (); ++i)
579 pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> ((*input_)[inliers[i]], projected_points[i]));
582 const Eigen::Vector3f c(model_coefficients[0], model_coefficients[1], model_coefficients[2]);
584 const Eigen::Vector3f n_axis(model_coefficients[5], model_coefficients[6], model_coefficients[7]);
586 const Eigen::Vector3f x_axis(model_coefficients[8], model_coefficients[9], model_coefficients[10]);
588 const Eigen::Vector3f y_axis = n_axis.cross(x_axis).normalized();
590 const float par_a(model_coefficients[3]);
592 const float par_b(model_coefficients[4]);
595 const Eigen::Matrix3f Rot = (Eigen::Matrix3f(3,3)
596 << x_axis(0), y_axis(0), n_axis(0),
597 x_axis(1), y_axis(1), n_axis(1),
598 x_axis(2), y_axis(2), n_axis(2))
600 const Eigen::Matrix3f Rot_T = Rot.transpose();
603 for (std::size_t i = 0; i < inliers.size (); ++i)
606 const Eigen::Vector3f p((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z);
609 const Eigen::Vector3f p_ = Rot_T * (p - c);
615 const Eigen::VectorXf params = (Eigen::VectorXf(5) << par_a, par_b, 0.0, 0.0, 0.0).finished();
617 dvec2ellipse(params, p_(0), p_(1), th_opt);
621 Eigen::Vector3f k_(0.0, 0.0, 0.0);
622 get_ellipse_point(params, th_opt, k_[0], k_[1]);
624 const Eigen::Vector3f k = c + Rot * k_;
626 projected_points[i].x =
static_cast<float> (k[0]);
627 projected_points[i].y =
static_cast<float> (k[1]);
628 projected_points[i].z =
static_cast<float> (k[2]);
636 const std::set<index_t> &indices,
637 const Eigen::VectorXf &model_coefficients,
638 const double threshold)
const
641 if (!isModelValid (model_coefficients))
643 PCL_ERROR (
"[pcl::SampleConsensusModelEllipse3D::doSamplesVerifyModel] Given model is invalid!\n");
648 const Eigen::Vector3f c(model_coefficients[0], model_coefficients[1], model_coefficients[2]);
650 const Eigen::Vector3f n_axis(model_coefficients[5], model_coefficients[6], model_coefficients[7]);
652 const Eigen::Vector3f x_axis(model_coefficients[8], model_coefficients[9], model_coefficients[10]);
654 const Eigen::Vector3f y_axis = n_axis.cross(x_axis).normalized();
656 const float par_a(model_coefficients[3]);
658 const float par_b(model_coefficients[4]);
661 const Eigen::Matrix3f Rot = (Eigen::Matrix3f(3,3)
662 << x_axis(0), y_axis(0), n_axis(0),
663 x_axis(1), y_axis(1), n_axis(1),
664 x_axis(2), y_axis(2), n_axis(2))
666 const Eigen::Matrix3f Rot_T = Rot.transpose();
668 const auto squared_threshold = threshold * threshold;
669 for (
const auto &index : indices)
672 const Eigen::Vector3f p((*input_)[index].x, (*input_)[index].y, (*input_)[index].z);
675 const Eigen::Vector3f p_ = Rot_T * (p - c);
681 const Eigen::VectorXf params = (Eigen::VectorXf(5) << par_a, par_b, 0.0, 0.0, 0.0).finished();
683 const Eigen::Vector2f distanceVector = dvec2ellipse(params, p_(0), p_(1), th_opt);
685 if (distanceVector.squaredNorm() > squared_threshold)