From 78cb403d48da2934a00140b2d64a5d77b3aaa433 Mon Sep 17 00:00:00 2001 From: Sergey Alexandrov Date: Thu, 29 Mar 2018 10:11:54 +0200 Subject: [PATCH 1/2] Refactor SAC models to eliminate tmp_inliers_ member field This field is used only by OptimizationFunctor inside optimizeModelCoefficients() function. Since it is logically local for that function, the pointer to inliers vector is now passed directly to OptimizationFunction. Additionally, mark the pointer to self that is passed to OptimizationFunctor as const, because the functor does not mutate the state of the model. After this patch the state of the SAC model is not mutated during the optimizeModelCoefficients() call and this method may be marked as const. --- .../impl/sac_model_circle.hpp | 4 +-- .../impl/sac_model_circle3d.hpp | 4 +-- .../sample_consensus/impl/sac_model_cone.hpp | 4 +-- .../impl/sac_model_cylinder.hpp | 4 +-- .../impl/sac_model_sphere.hpp | 4 +-- .../pcl/sample_consensus/sac_model_circle.h | 25 ++++++------- .../pcl/sample_consensus/sac_model_circle3d.h | 22 +++++------- .../pcl/sample_consensus/sac_model_cone.h | 24 +++++-------- .../pcl/sample_consensus/sac_model_cylinder.h | 25 +++++-------- .../pcl/sample_consensus/sac_model_sphere.h | 35 +++++++++---------- 10 files changed, 59 insertions(+), 92 deletions(-) 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..736e03c2cf3 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 @@ -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); 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..e73824fca8a 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 @@ -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; 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..ed24b626f06 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 @@ -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); 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..1dfd1064253 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 @@ -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); 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..92487855c45 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 @@ -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); 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..af9989d20f2 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); } @@ -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..5611a4ad76a 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); } @@ -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..dbec469da35 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); } @@ -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..6eb4d15594a 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); } @@ -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_sphere.h b/sample_consensus/include/pcl/sample_consensus/sac_model_sphere.h index d386aa74de2..22ec0eca648 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); } @@ -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++" From da3957aa1bb4636c9426bcc6a27935d23c63ee54 Mon Sep 17 00:00:00 2001 From: Sergey Alexandrov Date: Wed, 28 Mar 2018 15:45:39 +0200 Subject: [PATCH 2/2] Mark majority of methods in SAC models as const * isModelValid() * computeVariance() * getRadiusLimits() * getSamplesMaxDist() * computeModelCoefficients() * getDistancesToModel() * projectPoints() * estimateRigidTransformationSVD() * pointToLineDistance() * projectPointToLine() * projectPointToCylinder() * pointToAxisDistance() * countWithinDistance() * doSamplesVerifyModel() * optimizeModelCoefficients() --- .../impl/sac_model_circle.hpp | 14 ++--- .../impl/sac_model_circle3d.hpp | 22 +++---- .../sample_consensus/impl/sac_model_cone.hpp | 16 ++--- .../impl/sac_model_cylinder.hpp | 18 +++--- .../sample_consensus/impl/sac_model_line.hpp | 12 ++-- .../impl/sac_model_normal_parallel_plane.hpp | 2 +- .../impl/sac_model_normal_plane.hpp | 4 +- .../impl/sac_model_normal_sphere.hpp | 6 +- .../impl/sac_model_parallel_line.hpp | 6 +- .../impl/sac_model_parallel_plane.hpp | 6 +- .../impl/sac_model_perpendicular_plane.hpp | 6 +- .../sample_consensus/impl/sac_model_plane.hpp | 12 ++-- .../impl/sac_model_registration.hpp | 20 +++--- .../impl/sac_model_registration_2d.hpp | 4 +- .../impl/sac_model_sphere.hpp | 12 ++-- .../sample_consensus/impl/sac_model_stick.hpp | 12 ++-- .../include/pcl/sample_consensus/sac_model.h | 52 ++++++++-------- .../pcl/sample_consensus/sac_model_circle.h | 44 ++++++------- .../pcl/sample_consensus/sac_model_circle3d.h | 14 ++--- .../pcl/sample_consensus/sac_model_cone.h | 48 +++++++------- .../pcl/sample_consensus/sac_model_cylinder.h | 62 +++++++++---------- .../pcl/sample_consensus/sac_model_line.h | 42 ++++++------- .../sac_model_normal_parallel_plane.h | 2 +- .../sample_consensus/sac_model_normal_plane.h | 10 +-- .../sac_model_normal_sphere.h | 12 ++-- .../sac_model_parallel_line.h | 6 +- .../sac_model_parallel_plane.h | 6 +- .../sac_model_perpendicular_plane.h | 12 ++-- .../pcl/sample_consensus/sac_model_plane.h | 42 ++++++------- .../sample_consensus/sac_model_registration.h | 14 ++--- .../sac_model_registration_2d.h | 4 +- .../pcl/sample_consensus/sac_model_sphere.h | 44 ++++++------- .../pcl/sample_consensus/sac_model_stick.h | 42 ++++++------- 33 files changed, 314 insertions(+), 314 deletions(-) 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 736e03c2cf3..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; @@ -228,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) @@ -294,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) @@ -319,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 e73824fca8a..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; @@ -290,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) @@ -391,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) @@ -435,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 ed24b626f06..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; @@ -344,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) @@ -440,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) @@ -483,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); @@ -492,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 1dfd1064253..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; @@ -301,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) @@ -387,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) @@ -412,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); @@ -422,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); @@ -439,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 92487855c45..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; @@ -254,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) @@ -277,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 af9989d20f2..07762ea258c 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_circle.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_circle.h @@ -128,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 @@ -157,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) @@ -166,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 @@ -177,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 @@ -205,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 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 5611a4ad76a..af50086fc1e 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_circle3d.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_circle3d.h @@ -132,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 @@ -140,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 @@ -160,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) @@ -171,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 @@ -183,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 @@ -193,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 @@ -207,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 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 dbec469da35..187083064b0 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_cone.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_cone.h @@ -196,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 @@ -225,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. @@ -235,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. @@ -247,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 @@ -275,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. 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 6eb4d15594a..c347c00f0fd 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_cylinder.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_cylinder.h @@ -168,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 @@ -197,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) @@ -206,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. @@ -218,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 @@ -246,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 @@ -256,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 @@ -272,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. 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 22ec0eca648..330de6df66f 100644 --- a/sample_consensus/include/pcl/sample_consensus/sac_model_sphere.h +++ b/sample_consensus/include/pcl/sample_consensus/sac_model_sphere.h @@ -130,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 @@ -159,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) @@ -168,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 @@ -180,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); } @@ -207,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); 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