diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_circle.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_circle.hpp index 1871cd41e8d..e209e0f1ba3 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_circle.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_circle.hpp @@ -66,7 +66,7 @@ pcl::SampleConsensusModelCircle2D::isSampleGood(const std::vector & ////////////////////////////////////////////////////////////////////////// template bool -pcl::SampleConsensusModelCircle2D::computeModelCoefficients (const std::vector &samples, Eigen::VectorXf &model_coefficients) +pcl::SampleConsensusModelCircle2D::computeModelCoefficients (const std::vector &samples, Eigen::VectorXf &model_coefficients) const { // Need 3 samples if (samples.size () != 3) @@ -102,7 +102,7 @@ pcl::SampleConsensusModelCircle2D::computeModelCoefficients (const std:: ////////////////////////////////////////////////////////////////////////// template void -pcl::SampleConsensusModelCircle2D::getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector &distances) +pcl::SampleConsensusModelCircle2D::getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector &distances) const { // Check if the model is valid given the user constraints if (!isModelValid (model_coefficients)) @@ -168,7 +168,7 @@ pcl::SampleConsensusModelCircle2D::selectWithinDistance ( ////////////////////////////////////////////////////////////////////////// template int pcl::SampleConsensusModelCircle2D::countWithinDistance ( - const Eigen::VectorXf &model_coefficients, const double threshold) + const Eigen::VectorXf &model_coefficients, const double threshold) const { // Check if the model is valid given the user constraints if (!isModelValid (model_coefficients)) @@ -196,7 +196,7 @@ pcl::SampleConsensusModelCircle2D::countWithinDistance ( ////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients ( - const std::vector &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) + const std::vector &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const { optimized_coefficients = model_coefficients; @@ -214,9 +214,7 @@ pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients ( return; } - tmp_inliers_ = &inliers; - - OptimizationFunctor functor (static_cast (inliers.size ()), this); + OptimizationFunctor functor (this, inliers); Eigen::NumericalDiff num_diff (functor); Eigen::LevenbergMarquardt, float> lm (num_diff); int info = lm.minimize (optimized_coefficients); @@ -230,7 +228,7 @@ pcl::SampleConsensusModelCircle2D::optimizeModelCoefficients ( template void pcl::SampleConsensusModelCircle2D::projectPoints ( const std::vector &inliers, const Eigen::VectorXf &model_coefficients, - PointCloud &projected_points, bool copy_data_fields) + PointCloud &projected_points, bool copy_data_fields) const { // Needs a valid set of model coefficients if (model_coefficients.size () != 3) @@ -296,7 +294,7 @@ pcl::SampleConsensusModelCircle2D::projectPoints ( ////////////////////////////////////////////////////////////////////////// template bool pcl::SampleConsensusModelCircle2D::doSamplesVerifyModel ( - const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) + const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const { // Needs a valid model coefficients if (model_coefficients.size () != 3) @@ -321,7 +319,7 @@ pcl::SampleConsensusModelCircle2D::doSamplesVerifyModel ( ////////////////////////////////////////////////////////////////////////// template bool -pcl::SampleConsensusModelCircle2D::isModelValid (const Eigen::VectorXf &model_coefficients) +pcl::SampleConsensusModelCircle2D::isModelValid (const Eigen::VectorXf &model_coefficients) const { if (!SampleConsensusModel::isModelValid (model_coefficients)) return (false); diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_circle3d.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_circle3d.hpp index 6b4bff710f3..1f255af0639 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_circle3d.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_circle3d.hpp @@ -65,7 +65,7 @@ pcl::SampleConsensusModelCircle3D::isSampleGood ( ////////////////////////////////////////////////////////////////////////// template bool -pcl::SampleConsensusModelCircle3D::computeModelCoefficients (const std::vector &samples, Eigen::VectorXf &model_coefficients) +pcl::SampleConsensusModelCircle3D::computeModelCoefficients (const std::vector &samples, Eigen::VectorXf &model_coefficients) const { // Need 3 samples if (samples.size () != 3) @@ -115,7 +115,7 @@ pcl::SampleConsensusModelCircle3D::computeModelCoefficients (const std:: ////////////////////////////////////////////////////////////////////////// template void -pcl::SampleConsensusModelCircle3D::getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector &distances) +pcl::SampleConsensusModelCircle3D::getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector &distances) const { // Check if the model is valid given the user constraints if (!isModelValid (model_coefficients)) @@ -212,7 +212,7 @@ pcl::SampleConsensusModelCircle3D::selectWithinDistance ( ////////////////////////////////////////////////////////////////////////// template int pcl::SampleConsensusModelCircle3D::countWithinDistance ( - const Eigen::VectorXf &model_coefficients, const double threshold) + const Eigen::VectorXf &model_coefficients, const double threshold) const { // Check if the model is valid given the user constraints if (!isModelValid (model_coefficients)) @@ -253,9 +253,9 @@ pcl::SampleConsensusModelCircle3D::countWithinDistance ( ////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelCircle3D::optimizeModelCoefficients ( - const std::vector &inliers, - const Eigen::VectorXf &model_coefficients, - Eigen::VectorXf &optimized_coefficients) + const std::vector &inliers, + const Eigen::VectorXf &model_coefficients, + Eigen::VectorXf &optimized_coefficients) const { optimized_coefficients = model_coefficients; @@ -273,9 +273,7 @@ pcl::SampleConsensusModelCircle3D::optimizeModelCoefficients ( return; } - tmp_inliers_ = &inliers; - - OptimizationFunctor functor (static_cast (inliers.size ()), this); + OptimizationFunctor functor (this, inliers); Eigen::NumericalDiff num_diff (functor); Eigen::LevenbergMarquardt, double> lm (num_diff); Eigen::VectorXd coeff; @@ -292,7 +290,7 @@ pcl::SampleConsensusModelCircle3D::optimizeModelCoefficients ( template void pcl::SampleConsensusModelCircle3D::projectPoints ( const std::vector &inliers, const Eigen::VectorXf &model_coefficients, - PointCloud &projected_points, bool copy_data_fields) + PointCloud &projected_points, bool copy_data_fields) const { // Needs a valid set of model coefficients if (model_coefficients.size () != 7) @@ -393,9 +391,9 @@ pcl::SampleConsensusModelCircle3D::projectPoints ( ////////////////////////////////////////////////////////////////////////// template bool pcl::SampleConsensusModelCircle3D::doSamplesVerifyModel ( - const std::set &indices, - const Eigen::VectorXf &model_coefficients, - const double threshold) + const std::set &indices, + const Eigen::VectorXf &model_coefficients, + const double threshold) const { // Needs a valid model coefficients if (model_coefficients.size () != 7) @@ -437,7 +435,7 @@ pcl::SampleConsensusModelCircle3D::doSamplesVerifyModel ( ////////////////////////////////////////////////////////////////////////// template bool -pcl::SampleConsensusModelCircle3D::isModelValid (const Eigen::VectorXf &model_coefficients) +pcl::SampleConsensusModelCircle3D::isModelValid (const Eigen::VectorXf &model_coefficients) const { if (!SampleConsensusModel::isModelValid (model_coefficients)) return (false); 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 dd1a29a1ead..ad679bbd80d 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 @@ -53,7 +53,7 @@ pcl::SampleConsensusModelCone::isSampleGood(const std::vector bool pcl::SampleConsensusModelCone::computeModelCoefficients ( - const std::vector &samples, Eigen::VectorXf &model_coefficients) + const std::vector &samples, Eigen::VectorXf &model_coefficients) const { // Need 3 samples if (samples.size () != 3) @@ -135,7 +135,7 @@ pcl::SampleConsensusModelCone::computeModelCoefficients ( ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelCone::getDistancesToModel ( - const Eigen::VectorXf &model_coefficients, std::vector &distances) + const Eigen::VectorXf &model_coefficients, std::vector &distances) const { // Check if the model is valid given the user constraints if (!isModelValid (model_coefficients)) @@ -253,7 +253,7 @@ pcl::SampleConsensusModelCone::selectWithinDistance ( ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template int pcl::SampleConsensusModelCone::countWithinDistance ( - const Eigen::VectorXf &model_coefficients, const double threshold) + const Eigen::VectorXf &model_coefficients, const double threshold) const { // Check if the model is valid given the user constraints @@ -307,7 +307,7 @@ pcl::SampleConsensusModelCone::countWithinDistance ( ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelCone::optimizeModelCoefficients ( - const std::vector &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) + const std::vector &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const { optimized_coefficients = model_coefficients; @@ -324,9 +324,7 @@ pcl::SampleConsensusModelCone::optimizeModelCoefficients ( return; } - tmp_inliers_ = &inliers; - - OptimizationFunctor functor (static_cast (inliers.size ()), this); + OptimizationFunctor functor (this, inliers); Eigen::NumericalDiff num_diff (functor); Eigen::LevenbergMarquardt, float> lm (num_diff); int info = lm.minimize (optimized_coefficients); @@ -346,7 +344,7 @@ pcl::SampleConsensusModelCone::optimizeModelCoefficients ( ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelCone::projectPoints ( - const std::vector &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) + const std::vector &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) const { // Needs a valid set of model coefficients if (model_coefficients.size () != 7) @@ -442,7 +440,7 @@ pcl::SampleConsensusModelCone::projectPoints ( ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template bool pcl::SampleConsensusModelCone::doSamplesVerifyModel ( - const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) + const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const { // Needs a valid model coefficients if (model_coefficients.size () != 7) @@ -485,7 +483,7 @@ pcl::SampleConsensusModelCone::doSamplesVerifyModel ( ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template double pcl::SampleConsensusModelCone::pointToAxisDistance ( - const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients) + const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients) const { Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); @@ -494,7 +492,7 @@ pcl::SampleConsensusModelCone::pointToAxisDistance ( ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template bool -pcl::SampleConsensusModelCone::isModelValid (const Eigen::VectorXf &model_coefficients) +pcl::SampleConsensusModelCone::isModelValid (const Eigen::VectorXf &model_coefficients) const { if (!SampleConsensusModel::isModelValid (model_coefficients)) return (false); 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 798c41c79a4..bc2dcebee25 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 @@ -55,7 +55,7 @@ pcl::SampleConsensusModelCylinder::isSampleGood (const std::vec ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template bool pcl::SampleConsensusModelCylinder::computeModelCoefficients ( - const std::vector &samples, Eigen::VectorXf &model_coefficients) + const std::vector &samples, Eigen::VectorXf &model_coefficients) const { // Need 2 samples if (samples.size () != 2) @@ -129,7 +129,7 @@ pcl::SampleConsensusModelCylinder::computeModelCoefficients ( ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelCylinder::getDistancesToModel ( - const Eigen::VectorXf &model_coefficients, std::vector &distances) + const Eigen::VectorXf &model_coefficients, std::vector &distances) const { // Check if the model is valid given the user constraints if (!isModelValid (model_coefficients)) @@ -224,7 +224,7 @@ pcl::SampleConsensusModelCylinder::selectWithinDistance ( ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template int pcl::SampleConsensusModelCylinder::countWithinDistance ( - const Eigen::VectorXf &model_coefficients, const double threshold) + const Eigen::VectorXf &model_coefficients, const double threshold) const { // Check if the model is valid given the user constraints if (!isModelValid (model_coefficients)) @@ -264,7 +264,7 @@ pcl::SampleConsensusModelCylinder::countWithinDistance ( ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelCylinder::optimizeModelCoefficients ( - const std::vector &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) + const std::vector &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const { optimized_coefficients = model_coefficients; @@ -281,9 +281,7 @@ pcl::SampleConsensusModelCylinder::optimizeModelCoefficients ( return; } - tmp_inliers_ = &inliers; - - OptimizationFunctor functor (static_cast (inliers.size ()), this); + OptimizationFunctor functor (this, inliers); Eigen::NumericalDiff num_diff (functor); Eigen::LevenbergMarquardt, float> lm (num_diff); int info = lm.minimize (optimized_coefficients); @@ -303,7 +301,7 @@ pcl::SampleConsensusModelCylinder::optimizeModelCoefficients ( ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelCylinder::projectPoints ( - const std::vector &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) + const std::vector &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) const { // Needs a valid set of model coefficients if (model_coefficients.size () != 7) @@ -389,7 +387,7 @@ pcl::SampleConsensusModelCylinder::projectPoints ( ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template bool pcl::SampleConsensusModelCylinder::doSamplesVerifyModel ( - const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) + const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const { // Needs a valid model coefficients if (model_coefficients.size () != 7) @@ -414,7 +412,7 @@ pcl::SampleConsensusModelCylinder::doSamplesVerifyModel ( ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template double pcl::SampleConsensusModelCylinder::pointToLineDistance ( - const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients) + const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients) const { Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); @@ -424,7 +422,7 @@ pcl::SampleConsensusModelCylinder::pointToLineDistance ( ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelCylinder::projectPointToCylinder ( - const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients, Eigen::Vector4f &pt_proj) + const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients, Eigen::Vector4f &pt_proj) const { Eigen::Vector4f line_pt (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0); Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0); @@ -441,7 +439,7 @@ pcl::SampleConsensusModelCylinder::projectPointToCylinder ( ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template bool -pcl::SampleConsensusModelCylinder::isModelValid (const Eigen::VectorXf &model_coefficients) +pcl::SampleConsensusModelCylinder::isModelValid (const Eigen::VectorXf &model_coefficients) const { if (!SampleConsensusModel::isModelValid (model_coefficients)) return (false); diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_line.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_line.hpp index af019a1aaea..8c4f40726fc 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_line.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_line.hpp @@ -63,7 +63,7 @@ pcl::SampleConsensusModelLine::isSampleGood (const std::vector &sam ////////////////////////////////////////////////////////////////////////// template bool pcl::SampleConsensusModelLine::computeModelCoefficients ( - const std::vector &samples, Eigen::VectorXf &model_coefficients) + const std::vector &samples, Eigen::VectorXf &model_coefficients) const { // Need 2 samples if (samples.size () != 2) @@ -95,7 +95,7 @@ pcl::SampleConsensusModelLine::computeModelCoefficients ( ////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelLine::getDistancesToModel ( - const Eigen::VectorXf &model_coefficients, std::vector &distances) + const Eigen::VectorXf &model_coefficients, std::vector &distances) const { // Needs a valid set of model coefficients if (!isModelValid (model_coefficients)) @@ -160,7 +160,7 @@ pcl::SampleConsensusModelLine::selectWithinDistance ( ////////////////////////////////////////////////////////////////////////// template int pcl::SampleConsensusModelLine::countWithinDistance ( - const Eigen::VectorXf &model_coefficients, const double threshold) + const Eigen::VectorXf &model_coefficients, const double threshold) const { // Needs a valid set of model coefficients if (!isModelValid (model_coefficients)) @@ -191,7 +191,7 @@ pcl::SampleConsensusModelLine::countWithinDistance ( ////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelLine::optimizeModelCoefficients ( - const std::vector &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) + const std::vector &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const { // Needs a valid set of model coefficients if (!isModelValid (model_coefficients)) @@ -232,7 +232,7 @@ pcl::SampleConsensusModelLine::optimizeModelCoefficients ( ////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelLine::projectPoints ( - const std::vector &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) + const std::vector &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) const { // Needs a valid model coefficients if (!isModelValid (model_coefficients)) @@ -305,7 +305,7 @@ pcl::SampleConsensusModelLine::projectPoints ( ////////////////////////////////////////////////////////////////////////// template bool pcl::SampleConsensusModelLine::doSamplesVerifyModel ( - const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) + const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const { // Needs a valid set of model coefficients if (!isModelValid (model_coefficients)) diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_normal_parallel_plane.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_normal_parallel_plane.hpp index 55ce7d7f30a..d754dc94357 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_normal_parallel_plane.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_normal_parallel_plane.hpp @@ -45,7 +45,7 @@ ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template bool -pcl::SampleConsensusModelNormalParallelPlane::isModelValid (const Eigen::VectorXf &model_coefficients) +pcl::SampleConsensusModelNormalParallelPlane::isModelValid (const Eigen::VectorXf &model_coefficients) const { if (!SampleConsensusModel::isModelValid (model_coefficients)) return (false); diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_normal_plane.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_normal_plane.hpp index f56d1c02fd0..7dfbc08f75c 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_normal_plane.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_normal_plane.hpp @@ -104,7 +104,7 @@ pcl::SampleConsensusModelNormalPlane::selectWithinDistance ( ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template int pcl::SampleConsensusModelNormalPlane::countWithinDistance ( - const Eigen::VectorXf &model_coefficients, const double threshold) + const Eigen::VectorXf &model_coefficients, const double threshold) const { if (!normals_) { @@ -149,7 +149,7 @@ pcl::SampleConsensusModelNormalPlane::countWithinDistance ( ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelNormalPlane::getDistancesToModel ( - const Eigen::VectorXf &model_coefficients, std::vector &distances) + const Eigen::VectorXf &model_coefficients, std::vector &distances) const { if (!normals_) { diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_normal_sphere.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_normal_sphere.hpp index 3169f38eb66..02cdde31e62 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_normal_sphere.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_normal_sphere.hpp @@ -108,7 +108,7 @@ pcl::SampleConsensusModelNormalSphere::selectWithinDistance ( ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template int pcl::SampleConsensusModelNormalSphere::countWithinDistance ( - const Eigen::VectorXf &model_coefficients, const double threshold) + const Eigen::VectorXf &model_coefficients, const double threshold) const { if (!normals_) { @@ -158,7 +158,7 @@ pcl::SampleConsensusModelNormalSphere::countWithinDistance ( ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelNormalSphere::getDistancesToModel ( - const Eigen::VectorXf &model_coefficients, std::vector &distances) + const Eigen::VectorXf &model_coefficients, std::vector &distances) const { if (!normals_) { @@ -207,7 +207,7 @@ pcl::SampleConsensusModelNormalSphere::getDistancesToModel ( ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// template bool -pcl::SampleConsensusModelNormalSphere::isModelValid (const Eigen::VectorXf &model_coefficients) +pcl::SampleConsensusModelNormalSphere::isModelValid (const Eigen::VectorXf &model_coefficients) const { if (!SampleConsensusModel::isModelValid (model_coefficients)) return (false); diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_parallel_line.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_parallel_line.hpp index 366aaf82812..6426f6fdf1f 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_parallel_line.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_parallel_line.hpp @@ -62,7 +62,7 @@ pcl::SampleConsensusModelParallelLine::selectWithinDistance ( ////////////////////////////////////////////////////////////////////////// template int pcl::SampleConsensusModelParallelLine::countWithinDistance ( - const Eigen::VectorXf &model_coefficients, const double threshold) + const Eigen::VectorXf &model_coefficients, const double threshold) const { // Check if the model is valid given the user constraints if (!isModelValid (model_coefficients)) @@ -74,7 +74,7 @@ pcl::SampleConsensusModelParallelLine::countWithinDistance ( ////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelParallelLine::getDistancesToModel ( - const Eigen::VectorXf &model_coefficients, std::vector &distances) + const Eigen::VectorXf &model_coefficients, std::vector &distances) const { // Check if the model is valid given the user constraints if (!isModelValid (model_coefficients)) @@ -88,7 +88,7 @@ pcl::SampleConsensusModelParallelLine::getDistancesToModel ( ////////////////////////////////////////////////////////////////////////// template bool -pcl::SampleConsensusModelParallelLine::isModelValid (const Eigen::VectorXf &model_coefficients) +pcl::SampleConsensusModelParallelLine::isModelValid (const Eigen::VectorXf &model_coefficients) const { if (!SampleConsensusModel::isModelValid (model_coefficients)) return (false); diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_parallel_plane.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_parallel_plane.hpp index 9db4c1b61aa..c9dc36ea53a 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_parallel_plane.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_parallel_plane.hpp @@ -61,7 +61,7 @@ pcl::SampleConsensusModelParallelPlane::selectWithinDistance ( ////////////////////////////////////////////////////////////////////////// template int pcl::SampleConsensusModelParallelPlane::countWithinDistance ( - const Eigen::VectorXf &model_coefficients, const double threshold) + const Eigen::VectorXf &model_coefficients, const double threshold) const { // Check if the model is valid given the user constraints if (!isModelValid (model_coefficients)) @@ -73,7 +73,7 @@ pcl::SampleConsensusModelParallelPlane::countWithinDistance ( ////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelParallelPlane::getDistancesToModel ( - const Eigen::VectorXf &model_coefficients, std::vector &distances) + const Eigen::VectorXf &model_coefficients, std::vector &distances) const { // Check if the model is valid given the user constraints if (!isModelValid (model_coefficients)) @@ -87,7 +87,7 @@ pcl::SampleConsensusModelParallelPlane::getDistancesToModel ( ////////////////////////////////////////////////////////////////////////// template bool -pcl::SampleConsensusModelParallelPlane::isModelValid (const Eigen::VectorXf &model_coefficients) +pcl::SampleConsensusModelParallelPlane::isModelValid (const Eigen::VectorXf &model_coefficients) const { if (!SampleConsensusModel::isModelValid (model_coefficients)) return (false); diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_perpendicular_plane.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_perpendicular_plane.hpp index b080e286b50..036b7604e62 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_perpendicular_plane.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_perpendicular_plane.hpp @@ -61,7 +61,7 @@ pcl::SampleConsensusModelPerpendicularPlane::selectWithinDistance ( ////////////////////////////////////////////////////////////////////////// template int pcl::SampleConsensusModelPerpendicularPlane::countWithinDistance ( - const Eigen::VectorXf &model_coefficients, const double threshold) + const Eigen::VectorXf &model_coefficients, const double threshold) const { // Check if the model is valid given the user constraints if (!isModelValid (model_coefficients)) @@ -73,7 +73,7 @@ pcl::SampleConsensusModelPerpendicularPlane::countWithinDistance ( ////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelPerpendicularPlane::getDistancesToModel ( - const Eigen::VectorXf &model_coefficients, std::vector &distances) + const Eigen::VectorXf &model_coefficients, std::vector &distances) const { // Check if the model is valid given the user constraints if (!isModelValid (model_coefficients)) @@ -87,7 +87,7 @@ pcl::SampleConsensusModelPerpendicularPlane::getDistancesToModel ( ////////////////////////////////////////////////////////////////////////// template bool -pcl::SampleConsensusModelPerpendicularPlane::isModelValid (const Eigen::VectorXf &model_coefficients) +pcl::SampleConsensusModelPerpendicularPlane::isModelValid (const Eigen::VectorXf &model_coefficients) const { if (!SampleConsensusModel::isModelValid (model_coefficients)) return (false); diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_plane.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_plane.hpp index 17ec422068d..18ad459b6b4 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_plane.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_plane.hpp @@ -67,7 +67,7 @@ pcl::SampleConsensusModelPlane::isSampleGood (const std::vector &sa ////////////////////////////////////////////////////////////////////////// template bool pcl::SampleConsensusModelPlane::computeModelCoefficients ( - const std::vector &samples, Eigen::VectorXf &model_coefficients) + const std::vector &samples, Eigen::VectorXf &model_coefficients) const { // Need 3 samples if (samples.size () != sample_size_) @@ -110,7 +110,7 @@ pcl::SampleConsensusModelPlane::computeModelCoefficients ( ////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelPlane::getDistancesToModel ( - const Eigen::VectorXf &model_coefficients, std::vector &distances) + const Eigen::VectorXf &model_coefficients, std::vector &distances) const { // Needs a valid set of model coefficients if (model_coefficients.size () != model_size_) @@ -181,7 +181,7 @@ pcl::SampleConsensusModelPlane::selectWithinDistance ( ////////////////////////////////////////////////////////////////////////// template int pcl::SampleConsensusModelPlane::countWithinDistance ( - const Eigen::VectorXf &model_coefficients, const double threshold) + const Eigen::VectorXf &model_coefficients, const double threshold) const { // Needs a valid set of model coefficients if (model_coefficients.size () != model_size_) @@ -210,7 +210,7 @@ pcl::SampleConsensusModelPlane::countWithinDistance ( ////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelPlane::optimizeModelCoefficients ( - const std::vector &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) + const std::vector &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const { // Needs a valid set of model coefficients if (model_coefficients.size () != model_size_) @@ -259,7 +259,7 @@ pcl::SampleConsensusModelPlane::optimizeModelCoefficients ( ////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelPlane::projectPoints ( - const std::vector &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) + const std::vector &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) const { // Needs a valid set of model coefficients if (model_coefficients.size () != model_size_) @@ -343,7 +343,7 @@ pcl::SampleConsensusModelPlane::projectPoints ( ////////////////////////////////////////////////////////////////////////// template bool pcl::SampleConsensusModelPlane::doSamplesVerifyModel ( - const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) + const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const { // Needs a valid set of model coefficients if (model_coefficients.size () != model_size_) diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_registration.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_registration.hpp index 6f2de207257..9b214ecf089 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_registration.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_registration.hpp @@ -64,7 +64,7 @@ pcl::SampleConsensusModelRegistration::isSampleGood (const std::vector bool -pcl::SampleConsensusModelRegistration::computeModelCoefficients (const std::vector &samples, Eigen::VectorXf &model_coefficients) +pcl::SampleConsensusModelRegistration::computeModelCoefficients (const std::vector &samples, Eigen::VectorXf &model_coefficients) const { if (!target_) { @@ -77,7 +77,7 @@ pcl::SampleConsensusModelRegistration::computeModelCoefficients (const s std::vector indices_tgt (3); for (int i = 0; i < 3; ++i) - indices_tgt[i] = correspondences_[samples[i]]; + indices_tgt[i] = correspondences_.at (samples[i]); estimateRigidTransformationSVD (*input_, samples, *target_, indices_tgt, model_coefficients); return (true); @@ -85,7 +85,7 @@ pcl::SampleConsensusModelRegistration::computeModelCoefficients (const s ////////////////////////////////////////////////////////////////////////// template void -pcl::SampleConsensusModelRegistration::getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector &distances) +pcl::SampleConsensusModelRegistration::getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector &distances) const { if (indices_->size () != indices_tgt_->size ()) { @@ -191,7 +191,7 @@ pcl::SampleConsensusModelRegistration::selectWithinDistance (const Eigen ////////////////////////////////////////////////////////////////////////// template int pcl::SampleConsensusModelRegistration::countWithinDistance ( - const Eigen::VectorXf &model_coefficients, const double threshold) + const Eigen::VectorXf &model_coefficients, const double threshold) const { if (indices_->size () != indices_tgt_->size ()) { @@ -236,7 +236,7 @@ pcl::SampleConsensusModelRegistration::countWithinDistance ( ////////////////////////////////////////////////////////////////////////// template void -pcl::SampleConsensusModelRegistration::optimizeModelCoefficients (const std::vector &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) +pcl::SampleConsensusModelRegistration::optimizeModelCoefficients (const std::vector &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const { if (indices_->size () != indices_tgt_->size ()) { @@ -257,7 +257,7 @@ pcl::SampleConsensusModelRegistration::optimizeModelCoefficients (const for (size_t i = 0; i < inliers.size (); ++i) { indices_src[i] = inliers[i]; - indices_tgt[i] = correspondences_[indices_src[i]]; + indices_tgt[i] = correspondences_.at (indices_src[i]); } estimateRigidTransformationSVD (*input_, indices_src, *target_, indices_tgt, optimized_coefficients); @@ -266,11 +266,11 @@ pcl::SampleConsensusModelRegistration::optimizeModelCoefficients (const ////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelRegistration::estimateRigidTransformationSVD ( - const pcl::PointCloud &cloud_src, - const std::vector &indices_src, + const pcl::PointCloud &cloud_src, + const std::vector &indices_src, const pcl::PointCloud &cloud_tgt, - const std::vector &indices_tgt, - Eigen::VectorXf &transform) + const std::vector &indices_tgt, + Eigen::VectorXf &transform) const { transform.resize (16); diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_registration_2d.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_registration_2d.hpp index 54b8688e7ca..afc6b93b26f 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_registration_2d.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_registration_2d.hpp @@ -61,7 +61,7 @@ pcl::SampleConsensusModelRegistration2D::isSampleGood (const std::vector ////////////////////////////////////////////////////////////////////////// template void -pcl::SampleConsensusModelRegistration2D::getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector &distances) +pcl::SampleConsensusModelRegistration2D::getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector &distances) const { PCL_INFO ("[pcl::SampleConsensusModelRegistration2D::getDistancesToModel]\n"); if (indices_->size () != indices_tgt_->size ()) @@ -176,7 +176,7 @@ pcl::SampleConsensusModelRegistration2D::selectWithinDistance (const Eig ////////////////////////////////////////////////////////////////////////// template int pcl::SampleConsensusModelRegistration2D::countWithinDistance ( - const Eigen::VectorXf &model_coefficients, const double threshold) + const Eigen::VectorXf &model_coefficients, const double threshold) const { if (indices_->size () != indices_tgt_->size ()) { 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 f0791721083..604b42be5a6 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 @@ -54,7 +54,7 @@ pcl::SampleConsensusModelSphere::isSampleGood (const std::vector &) ////////////////////////////////////////////////////////////////////////// template bool pcl::SampleConsensusModelSphere::computeModelCoefficients ( - const std::vector &samples, Eigen::VectorXf &model_coefficients) + const std::vector &samples, Eigen::VectorXf &model_coefficients) const { // Need 4 samples if (samples.size () != 4) @@ -120,7 +120,7 @@ pcl::SampleConsensusModelSphere::computeModelCoefficients ( ////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelSphere::getDistancesToModel ( - const Eigen::VectorXf &model_coefficients, std::vector &distances) + const Eigen::VectorXf &model_coefficients, std::vector &distances) const { // Check if the model is valid given the user constraints if (!isModelValid (model_coefficients)) @@ -192,7 +192,7 @@ pcl::SampleConsensusModelSphere::selectWithinDistance ( ////////////////////////////////////////////////////////////////////////// template int pcl::SampleConsensusModelSphere::countWithinDistance ( - const Eigen::VectorXf &model_coefficients, const double threshold) + const Eigen::VectorXf &model_coefficients, const double threshold) const { // Check if the model is valid given the user constraints if (!isModelValid (model_coefficients)) @@ -223,7 +223,7 @@ pcl::SampleConsensusModelSphere::countWithinDistance ( ////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelSphere::optimizeModelCoefficients ( - const std::vector &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) + const std::vector &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const { optimized_coefficients = model_coefficients; @@ -241,9 +241,7 @@ pcl::SampleConsensusModelSphere::optimizeModelCoefficients ( return; } - tmp_inliers_ = &inliers; - - OptimizationFunctor functor (static_cast (inliers.size ()), this); + OptimizationFunctor functor (this, inliers); Eigen::NumericalDiff num_diff (functor); Eigen::LevenbergMarquardt, float> lm (num_diff); int info = lm.minimize (optimized_coefficients); @@ -256,7 +254,7 @@ pcl::SampleConsensusModelSphere::optimizeModelCoefficients ( ////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelSphere::projectPoints ( - const std::vector &, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool) + const std::vector &, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool) const { // Needs a valid model coefficients if (model_coefficients.size () != 4) @@ -279,7 +277,7 @@ pcl::SampleConsensusModelSphere::projectPoints ( ////////////////////////////////////////////////////////////////////////// template bool pcl::SampleConsensusModelSphere::doSamplesVerifyModel ( - const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) + const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const { // Needs a valid model coefficients if (model_coefficients.size () != 4) diff --git a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_stick.hpp b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_stick.hpp index 3e43cdedccf..ebfe50d3b39 100644 --- a/sample_consensus/include/pcl/sample_consensus/impl/sac_model_stick.hpp +++ b/sample_consensus/include/pcl/sample_consensus/impl/sac_model_stick.hpp @@ -63,7 +63,7 @@ pcl::SampleConsensusModelStick::isSampleGood (const std::vector &sa ////////////////////////////////////////////////////////////////////////// template bool pcl::SampleConsensusModelStick::computeModelCoefficients ( - const std::vector &samples, Eigen::VectorXf &model_coefficients) + const std::vector &samples, Eigen::VectorXf &model_coefficients) const { // Need 2 samples if (samples.size () != 2) @@ -94,7 +94,7 @@ pcl::SampleConsensusModelStick::computeModelCoefficients ( ////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelStick::getDistancesToModel ( - const Eigen::VectorXf &model_coefficients, std::vector &distances) + const Eigen::VectorXf &model_coefficients, std::vector &distances) const { // Needs a valid set of model coefficients if (!isModelValid (model_coefficients)) @@ -176,7 +176,7 @@ pcl::SampleConsensusModelStick::selectWithinDistance ( /////////////////////////////////////////////////////////////////////////// template int pcl::SampleConsensusModelStick::countWithinDistance ( - const Eigen::VectorXf &model_coefficients, const double threshold) + const Eigen::VectorXf &model_coefficients, const double threshold) const { // Needs a valid set of model coefficients if (!isModelValid (model_coefficients)) @@ -221,7 +221,7 @@ pcl::SampleConsensusModelStick::countWithinDistance ( ////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelStick::optimizeModelCoefficients ( - const std::vector &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) + const std::vector &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const { // Needs a valid set of model coefficients if (!isModelValid (model_coefficients)) @@ -262,7 +262,7 @@ pcl::SampleConsensusModelStick::optimizeModelCoefficients ( ////////////////////////////////////////////////////////////////////////// template void pcl::SampleConsensusModelStick::projectPoints ( - const std::vector &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) + const std::vector &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) const { // Needs a valid model coefficients if (!isModelValid (model_coefficients)) @@ -335,7 +335,7 @@ pcl::SampleConsensusModelStick::projectPoints ( ////////////////////////////////////////////////////////////////////////// template bool pcl::SampleConsensusModelStick::doSamplesVerifyModel ( - const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) + const std::set &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const { // Needs a valid set of model coefficients if (!isModelValid (model_coefficients)) diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model.h b/sample_consensus/include/pcl/sample_consensus/sac_model.h index 7cd0a72e6d9..1af4891ad43 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model.h @@ -216,9 +216,9 @@ namespace pcl * for creating a valid model * \param[out] model_coefficients the computed model coefficients */ - virtual bool - computeModelCoefficients (const std::vector &samples, - Eigen::VectorXf &model_coefficients) = 0; + virtual bool + computeModelCoefficients (const std::vector &samples, + Eigen::VectorXf &model_coefficients) const = 0; /** \brief Recompute the model coefficients using the given inlier set * and return them to the user. Pure virtual. @@ -230,19 +230,19 @@ namespace pcl * \param[in] model_coefficients the initial guess for the model coefficients * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization */ - virtual void - optimizeModelCoefficients (const std::vector &inliers, + virtual void + optimizeModelCoefficients (const std::vector &inliers, const Eigen::VectorXf &model_coefficients, - Eigen::VectorXf &optimized_coefficients) = 0; + Eigen::VectorXf &optimized_coefficients) const = 0; /** \brief Compute all distances from the cloud data to a given model. Pure virtual. * * \param[in] model_coefficients the coefficients of a model that we need to compute distances to * \param[out] distances the resultant estimated distances */ - virtual void - getDistancesToModel (const Eigen::VectorXf &model_coefficients, - std::vector &distances) = 0; + virtual void + getDistancesToModel (const Eigen::VectorXf &model_coefficients, + std::vector &distances) const = 0; /** \brief Select all the points which respect the given model * coefficients as inliers. Pure virtual. @@ -267,8 +267,8 @@ namespace pcl * \return the resultant number of inliers */ virtual int - countWithinDistance (const Eigen::VectorXf &model_coefficients, - const double threshold) = 0; + countWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold) const = 0; /** \brief Create a new point cloud with inliers projected onto the model. Pure virtual. * \param[in] inliers the data inliers that we want to project on the model @@ -278,11 +278,11 @@ namespace pcl * projected_points cloud to be an exact copy of the input dataset minus * the point projections on the plane model */ - virtual void - projectPoints (const std::vector &inliers, + virtual void + projectPoints (const std::vector &inliers, const Eigen::VectorXf &model_coefficients, - PointCloud &projected_points, - bool copy_data_fields = true) = 0; + PointCloud &projected_points, + bool copy_data_fields = true) const = 0; /** \brief Verify whether a subset of indices verifies a given set of * model coefficients. Pure virtual. @@ -293,9 +293,9 @@ namespace pcl * determining the inliers from the outliers */ virtual bool - doSamplesVerifyModel (const std::set &indices, - const Eigen::VectorXf &model_coefficients, - const double threshold) = 0; + doSamplesVerifyModel (const std::set &indices, + const Eigen::VectorXf &model_coefficients, + const double threshold) const = 0; /** \brief Provide a pointer to the input dataset * \param[in] cloud the const boost shared pointer to a PointCloud message @@ -389,12 +389,12 @@ namespace pcl * \param[out] max_radius the resultant maximum radius model */ inline void - getRadiusLimits (double &min_radius, double &max_radius) + getRadiusLimits (double &min_radius, double &max_radius) const { min_radius = radius_min_; max_radius = radius_max_; } - + /** \brief Set the maximum distance allowed when drawing random samples * \param[in] radius the maximum distance (L2 norm) * \param search @@ -411,7 +411,7 @@ namespace pcl * \param[out] radius the maximum distance (L2 norm) */ inline void - getSamplesMaxDist (double &radius) + getSamplesMaxDist (double &radius) const { radius = samples_radius_; } @@ -419,10 +419,10 @@ namespace pcl friend class ProgressiveSampleConsensus; /** \brief Compute the variance of the errors to the model. - * \param[in] error_sqr_dists a vector holding the distances - */ + * \param[in] error_sqr_dists a vector holding the distances + */ inline double - computeVariance (const std::vector &error_sqr_dists) + computeVariance (const std::vector &error_sqr_dists) const { std::vector dists (error_sqr_dists); const size_t medIdx = dists.size () >> 1; @@ -436,7 +436,7 @@ namespace pcl * selectWithinDistance must be called). */ inline double - computeVariance () + computeVariance () const { if (error_sqr_dists_.empty ()) { @@ -513,7 +513,7 @@ namespace pcl * \param[in] model_coefficients the set of model coefficients */ virtual bool - isModelValid (const Eigen::VectorXf &model_coefficients) + isModelValid (const Eigen::VectorXf &model_coefficients) const { if (model_coefficients.size () != model_size_) { diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_circle.h b/sample_consensus/include/pcl/sample_consensus/sac_model_circle.h index bcaeb55187f..07762ea258c 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_circle.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_circle.h @@ -78,7 +78,7 @@ namespace pcl * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) */ SampleConsensusModelCircle2D (const PointCloudConstPtr &cloud, bool random = false) - : SampleConsensusModel (cloud, random), tmp_inliers_ () + : SampleConsensusModel (cloud, random) { model_name_ = "SampleConsensusModelCircle2D"; sample_size_ = 3; @@ -93,7 +93,7 @@ namespace pcl SampleConsensusModelCircle2D (const PointCloudConstPtr &cloud, const std::vector &indices, bool random = false) - : SampleConsensusModel (cloud, indices, random), tmp_inliers_ () + : SampleConsensusModel (cloud, indices, random) { model_name_ = "SampleConsensusModelCircle2D"; sample_size_ = 3; @@ -104,7 +104,7 @@ namespace pcl * \param[in] source the model to copy into this */ SampleConsensusModelCircle2D (const SampleConsensusModelCircle2D &source) : - SampleConsensusModel (), tmp_inliers_ () + SampleConsensusModel () { *this = source; model_name_ = "SampleConsensusModelCircle2D"; @@ -120,7 +120,6 @@ namespace pcl operator = (const SampleConsensusModelCircle2D &source) { SampleConsensusModel::operator=(source); - tmp_inliers_ = source.tmp_inliers_; return (*this); } @@ -129,17 +128,17 @@ namespace pcl * \param[in] samples the point indices found as possible good candidates for creating a valid model * \param[out] model_coefficients the resultant model coefficients */ - bool - computeModelCoefficients (const std::vector &samples, - Eigen::VectorXf &model_coefficients); + bool + computeModelCoefficients (const std::vector &samples, + Eigen::VectorXf &model_coefficients) const; /** \brief Compute all distances from the cloud data to a given 2D circle model. * \param[in] model_coefficients the coefficients of a 2D circle model that we need to compute distances to * \param[out] distances the resultant estimated distances */ - void - getDistancesToModel (const Eigen::VectorXf &model_coefficients, - std::vector &distances); + void + getDistancesToModel (const Eigen::VectorXf &model_coefficients, + std::vector &distances) const; /** \brief Compute all distances from the cloud data to a given 2D circle model. * \param[in] model_coefficients the coefficients of a 2D circle model that we need to compute distances to @@ -158,8 +157,8 @@ namespace pcl * \return the resultant number of inliers */ virtual int - countWithinDistance (const Eigen::VectorXf &model_coefficients, - const double threshold); + countWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold) const; /** \brief Recompute the 2d circle coefficients using the given inlier set and return them to the user. * @note: these are the coefficients of the 2d circle model after refinement (e.g. after SVD) @@ -167,10 +166,10 @@ namespace pcl * \param[in] model_coefficients the initial guess for the optimization * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization */ - void - optimizeModelCoefficients (const std::vector &inliers, - const Eigen::VectorXf &model_coefficients, - Eigen::VectorXf &optimized_coefficients); + void + optimizeModelCoefficients (const std::vector &inliers, + const Eigen::VectorXf &model_coefficients, + Eigen::VectorXf &optimized_coefficients) const; /** \brief Create a new point cloud with inliers projected onto the 2d circle model. * \param[in] inliers the data inliers that we want to project on the 2d circle model @@ -178,21 +177,21 @@ namespace pcl * \param[out] projected_points the resultant projected points * \param[in] copy_data_fields set to true if we need to copy the other data fields */ - void - projectPoints (const std::vector &inliers, - const Eigen::VectorXf &model_coefficients, - PointCloud &projected_points, - bool copy_data_fields = true); + void + projectPoints (const std::vector &inliers, + const Eigen::VectorXf &model_coefficients, + PointCloud &projected_points, + bool copy_data_fields = true) const; /** \brief Verify whether a subset of indices verifies the given 2d circle model coefficients. * \param[in] indices the data indices that need to be tested against the 2d circle model * \param[in] model_coefficients the 2d circle model coefficients * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers */ - bool - doSamplesVerifyModel (const std::set &indices, - const Eigen::VectorXf &model_coefficients, - const double threshold); + bool + doSamplesVerifyModel (const std::set &indices, + const Eigen::VectorXf &model_coefficients, + const double threshold) const; /** \brief Return an unique id for this model (SACMODEL_CIRCLE2D). */ inline pcl::SacModel @@ -206,7 +205,7 @@ namespace pcl * \param[in] model_coefficients the set of model coefficients */ virtual bool - isModelValid (const Eigen::VectorXf &model_coefficients); + isModelValid (const Eigen::VectorXf &model_coefficients) const; /** \brief Check if a sample of indices results in a good sample of points indices. * \param[in] samples the resultant index samples @@ -215,8 +214,6 @@ namespace pcl isSampleGood(const std::vector &samples) const; private: - /** \brief Temporary pointer to a list of given indices for optimizeModelCoefficients () */ - const std::vector *tmp_inliers_; #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3 #pragma GCC diagnostic ignored "-Weffc++" @@ -225,12 +222,11 @@ namespace pcl struct OptimizationFunctor : pcl::Functor { /** \brief Functor constructor - * \param[in] m_data_points the number of data points to evaluate + * \param[in] indices the indices of data points to evaluate * \param[in] estimator pointer to the estimator object - * \param[in] distance distance computation function pointer */ - OptimizationFunctor (int m_data_points, pcl::SampleConsensusModelCircle2D *model) : - pcl::Functor(m_data_points), model_ (model) {} + OptimizationFunctor (const pcl::SampleConsensusModelCircle2D *model, const std::vector& indices) : + pcl::Functor (indices.size ()), model_ (model), indices_ (indices) {} /** Cost function to be minimized * \param[in] x the variables array @@ -243,16 +239,17 @@ namespace pcl for (int i = 0; i < values (); ++i) { // Compute the difference between the center of the circle and the datapoint X_i - float xt = model_->input_->points[(*model_->tmp_inliers_)[i]].x - x[0]; - float yt = model_->input_->points[(*model_->tmp_inliers_)[i]].y - x[1]; - + float xt = model_->input_->points[indices_[i]].x - x[0]; + float yt = model_->input_->points[indices_[i]].y - x[1]; + // g = sqrt ((x-a)^2 + (y-b)^2) - R fvec[i] = std::sqrt (xt * xt + yt * yt) - x[2]; } return (0); } - pcl::SampleConsensusModelCircle2D *model_; + const pcl::SampleConsensusModelCircle2D *model_; + const std::vector &indices_; }; #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3 #pragma GCC diagnostic warning "-Weffc++" diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_circle3d.h b/sample_consensus/include/pcl/sample_consensus/sac_model_circle3d.h index 54dddd5f18c..af50086fc1e 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_circle3d.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_circle3d.h @@ -109,7 +109,7 @@ namespace pcl * \param[in] source the model to copy into this */ SampleConsensusModelCircle3D (const SampleConsensusModelCircle3D &source) : - SampleConsensusModel (), tmp_inliers_ () + SampleConsensusModel () { *this = source; model_name_ = "SampleConsensusModelCircle3D"; @@ -122,7 +122,6 @@ namespace pcl operator = (const SampleConsensusModelCircle3D &source) { SampleConsensusModel::operator=(source); - tmp_inliers_ = source.tmp_inliers_; return (*this); } @@ -133,7 +132,7 @@ namespace pcl */ bool computeModelCoefficients (const std::vector &samples, - Eigen::VectorXf &model_coefficients); + Eigen::VectorXf &model_coefficients) const; /** \brief Compute all distances from the cloud data to a given 3D circle model. * \param[in] model_coefficients the coefficients of a 2D circle model that we need to compute distances to @@ -141,7 +140,7 @@ namespace pcl */ void getDistancesToModel (const Eigen::VectorXf &model_coefficients, - std::vector &distances); + std::vector &distances) const; /** \brief Compute all distances from the cloud data to a given 3D circle model. * \param[in] model_coefficients the coefficients of a 3D circle model that we need to compute distances to @@ -161,7 +160,7 @@ namespace pcl */ virtual int countWithinDistance (const Eigen::VectorXf &model_coefficients, - const double threshold); + const double threshold) const; /** \brief Recompute the 3d circle coefficients using the given inlier set and return them to the user. * @note: these are the coefficients of the 3d circle model after refinement (e.g. after SVD) @@ -172,7 +171,7 @@ namespace pcl void optimizeModelCoefficients (const std::vector &inliers, const Eigen::VectorXf &model_coefficients, - Eigen::VectorXf &optimized_coefficients); + Eigen::VectorXf &optimized_coefficients) const; /** \brief Create a new point cloud with inliers projected onto the 3d circle model. * \param[in] inliers the data inliers that we want to project on the 3d circle model @@ -184,7 +183,7 @@ namespace pcl projectPoints (const std::vector &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, - bool copy_data_fields = true); + bool copy_data_fields = true) const; /** \brief Verify whether a subset of indices verifies the given 3d circle model coefficients. * \param[in] indices the data indices that need to be tested against the 3d circle model @@ -194,7 +193,7 @@ namespace pcl bool doSamplesVerifyModel (const std::set &indices, const Eigen::VectorXf &model_coefficients, - const double threshold); + const double threshold) const; /** \brief Return an unique id for this model (SACMODEL_CIRCLE3D). */ inline pcl::SacModel @@ -208,7 +207,7 @@ namespace pcl * \param[in] model_coefficients the set of model coefficients */ virtual bool - isModelValid (const Eigen::VectorXf &model_coefficients); + isModelValid (const Eigen::VectorXf &model_coefficients) const; /** \brief Check if a sample of indices results in a good sample of points indices. * \param[in] samples the resultant index samples @@ -217,19 +216,15 @@ namespace pcl isSampleGood(const std::vector &samples) const; private: - /** \brief Temporary pointer to a list of given indices for optimizeModelCoefficients () */ - const std::vector *tmp_inliers_; - /** \brief Functor for the optimization function */ struct OptimizationFunctor : pcl::Functor { /** Functor constructor - * \param[in] m_data_points the number of functions - * \param[in] estimator pointer to the estimator object - * \param[in] distance distance computation function pointer - */ - OptimizationFunctor (int m_data_points, pcl::SampleConsensusModelCircle3D *model) : - pcl::Functor (m_data_points), model_ (model) {} + * \param[in] indices the indices of data points to evaluate + * \param[in] estimator pointer to the estimator object + */ + OptimizationFunctor (const pcl::SampleConsensusModelCircle3D *model, const std::vector& indices) : + pcl::Functor (indices.size ()), model_ (model), indices_ (indices) {} /** Cost function to be minimized * \param[in] x the variables array @@ -242,7 +237,7 @@ namespace pcl { // what i have: // P : Sample Point - Eigen::Vector3d P (model_->input_->points[(*model_->tmp_inliers_)[i]].x, model_->input_->points[(*model_->tmp_inliers_)[i]].y, model_->input_->points[(*model_->tmp_inliers_)[i]].z); + Eigen::Vector3d P (model_->input_->points[indices_[i]].x, model_->input_->points[indices_[i]].y, model_->input_->points[indices_[i]].z); // C : Circle Center Eigen::Vector3d C (x[0], x[1], x[2]); // N : Circle (Plane) Normal @@ -267,7 +262,8 @@ namespace pcl return (0); } - pcl::SampleConsensusModelCircle3D *model_; + const pcl::SampleConsensusModelCircle3D *model_; + const std::vector &indices_; }; }; } 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 0f9f64fa9d4..187083064b0 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_cone.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_cone.h @@ -91,7 +91,6 @@ namespace pcl , eps_angle_ (0) , min_angle_ (-std::numeric_limits::max ()) , max_angle_ (std::numeric_limits::max ()) - , tmp_inliers_ () { model_name_ = "SampleConsensusModelCone"; sample_size_ = 3; @@ -112,7 +111,6 @@ namespace pcl , eps_angle_ (0) , min_angle_ (-std::numeric_limits::max ()) , max_angle_ (std::numeric_limits::max ()) - , tmp_inliers_ () { model_name_ = "SampleConsensusModelCone"; sample_size_ = 3; @@ -125,7 +123,7 @@ namespace pcl SampleConsensusModelCone (const SampleConsensusModelCone &source) : SampleConsensusModel (), SampleConsensusModelFromNormals (), - axis_ (), eps_angle_ (), min_angle_ (), max_angle_ (), tmp_inliers_ () + axis_ (), eps_angle_ (), min_angle_ (), max_angle_ () { *this = source; model_name_ = "SampleConsensusModelCone"; @@ -146,7 +144,6 @@ namespace pcl eps_angle_ = source.eps_angle_; min_angle_ = source.min_angle_; max_angle_ = source.max_angle_; - tmp_inliers_ = source.tmp_inliers_; return (*this); } @@ -199,17 +196,17 @@ namespace pcl * \param[in] samples the point indices found as possible good candidates for creating a valid model * \param[out] model_coefficients the resultant model coefficients */ - bool - computeModelCoefficients (const std::vector &samples, - Eigen::VectorXf &model_coefficients); + bool + computeModelCoefficients (const std::vector &samples, + Eigen::VectorXf &model_coefficients) const; /** \brief Compute all distances from the cloud data to a given cone model. * \param[in] model_coefficients the coefficients of a cone model that we need to compute distances to * \param[out] distances the resultant estimated distances */ - void - getDistancesToModel (const Eigen::VectorXf &model_coefficients, - std::vector &distances); + void + getDistancesToModel (const Eigen::VectorXf &model_coefficients, + std::vector &distances) const; /** \brief Select all the points which respect the given model coefficients as inliers. * \param[in] model_coefficients the coefficients of a cone model that we need to compute distances to @@ -228,8 +225,8 @@ namespace pcl * \return the resultant number of inliers */ virtual int - countWithinDistance (const Eigen::VectorXf &model_coefficients, - const double threshold); + countWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold) const; /** \brief Recompute the cone coefficients using the given inlier set and return them to the user. @@ -238,10 +235,10 @@ namespace pcl * \param[in] model_coefficients the initial guess for the optimization * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization */ - void - optimizeModelCoefficients (const std::vector &inliers, - const Eigen::VectorXf &model_coefficients, - Eigen::VectorXf &optimized_coefficients); + void + optimizeModelCoefficients (const std::vector &inliers, + const Eigen::VectorXf &model_coefficients, + Eigen::VectorXf &optimized_coefficients) const; /** \brief Create a new point cloud with inliers projected onto the cone model. @@ -250,21 +247,21 @@ namespace pcl * \param[out] projected_points the resultant projected points * \param[in] copy_data_fields set to true if we need to copy the other data fields */ - void - projectPoints (const std::vector &inliers, - const Eigen::VectorXf &model_coefficients, - PointCloud &projected_points, - bool copy_data_fields = true); + void + projectPoints (const std::vector &inliers, + const Eigen::VectorXf &model_coefficients, + PointCloud &projected_points, + bool copy_data_fields = true) const; /** \brief Verify whether a subset of indices verifies the given cone model coefficients. * \param[in] indices the data indices that need to be tested against the cone model * \param[in] model_coefficients the cone model coefficients * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers */ - bool - doSamplesVerifyModel (const std::set &indices, - const Eigen::VectorXf &model_coefficients, - const double threshold); + bool + doSamplesVerifyModel (const std::set &indices, + const Eigen::VectorXf &model_coefficients, + const double threshold) const; /** \brief Return an unique id for this model (SACMODEL_CONE). */ inline pcl::SacModel @@ -278,14 +275,14 @@ namespace pcl * \param[in] pt a point * \param[in] model_coefficients the line coefficients (a point on the line, line direction) */ - double - pointToAxisDistance (const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients); + double + pointToAxisDistance (const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients) const; /** \brief Check whether a model is valid given the user constraints. * \param[in] model_coefficients the set of model coefficients */ virtual bool - isModelValid (const Eigen::VectorXf &model_coefficients); + isModelValid (const Eigen::VectorXf &model_coefficients) const; /** \brief Check if a sample of indices results in a good sample of points * indices. Pure virtual. @@ -305,9 +302,6 @@ namespace pcl double min_angle_; double max_angle_; - /** \brief temporary pointer to a list of given indices for optimizeModelCoefficients () */ - const std::vector *tmp_inliers_; - #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3 #pragma GCC diagnostic ignored "-Weffc++" #endif @@ -315,12 +309,11 @@ namespace pcl struct OptimizationFunctor : pcl::Functor { /** Functor constructor - * \param[in] m_data_points the number of data points to evaluate + * \param[in] indices the indices of data points to evaluate * \param[in] estimator pointer to the estimator object - * \param[in] distance distance computation function pointer */ - OptimizationFunctor (int m_data_points, pcl::SampleConsensusModelCone *model) : - pcl::Functor (m_data_points), model_ (model) {} + OptimizationFunctor (const pcl::SampleConsensusModelCone *model, const std::vector& indices) : + pcl::Functor (indices.size ()), model_ (model), indices_ (indices) {} /** Cost function to be minimized * \param[in] x variables array @@ -340,9 +333,9 @@ namespace pcl for (int i = 0; i < values (); ++i) { // dist = f - r - Eigen::Vector4f pt (model_->input_->points[(*model_->tmp_inliers_)[i]].x, - model_->input_->points[(*model_->tmp_inliers_)[i]].y, - model_->input_->points[(*model_->tmp_inliers_)[i]].z, 0); + Eigen::Vector4f pt (model_->input_->points[indices_[i]].x, + model_->input_->points[indices_[i]].y, + model_->input_->points[indices_[i]].z, 0); // Calculate the point's projection on the cone axis float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir; @@ -357,7 +350,8 @@ namespace pcl return (0); } - pcl::SampleConsensusModelCone *model_; + const pcl::SampleConsensusModelCone *model_; + const std::vector &indices_; }; #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3 #pragma GCC diagnostic warning "-Weffc++" 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 d7f09aaa1ad..c347c00f0fd 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_cylinder.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_cylinder.h @@ -89,7 +89,6 @@ namespace pcl , SampleConsensusModelFromNormals () , axis_ (Eigen::Vector3f::Zero ()) , eps_angle_ (0) - , tmp_inliers_ () { model_name_ = "SampleConsensusModelCylinder"; sample_size_ = 2; @@ -108,7 +107,6 @@ namespace pcl , SampleConsensusModelFromNormals () , axis_ (Eigen::Vector3f::Zero ()) , eps_angle_ (0) - , tmp_inliers_ () { model_name_ = "SampleConsensusModelCylinder"; sample_size_ = 2; @@ -122,8 +120,7 @@ namespace pcl SampleConsensusModel (), SampleConsensusModelFromNormals (), axis_ (Eigen::Vector3f::Zero ()), - eps_angle_ (0), - tmp_inliers_ () + eps_angle_ (0) { *this = source; model_name_ = "SampleConsensusModelCylinder"; @@ -142,7 +139,6 @@ namespace pcl SampleConsensusModelFromNormals::operator=(source); axis_ = source.axis_; eps_angle_ = source.eps_angle_; - tmp_inliers_ = source.tmp_inliers_; return (*this); } @@ -172,17 +168,17 @@ namespace pcl * \param[in] samples the point indices found as possible good candidates for creating a valid model * \param[out] model_coefficients the resultant model coefficients */ - bool - computeModelCoefficients (const std::vector &samples, - Eigen::VectorXf &model_coefficients); + bool + computeModelCoefficients (const std::vector &samples, + Eigen::VectorXf &model_coefficients) const; /** \brief Compute all distances from the cloud data to a given cylinder model. * \param[in] model_coefficients the coefficients of a cylinder model that we need to compute distances to * \param[out] distances the resultant estimated distances */ - void - getDistancesToModel (const Eigen::VectorXf &model_coefficients, - std::vector &distances); + void + getDistancesToModel (const Eigen::VectorXf &model_coefficients, + std::vector &distances) const; /** \brief Select all the points which respect the given model coefficients as inliers. * \param[in] model_coefficients the coefficients of a cylinder model that we need to compute distances to @@ -201,8 +197,8 @@ namespace pcl * \return the resultant number of inliers */ virtual int - countWithinDistance (const Eigen::VectorXf &model_coefficients, - const double threshold); + countWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold) const; /** \brief Recompute the cylinder coefficients using the given inlier set and return them to the user. * @note: these are the coefficients of the cylinder model after refinement (e.g. after SVD) @@ -210,10 +206,10 @@ namespace pcl * \param[in] model_coefficients the initial guess for the optimization * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization */ - void - optimizeModelCoefficients (const std::vector &inliers, - const Eigen::VectorXf &model_coefficients, - Eigen::VectorXf &optimized_coefficients); + void + optimizeModelCoefficients (const std::vector &inliers, + const Eigen::VectorXf &model_coefficients, + Eigen::VectorXf &optimized_coefficients) const; /** \brief Create a new point cloud with inliers projected onto the cylinder model. @@ -222,21 +218,21 @@ namespace pcl * \param[out] projected_points the resultant projected points * \param[in] copy_data_fields set to true if we need to copy the other data fields */ - void - projectPoints (const std::vector &inliers, - const Eigen::VectorXf &model_coefficients, - PointCloud &projected_points, - bool copy_data_fields = true); + void + projectPoints (const std::vector &inliers, + const Eigen::VectorXf &model_coefficients, + PointCloud &projected_points, + bool copy_data_fields = true) const; /** \brief Verify whether a subset of indices verifies the given cylinder model coefficients. * \param[in] indices the data indices that need to be tested against the cylinder model * \param[in] model_coefficients the cylinder model coefficients * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers */ - bool - doSamplesVerifyModel (const std::set &indices, - const Eigen::VectorXf &model_coefficients, - const double threshold); + bool + doSamplesVerifyModel (const std::set &indices, + const Eigen::VectorXf &model_coefficients, + const double threshold) const; /** \brief Return an unique id for this model (SACMODEL_CYLINDER). */ inline pcl::SacModel @@ -250,8 +246,8 @@ namespace pcl * \param[in] pt a point * \param[in] model_coefficients the line coefficients (a point on the line, line direction) */ - double - pointToLineDistance (const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients); + double + pointToLineDistance (const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients) const; /** \brief Project a point onto a line given by a point and a direction vector * \param[in] pt the input point to project @@ -260,10 +256,10 @@ namespace pcl * \param[out] pt_proj the resultant projected point */ inline void - projectPointToLine (const Eigen::Vector4f &pt, - const Eigen::Vector4f &line_pt, + projectPointToLine (const Eigen::Vector4f &pt, + const Eigen::Vector4f &line_pt, const Eigen::Vector4f &line_dir, - Eigen::Vector4f &pt_proj) + Eigen::Vector4f &pt_proj) const { float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir); // Calculate the projection of the point on the line @@ -276,16 +272,16 @@ namespace pcl * \param[in] model_coefficients the coefficients of the cylinder (point_on_axis, axis_direction, cylinder_radius_R) * \param[out] pt_proj the resultant projected point */ - void - projectPointToCylinder (const Eigen::Vector4f &pt, - const Eigen::VectorXf &model_coefficients, - Eigen::Vector4f &pt_proj); + void + projectPointToCylinder (const Eigen::Vector4f &pt, + const Eigen::VectorXf &model_coefficients, + Eigen::Vector4f &pt_proj) const; /** \brief Check whether a model is valid given the user constraints. * \param[in] model_coefficients the set of model coefficients */ virtual bool - isModelValid (const Eigen::VectorXf &model_coefficients); + isModelValid (const Eigen::VectorXf &model_coefficients) const; /** \brief Check if a sample of indices results in a good sample of points * indices. Pure virtual. @@ -301,9 +297,6 @@ namespace pcl /** \brief The maximum allowed difference between the plane normal and the given axis. */ double eps_angle_; - /** \brief temporary pointer to a list of given indices for optimizeModelCoefficients () */ - const std::vector *tmp_inliers_; - #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3 #pragma GCC diagnostic ignored "-Weffc++" #endif @@ -311,12 +304,11 @@ namespace pcl struct OptimizationFunctor : pcl::Functor { /** Functor constructor - * \param[in] m_data_points the number of data points to evaluate + * \param[in] indices the indices of data points to evaluate * \param[in] estimator pointer to the estimator object - * \param[in] distance distance computation function pointer */ - OptimizationFunctor (int m_data_points, pcl::SampleConsensusModelCylinder *model) : - pcl::Functor (m_data_points), model_ (model) {} + OptimizationFunctor (const pcl::SampleConsensusModelCylinder *model, const std::vector& indices) : + pcl::Functor (indices.size ()), model_ (model), indices_ (indices) {} /** Cost function to be minimized * \param[in] x variables array @@ -332,16 +324,17 @@ namespace pcl for (int i = 0; i < values (); ++i) { // dist = f - r - Eigen::Vector4f pt (model_->input_->points[(*model_->tmp_inliers_)[i]].x, - model_->input_->points[(*model_->tmp_inliers_)[i]].y, - model_->input_->points[(*model_->tmp_inliers_)[i]].z, 0); + Eigen::Vector4f pt (model_->input_->points[indices_[i]].x, + model_->input_->points[indices_[i]].y, + model_->input_->points[indices_[i]].z, 0); fvec[i] = static_cast (pcl::sqrPointToLineDistance (pt, line_pt, line_dir) - x[6]*x[6]); } return (0); } - pcl::SampleConsensusModelCylinder *model_; + const pcl::SampleConsensusModelCylinder *model_; + const std::vector &indices_; }; #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3 #pragma GCC diagnostic warning "-Weffc++" diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_line.h b/sample_consensus/include/pcl/sample_consensus/sac_model_line.h index 07eef59067b..7b383465294 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_line.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_line.h @@ -111,17 +111,17 @@ namespace pcl * \param[in] samples the point indices found as possible good candidates for creating a valid model * \param[out] model_coefficients the resultant model coefficients */ - bool - computeModelCoefficients (const std::vector &samples, - Eigen::VectorXf &model_coefficients); + bool + computeModelCoefficients (const std::vector &samples, + Eigen::VectorXf &model_coefficients) const; /** \brief Compute all squared distances from the cloud data to a given line model. * \param[in] model_coefficients the coefficients of a line model that we need to compute distances to * \param[out] distances the resultant estimated squared distances */ - void - getDistancesToModel (const Eigen::VectorXf &model_coefficients, - std::vector &distances); + void + getDistancesToModel (const Eigen::VectorXf &model_coefficients, + std::vector &distances) const; /** \brief Select all the points which respect the given model coefficients as inliers. * \param[in] model_coefficients the coefficients of a line model that we need to compute distances to @@ -140,8 +140,8 @@ namespace pcl * \return the resultant number of inliers */ virtual int - countWithinDistance (const Eigen::VectorXf &model_coefficients, - const double threshold); + countWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold) const; /** \brief Recompute the line coefficients using the given inlier set and return them to the user. * @note: these are the coefficients of the line model after refinement (e.g. after SVD) @@ -149,10 +149,10 @@ namespace pcl * \param[in] model_coefficients the initial guess for the model coefficients * \param[out] optimized_coefficients the resultant recomputed coefficients after optimization */ - void - optimizeModelCoefficients (const std::vector &inliers, - const Eigen::VectorXf &model_coefficients, - Eigen::VectorXf &optimized_coefficients); + void + optimizeModelCoefficients (const std::vector &inliers, + const Eigen::VectorXf &model_coefficients, + Eigen::VectorXf &optimized_coefficients) const; /** \brief Create a new point cloud with inliers projected onto the line model. * \param[in] inliers the data inliers that we want to project on the line model @@ -160,21 +160,21 @@ namespace pcl * \param[out] projected_points the resultant projected points * \param[in] copy_data_fields set to true if we need to copy the other data fields */ - void - projectPoints (const std::vector &inliers, - const Eigen::VectorXf &model_coefficients, - PointCloud &projected_points, - bool copy_data_fields = true); + void + projectPoints (const std::vector &inliers, + const Eigen::VectorXf &model_coefficients, + PointCloud &projected_points, + bool copy_data_fields = true) const; /** \brief Verify whether a subset of indices verifies the given line model coefficients. * \param[in] indices the data indices that need to be tested against the line model * \param[in] model_coefficients the line model coefficients * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers */ - bool - doSamplesVerifyModel (const std::set &indices, - const Eigen::VectorXf &model_coefficients, - const double threshold); + bool + doSamplesVerifyModel (const std::set &indices, + const Eigen::VectorXf &model_coefficients, + const double threshold) const; /** \brief Return an unique id for this model (SACMODEL_LINE). */ inline pcl::SacModel diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_normal_parallel_plane.h b/sample_consensus/include/pcl/sample_consensus/sac_model_normal_parallel_plane.h index 2b29b667655..b828bbd5a77 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_normal_parallel_plane.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_normal_parallel_plane.h @@ -196,7 +196,7 @@ namespace pcl * \param[in] model_coefficients the set of model coefficients */ virtual bool - isModelValid (const Eigen::VectorXf &model_coefficients); + isModelValid (const Eigen::VectorXf &model_coefficients) const; private: /** \brief The axis along which we need to search for a plane perpendicular to. */ diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_normal_plane.h b/sample_consensus/include/pcl/sample_consensus/sac_model_normal_plane.h index 1e762ba3ba8..509f94a7933 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_normal_plane.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_normal_plane.h @@ -143,16 +143,16 @@ namespace pcl * \return the resultant number of inliers */ virtual int - countWithinDistance (const Eigen::VectorXf &model_coefficients, - const double threshold); + countWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold) const; /** \brief Compute all distances from the cloud data to a given plane model. * \param[in] model_coefficients the coefficients of a plane model that we need to compute distances to * \param[out] distances the resultant estimated distances */ - void - getDistancesToModel (const Eigen::VectorXf &model_coefficients, - std::vector &distances); + void + getDistancesToModel (const Eigen::VectorXf &model_coefficients, + std::vector &distances) const; /** \brief Return an unique id for this model (SACMODEL_NORMAL_PLANE). */ inline pcl::SacModel diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_normal_sphere.h b/sample_consensus/include/pcl/sample_consensus/sac_model_normal_sphere.h index 9a40dbf69a1..ef5f27e5796 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_normal_sphere.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_normal_sphere.h @@ -136,16 +136,16 @@ namespace pcl * \return the resultant number of inliers */ virtual int - countWithinDistance (const Eigen::VectorXf &model_coefficients, - const double threshold); + countWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold) const; /** \brief Compute all distances from the cloud data to a given sphere model. * \param[in] model_coefficients the coefficients of a sphere model that we need to compute distances to * \param[out] distances the resultant estimated distances */ - void - getDistancesToModel (const Eigen::VectorXf &model_coefficients, - std::vector &distances); + void + getDistancesToModel (const Eigen::VectorXf &model_coefficients, + std::vector &distances) const; /** \brief Return an unique id for this model (SACMODEL_NORMAL_SPHERE). */ inline pcl::SacModel @@ -161,7 +161,7 @@ namespace pcl * \param[in] model_coefficients the set of model coefficients */ virtual bool - isModelValid (const Eigen::VectorXf &model_coefficients); + isModelValid (const Eigen::VectorXf &model_coefficients) const; }; } diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_parallel_line.h b/sample_consensus/include/pcl/sample_consensus/sac_model_parallel_line.h index 72fda46b810..afff15c1690 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_parallel_line.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_parallel_line.h @@ -146,7 +146,7 @@ namespace pcl */ virtual int countWithinDistance (const Eigen::VectorXf &model_coefficients, - const double threshold); + const double threshold) const; /** \brief Compute all squared distances from the cloud data to a given line model. * \param[in] model_coefficients the coefficients of a line model that we need to compute distances to @@ -154,7 +154,7 @@ namespace pcl */ void getDistancesToModel (const Eigen::VectorXf &model_coefficients, - std::vector &distances); + std::vector &distances) const; /** \brief Return an unique id for this model (SACMODEL_PARALLEL_LINE). */ inline pcl::SacModel @@ -168,7 +168,7 @@ namespace pcl * \param[in] model_coefficients the set of model coefficients */ virtual bool - isModelValid (const Eigen::VectorXf &model_coefficients); + isModelValid (const Eigen::VectorXf &model_coefficients) const; /** \brief The axis along which we need to search for a line. */ Eigen::Vector3f axis_; diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_parallel_plane.h b/sample_consensus/include/pcl/sample_consensus/sac_model_parallel_plane.h index 7e681b1b9bf..4807be6c935 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_parallel_plane.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_parallel_plane.h @@ -150,7 +150,7 @@ namespace pcl */ virtual int countWithinDistance (const Eigen::VectorXf &model_coefficients, - const double threshold); + const double threshold) const; /** \brief Compute all distances from the cloud data to a given plane model. * \param[in] model_coefficients the coefficients of a plane model that we need to compute distances to @@ -158,7 +158,7 @@ namespace pcl */ void getDistancesToModel (const Eigen::VectorXf &model_coefficients, - std::vector &distances); + std::vector &distances) const; /** \brief Return an unique id for this model (SACMODEL_PARALLEL_PLANE). */ inline pcl::SacModel @@ -172,7 +172,7 @@ namespace pcl * \param[in] model_coefficients the set of model coefficients */ virtual bool - isModelValid (const Eigen::VectorXf &model_coefficients); + isModelValid (const Eigen::VectorXf &model_coefficients) const; /** \brief The axis along which we need to search for a plane perpendicular to. */ Eigen::Vector3f axis_; diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_perpendicular_plane.h b/sample_consensus/include/pcl/sample_consensus/sac_model_perpendicular_plane.h index 43e7d0e1c94..91ff7bcf8a1 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_perpendicular_plane.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_perpendicular_plane.h @@ -152,16 +152,16 @@ namespace pcl * \return the resultant number of inliers */ virtual int - countWithinDistance (const Eigen::VectorXf &model_coefficients, - const double threshold); + countWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold) const; /** \brief Compute all distances from the cloud data to a given plane model. * \param[in] model_coefficients the coefficients of a plane model that we need to compute distances to * \param[out] distances the resultant estimated distances */ - void - getDistancesToModel (const Eigen::VectorXf &model_coefficients, - std::vector &distances); + void + getDistancesToModel (const Eigen::VectorXf &model_coefficients, + std::vector &distances) const; /** \brief Return an unique id for this model (SACMODEL_PERPENDICULAR_PLANE). */ inline pcl::SacModel @@ -175,7 +175,7 @@ namespace pcl * \param[in] model_coefficients the set of model coefficients */ virtual bool - isModelValid (const Eigen::VectorXf &model_coefficients); + isModelValid (const Eigen::VectorXf &model_coefficients) const; /** \brief The axis along which we need to search for a plane perpendicular to. */ Eigen::Vector3f axis_; diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_plane.h b/sample_consensus/include/pcl/sample_consensus/sac_model_plane.h index 4ec78d91b47..e95d422415c 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_plane.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_plane.h @@ -184,17 +184,17 @@ namespace pcl * \param[in] samples the point indices found as possible good candidates for creating a valid model * \param[out] model_coefficients the resultant model coefficients */ - bool - computeModelCoefficients (const std::vector &samples, - Eigen::VectorXf &model_coefficients); + bool + computeModelCoefficients (const std::vector &samples, + Eigen::VectorXf &model_coefficients) const; /** \brief Compute all distances from the cloud data to a given plane model. * \param[in] model_coefficients the coefficients of a plane model that we need to compute distances to * \param[out] distances the resultant estimated distances */ - void - getDistancesToModel (const Eigen::VectorXf &model_coefficients, - std::vector &distances); + void + getDistancesToModel (const Eigen::VectorXf &model_coefficients, + std::vector &distances) const; /** \brief Select all the points which respect the given model coefficients as inliers. * \param[in] model_coefficients the coefficients of a plane model that we need to compute distances to @@ -213,8 +213,8 @@ namespace pcl * \return the resultant number of inliers */ virtual int - countWithinDistance (const Eigen::VectorXf &model_coefficients, - const double threshold); + countWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold) const; /** \brief Recompute the plane coefficients using the given inlier set and return them to the user. * @note: these are the coefficients of the plane model after refinement (e.g. after SVD) @@ -222,10 +222,10 @@ namespace pcl * \param[in] model_coefficients the initial guess for the model coefficients * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization */ - void - optimizeModelCoefficients (const std::vector &inliers, - const Eigen::VectorXf &model_coefficients, - Eigen::VectorXf &optimized_coefficients); + void + optimizeModelCoefficients (const std::vector &inliers, + const Eigen::VectorXf &model_coefficients, + Eigen::VectorXf &optimized_coefficients) const; /** \brief Create a new point cloud with inliers projected onto the plane model. * \param[in] inliers the data inliers that we want to project on the plane model @@ -233,21 +233,21 @@ namespace pcl * \param[out] projected_points the resultant projected points * \param[in] copy_data_fields set to true if we need to copy the other data fields */ - void - projectPoints (const std::vector &inliers, - const Eigen::VectorXf &model_coefficients, - PointCloud &projected_points, - bool copy_data_fields = true); + void + projectPoints (const std::vector &inliers, + const Eigen::VectorXf &model_coefficients, + PointCloud &projected_points, + bool copy_data_fields = true) const; /** \brief Verify whether a subset of indices verifies the given plane model coefficients. * \param[in] indices the data indices that need to be tested against the plane model * \param[in] model_coefficients the plane model coefficients * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers */ - bool - doSamplesVerifyModel (const std::set &indices, - const Eigen::VectorXf &model_coefficients, - const double threshold); + bool + doSamplesVerifyModel (const std::set &indices, + const Eigen::VectorXf &model_coefficients, + const double threshold) const; /** \brief Return an unique id for this model (SACMODEL_PLANE). */ inline pcl::SacModel diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_registration.h b/sample_consensus/include/pcl/sample_consensus/sac_model_registration.h index 6da4d57725f..5d88cca3294 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_registration.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_registration.h @@ -159,7 +159,7 @@ namespace pcl */ bool computeModelCoefficients (const std::vector &samples, - Eigen::VectorXf &model_coefficients); + Eigen::VectorXf &model_coefficients) const; /** \brief Compute all distances from the transformed points to their correspondences * \param[in] model_coefficients the 4x4 transformation matrix @@ -167,7 +167,7 @@ namespace pcl */ void getDistancesToModel (const Eigen::VectorXf &model_coefficients, - std::vector &distances); + std::vector &distances) const; /** \brief Select all the points which respect the given model coefficients as inliers. * \param[in] model_coefficients the 4x4 transformation matrix @@ -187,7 +187,7 @@ namespace pcl */ virtual int countWithinDistance (const Eigen::VectorXf &model_coefficients, - const double threshold); + const double threshold) const; /** \brief Recompute the 4x4 transformation using the given inlier set * \param[in] inliers the data inliers found as supporting the model @@ -197,19 +197,19 @@ namespace pcl void optimizeModelCoefficients (const std::vector &inliers, const Eigen::VectorXf &model_coefficients, - Eigen::VectorXf &optimized_coefficients); + Eigen::VectorXf &optimized_coefficients) const; void projectPoints (const std::vector &, const Eigen::VectorXf &, - PointCloud &, bool = true) + PointCloud &, bool = true) const { }; bool doSamplesVerifyModel (const std::set &, const Eigen::VectorXf &, - const double) + const double) const { return (false); } @@ -302,7 +302,7 @@ namespace pcl const std::vector &indices_src, const pcl::PointCloud &cloud_tgt, const std::vector &indices_tgt, - Eigen::VectorXf &transform); + Eigen::VectorXf &transform) const; /** \brief Compute mappings between original indices of the input_/target_ clouds. */ void diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_registration_2d.h b/sample_consensus/include/pcl/sample_consensus/sac_model_registration_2d.h index c64a5483520..1268d63a46e 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_registration_2d.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_registration_2d.h @@ -112,7 +112,7 @@ namespace pcl */ void getDistancesToModel (const Eigen::VectorXf &model_coefficients, - std::vector &distances); + std::vector &distances) const; /** \brief Select all the points which respect the given model coefficients as inliers. * \param[in] model_coefficients the 4x4 transformation matrix @@ -132,7 +132,7 @@ namespace pcl */ virtual int countWithinDistance (const Eigen::VectorXf &model_coefficients, - const double threshold); + const double threshold) const; /** \brief Set the camera projection matrix. * \param[in] projection_matrix the camera projection matrix 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 d386aa74de2..330de6df66f 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_sphere.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_sphere.h @@ -78,8 +78,8 @@ namespace pcl * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) */ SampleConsensusModelSphere (const PointCloudConstPtr &cloud, - bool random = false) - : SampleConsensusModel (cloud, random), tmp_inliers_ () + bool random = false) + : SampleConsensusModel (cloud, random) { model_name_ = "SampleConsensusModelSphere"; sample_size_ = 4; @@ -91,10 +91,10 @@ namespace pcl * \param[in] indices a vector of point indices to be used from \a cloud * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false) */ - SampleConsensusModelSphere (const PointCloudConstPtr &cloud, + SampleConsensusModelSphere (const PointCloudConstPtr &cloud, const std::vector &indices, - bool random = false) - : SampleConsensusModel (cloud, indices, random), tmp_inliers_ () + bool random = false) + : SampleConsensusModel (cloud, indices, random) { model_name_ = "SampleConsensusModelSphere"; sample_size_ = 4; @@ -108,7 +108,7 @@ namespace pcl * \param[in] source the model to copy into this */ SampleConsensusModelSphere (const SampleConsensusModelSphere &source) : - SampleConsensusModel (), tmp_inliers_ () + SampleConsensusModel () { *this = source; model_name_ = "SampleConsensusModelSphere"; @@ -121,7 +121,6 @@ namespace pcl operator = (const SampleConsensusModelSphere &source) { SampleConsensusModel::operator=(source); - tmp_inliers_ = source.tmp_inliers_; return (*this); } @@ -131,17 +130,17 @@ namespace pcl * \param[in] samples the point indices found as possible good candidates for creating a valid model * \param[out] model_coefficients the resultant model coefficients */ - bool - computeModelCoefficients (const std::vector &samples, - Eigen::VectorXf &model_coefficients); + bool + computeModelCoefficients (const std::vector &samples, + Eigen::VectorXf &model_coefficients) const; /** \brief Compute all distances from the cloud data to a given sphere model. * \param[in] model_coefficients the coefficients of a sphere model that we need to compute distances to * \param[out] distances the resultant estimated distances */ - void - getDistancesToModel (const Eigen::VectorXf &model_coefficients, - std::vector &distances); + void + getDistancesToModel (const Eigen::VectorXf &model_coefficients, + std::vector &distances) const; /** \brief Select all the points which respect the given model coefficients as inliers. * \param[in] model_coefficients the coefficients of a sphere model that we need to compute distances to @@ -160,8 +159,8 @@ namespace pcl * \return the resultant number of inliers */ virtual int - countWithinDistance (const Eigen::VectorXf &model_coefficients, - const double threshold); + countWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold) const; /** \brief Recompute the sphere coefficients using the given inlier set and return them to the user. * @note: these are the coefficients of the sphere model after refinement (e.g. after SVD) @@ -169,10 +168,10 @@ namespace pcl * \param[in] model_coefficients the initial guess for the optimization * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization */ - void - optimizeModelCoefficients (const std::vector &inliers, - const Eigen::VectorXf &model_coefficients, - Eigen::VectorXf &optimized_coefficients); + void + optimizeModelCoefficients (const std::vector &inliers, + const Eigen::VectorXf &model_coefficients, + Eigen::VectorXf &optimized_coefficients) const; /** \brief Create a new point cloud with inliers projected onto the sphere model. * \param[in] inliers the data inliers that we want to project on the sphere model @@ -181,21 +180,21 @@ namespace pcl * \param[in] copy_data_fields set to true if we need to copy the other data fields * \todo implement this. */ - void - projectPoints (const std::vector &inliers, - const Eigen::VectorXf &model_coefficients, - PointCloud &projected_points, - bool copy_data_fields = true); + void + projectPoints (const std::vector &inliers, + const Eigen::VectorXf &model_coefficients, + PointCloud &projected_points, + bool copy_data_fields = true) const; /** \brief Verify whether a subset of indices verifies the given sphere model coefficients. * \param[in] indices the data indices that need to be tested against the sphere model * \param[in] model_coefficients the sphere model coefficients * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers */ - bool - doSamplesVerifyModel (const std::set &indices, - const Eigen::VectorXf &model_coefficients, - const double threshold); + bool + doSamplesVerifyModel (const std::set &indices, + const Eigen::VectorXf &model_coefficients, + const double threshold) const; /** \brief Return an unique id for this model (SACMODEL_SPHERE). */ inline pcl::SacModel getModelType () const { return (SACMODEL_SPHERE); } @@ -208,7 +207,7 @@ namespace pcl * \param[in] model_coefficients the set of model coefficients */ virtual bool - isModelValid (const Eigen::VectorXf &model_coefficients) + isModelValid (const Eigen::VectorXf &model_coefficients) const { if (!SampleConsensusModel::isModelValid (model_coefficients)) return (false); @@ -229,8 +228,6 @@ namespace pcl isSampleGood(const std::vector &samples) const; private: - /** \brief Temporary pointer to a list of given indices for optimizeModelCoefficients () */ - const std::vector *tmp_inliers_; #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3 #pragma GCC diagnostic ignored "-Weffc++" @@ -238,12 +235,11 @@ namespace pcl struct OptimizationFunctor : pcl::Functor { /** Functor constructor - * \param[in] m_data_points the number of data points to evaluate + * \param[in] indices the indices of data points to evaluate * \param[in] estimator pointer to the estimator object - * \param[in] distance distance computation function pointer */ - OptimizationFunctor (int m_data_points, pcl::SampleConsensusModelSphere *model) : - pcl::Functor(m_data_points), model_ (model) {} + OptimizationFunctor (const pcl::SampleConsensusModelSphere *model, const std::vector& indices) : + pcl::Functor (indices.size ()), model_ (model), indices_ (indices) {} /** Cost function to be minimized * \param[in] x the variables array @@ -258,17 +254,18 @@ namespace pcl for (int i = 0; i < values (); ++i) { // Compute the difference between the center of the sphere and the datapoint X_i - cen_t[0] = model_->input_->points[(*model_->tmp_inliers_)[i]].x - x[0]; - cen_t[1] = model_->input_->points[(*model_->tmp_inliers_)[i]].y - x[1]; - cen_t[2] = model_->input_->points[(*model_->tmp_inliers_)[i]].z - x[2]; - + cen_t[0] = model_->input_->points[indices_[i]].x - x[0]; + cen_t[1] = model_->input_->points[indices_[i]].y - x[1]; + cen_t[2] = model_->input_->points[indices_[i]].z - x[2]; + // 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); } - - pcl::SampleConsensusModelSphere *model_; + + const pcl::SampleConsensusModelSphere *model_; + const std::vector &indices_; }; #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3 #pragma GCC diagnostic warning "-Weffc++" diff --git a/sample_consensus/include/pcl/sample_consensus/sac_model_stick.h b/sample_consensus/include/pcl/sample_consensus/sac_model_stick.h index 5c307ff6b32..3755d64e5f8 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_stick.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_stick.h @@ -115,17 +115,17 @@ namespace pcl * \param[in] samples the point indices found as possible good candidates for creating a valid model * \param[out] model_coefficients the resultant model coefficients */ - bool - computeModelCoefficients (const std::vector &samples, - Eigen::VectorXf &model_coefficients); + bool + computeModelCoefficients (const std::vector &samples, + Eigen::VectorXf &model_coefficients) const; /** \brief Compute all squared distances from the cloud data to a given stick model. * \param[in] model_coefficients the coefficients of a stick model that we need to compute distances to * \param[out] distances the resultant estimated squared distances */ - void - getDistancesToModel (const Eigen::VectorXf &model_coefficients, - std::vector &distances); + void + getDistancesToModel (const Eigen::VectorXf &model_coefficients, + std::vector &distances) const; /** \brief Select all the points which respect the given model coefficients as inliers. * \param[in] model_coefficients the coefficients of a stick model that we need to compute distances to @@ -144,8 +144,8 @@ namespace pcl * \return the resultant number of inliers */ virtual int - countWithinDistance (const Eigen::VectorXf &model_coefficients, - const double threshold); + countWithinDistance (const Eigen::VectorXf &model_coefficients, + const double threshold) const; /** \brief Recompute the stick coefficients using the given inlier set and return them to the user. * @note: these are the coefficients of the stick model after refinement (e.g. after SVD) @@ -153,10 +153,10 @@ namespace pcl * \param[in] model_coefficients the initial guess for the model coefficients * \param[out] optimized_coefficients the resultant recomputed coefficients after optimization */ - void - optimizeModelCoefficients (const std::vector &inliers, - const Eigen::VectorXf &model_coefficients, - Eigen::VectorXf &optimized_coefficients); + void + optimizeModelCoefficients (const std::vector &inliers, + const Eigen::VectorXf &model_coefficients, + Eigen::VectorXf &optimized_coefficients) const; /** \brief Create a new point cloud with inliers projected onto the stick model. * \param[in] inliers the data inliers that we want to project on the stick model @@ -164,21 +164,21 @@ namespace pcl * \param[out] projected_points the resultant projected points * \param[in] copy_data_fields set to true if we need to copy the other data fields */ - void - projectPoints (const std::vector &inliers, - const Eigen::VectorXf &model_coefficients, - PointCloud &projected_points, - bool copy_data_fields = true); + void + projectPoints (const std::vector &inliers, + const Eigen::VectorXf &model_coefficients, + PointCloud &projected_points, + bool copy_data_fields = true) const; /** \brief Verify whether a subset of indices verifies the given stick model coefficients. * \param[in] indices the data indices that need to be tested against the plane model * \param[in] model_coefficients the plane model coefficients * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers */ - bool - doSamplesVerifyModel (const std::set &indices, - const Eigen::VectorXf &model_coefficients, - const double threshold); + bool + doSamplesVerifyModel (const std::set &indices, + const Eigen::VectorXf &model_coefficients, + const double threshold) const; /** \brief Return an unique id for this model (SACMODEL_STICK). */ inline pcl::SacModel