Skip to content

Commit

Permalink
Merge pull request #2270 from taketwo/sac-const
Browse files Browse the repository at this point in the history 
Const-qualify most of the methods in SAC model classes
  • Loading branch information
@SergioRAgostinho
SergioRAgostinho authored May 2, 2018
2 parents d3b500a + da3957a commit 6770ab3
Show file tree
Hide file tree
Showing 33 changed files with 373 additions and 406 deletions.
18 changes: 8 additions & 10 deletions sample_consensus/include/pcl/sample_consensus/impl/sac_model_circle.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,7 +66,7 @@ pcl::SampleConsensusModelCircle2D<PointT>::isSampleGood(const std::vector<int> &

//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
pcl::SampleConsensusModelCircle2D<PointT>::computeModelCoefficients (const std::vector<int> &samples, Eigen::VectorXf &model_coefficients)
pcl::SampleConsensusModelCircle2D<PointT>::computeModelCoefficients (const std::vector<int> &samples, Eigen::VectorXf &model_coefficients) const
{
// Need 3 samples
if (samples.size () != 3)
Expand Down Expand Up @@ -102,7 +102,7 @@ pcl::SampleConsensusModelCircle2D<PointT>::computeModelCoefficients (const std::

//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelCircle2D<PointT>::getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
pcl::SampleConsensusModelCircle2D<PointT>::getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector<double> &distances) const
{
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
Expand Down Expand Up @@ -168,7 +168,7 @@ pcl::SampleConsensusModelCircle2D<PointT>::selectWithinDistance (
//////////////////////////////////////////////////////////////////////////
template <typename PointT> int
pcl::SampleConsensusModelCircle2D<PointT>::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))
Expand Down Expand Up @@ -196,7 +196,7 @@ pcl::SampleConsensusModelCircle2D<PointT>::countWithinDistance (
//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelCircle2D<PointT>::optimizeModelCoefficients (
const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const
{
optimized_coefficients = model_coefficients;

Expand All @@ -214,9 +214,7 @@ pcl::SampleConsensusModelCircle2D<PointT>::optimizeModelCoefficients (
return;
}

tmp_inliers_ = &inliers;

OptimizationFunctor functor (static_cast<int> (inliers.size ()), this);
OptimizationFunctor functor (this, inliers);
Eigen::NumericalDiff<OptimizationFunctor> num_diff (functor);
Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, float> lm (num_diff);
int info = lm.minimize (optimized_coefficients);
Expand All @@ -230,7 +228,7 @@ pcl::SampleConsensusModelCircle2D<PointT>::optimizeModelCoefficients (
template <typename PointT> void
pcl::SampleConsensusModelCircle2D<PointT>::projectPoints (
const std::vector<int> &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)
Expand Down Expand Up @@ -296,7 +294,7 @@ pcl::SampleConsensusModelCircle2D<PointT>::projectPoints (
//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
pcl::SampleConsensusModelCircle2D<PointT>::doSamplesVerifyModel (
const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, const double threshold)
const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const
{
// Needs a valid model coefficients
if (model_coefficients.size () != 3)
Expand All @@ -321,7 +319,7 @@ pcl::SampleConsensusModelCircle2D<PointT>::doSamplesVerifyModel (

//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
pcl::SampleConsensusModelCircle2D<PointT>::isModelValid (const Eigen::VectorXf &model_coefficients)
pcl::SampleConsensusModelCircle2D<PointT>::isModelValid (const Eigen::VectorXf &model_coefficients) const
{
if (!SampleConsensusModel<PointT>::isModelValid (model_coefficients))
return (false);
Expand Down
26 changes: 12 additions & 14 deletions sample_consensus/include/pcl/sample_consensus/impl/sac_model_circle3d.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,7 +65,7 @@ pcl::SampleConsensusModelCircle3D<PointT>::isSampleGood (

//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
pcl::SampleConsensusModelCircle3D<PointT>::computeModelCoefficients (const std::vector<int> &samples, Eigen::VectorXf &model_coefficients)
pcl::SampleConsensusModelCircle3D<PointT>::computeModelCoefficients (const std::vector<int> &samples, Eigen::VectorXf &model_coefficients) const
{
// Need 3 samples
if (samples.size () != 3)
Expand Down Expand Up @@ -115,7 +115,7 @@ pcl::SampleConsensusModelCircle3D<PointT>::computeModelCoefficients (const std::

//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelCircle3D<PointT>::getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
pcl::SampleConsensusModelCircle3D<PointT>::getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector<double> &distances) const
{
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
Expand Down Expand Up @@ -212,7 +212,7 @@ pcl::SampleConsensusModelCircle3D<PointT>::selectWithinDistance (
//////////////////////////////////////////////////////////////////////////
template <typename PointT> int
pcl::SampleConsensusModelCircle3D<PointT>::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))
Expand Down Expand Up @@ -253,9 +253,9 @@ pcl::SampleConsensusModelCircle3D<PointT>::countWithinDistance (
//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelCircle3D<PointT>::optimizeModelCoefficients (
const std::vector<int> &inliers,
const Eigen::VectorXf &model_coefficients,
Eigen::VectorXf &optimized_coefficients)
const std::vector<int> &inliers,
const Eigen::VectorXf &model_coefficients,
Eigen::VectorXf &optimized_coefficients) const
{
optimized_coefficients = model_coefficients;

Expand All @@ -273,9 +273,7 @@ pcl::SampleConsensusModelCircle3D<PointT>::optimizeModelCoefficients (
return;
}

tmp_inliers_ = &inliers;

OptimizationFunctor functor (static_cast<int> (inliers.size ()), this);
OptimizationFunctor functor (this, inliers);
Eigen::NumericalDiff<OptimizationFunctor> num_diff (functor);
Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, double> lm (num_diff);
Eigen::VectorXd coeff;
Expand All @@ -292,7 +290,7 @@ pcl::SampleConsensusModelCircle3D<PointT>::optimizeModelCoefficients (
template <typename PointT> void
pcl::SampleConsensusModelCircle3D<PointT>::projectPoints (
const std::vector<int> &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)
Expand Down Expand Up @@ -393,9 +391,9 @@ pcl::SampleConsensusModelCircle3D<PointT>::projectPoints (
//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
pcl::SampleConsensusModelCircle3D<PointT>::doSamplesVerifyModel (
const std::set<int> &indices,
const Eigen::VectorXf &model_coefficients,
const double threshold)
const std::set<int> &indices,
const Eigen::VectorXf &model_coefficients,
const double threshold) const
{
// Needs a valid model coefficients
if (model_coefficients.size () != 7)
Expand Down Expand Up @@ -437,7 +435,7 @@ pcl::SampleConsensusModelCircle3D<PointT>::doSamplesVerifyModel (

//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
pcl::SampleConsensusModelCircle3D<PointT>::isModelValid (const Eigen::VectorXf &model_coefficients)
pcl::SampleConsensusModelCircle3D<PointT>::isModelValid (const Eigen::VectorXf &model_coefficients) const
{
if (!SampleConsensusModel<PointT>::isModelValid (model_coefficients))
return (false);
Expand Down
20 changes: 9 additions & 11 deletions sample_consensus/include/pcl/sample_consensus/impl/sac_model_cone.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,7 @@ pcl::SampleConsensusModelCone<PointT, PointNT>::isSampleGood(const std::vector<i
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT> bool
pcl::SampleConsensusModelCone<PointT, PointNT>::computeModelCoefficients (
const std::vector<int> &samples, Eigen::VectorXf &model_coefficients)
const std::vector<int> &samples, Eigen::VectorXf &model_coefficients) const
{
// Need 3 samples
if (samples.size () != 3)
Expand Down Expand Up @@ -135,7 +135,7 @@ pcl::SampleConsensusModelCone<PointT, PointNT>::computeModelCoefficients (
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelCone<PointT, PointNT>::getDistancesToModel (
const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
const Eigen::VectorXf &model_coefficients, std::vector<double> &distances) const
{
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
Expand Down Expand Up @@ -253,7 +253,7 @@ pcl::SampleConsensusModelCone<PointT, PointNT>::selectWithinDistance (
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT> int
pcl::SampleConsensusModelCone<PointT, PointNT>::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
Expand Down Expand Up @@ -307,7 +307,7 @@ pcl::SampleConsensusModelCone<PointT, PointNT>::countWithinDistance (
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelCone<PointT, PointNT>::optimizeModelCoefficients (
const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const
{
optimized_coefficients = model_coefficients;

Expand All @@ -324,9 +324,7 @@ pcl::SampleConsensusModelCone<PointT, PointNT>::optimizeModelCoefficients (
return;
}

tmp_inliers_ = &inliers;

OptimizationFunctor functor (static_cast<int> (inliers.size ()), this);
OptimizationFunctor functor (this, inliers);
Eigen::NumericalDiff<OptimizationFunctor > num_diff (functor);
Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, float> lm (num_diff);
int info = lm.minimize (optimized_coefficients);
Expand All @@ -346,7 +344,7 @@ pcl::SampleConsensusModelCone<PointT, PointNT>::optimizeModelCoefficients (
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelCone<PointT, PointNT>::projectPoints (
const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields)
const std::vector<int> &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)
Expand Down Expand Up @@ -442,7 +440,7 @@ pcl::SampleConsensusModelCone<PointT, PointNT>::projectPoints (
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT> bool
pcl::SampleConsensusModelCone<PointT, PointNT>::doSamplesVerifyModel (
const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, const double threshold)
const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const
{
// Needs a valid model coefficients
if (model_coefficients.size () != 7)
Expand Down Expand Up @@ -485,7 +483,7 @@ pcl::SampleConsensusModelCone<PointT, PointNT>::doSamplesVerifyModel (
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT> double
pcl::SampleConsensusModelCone<PointT, PointNT>::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);
Expand All @@ -494,7 +492,7 @@ pcl::SampleConsensusModelCone<PointT, PointNT>::pointToAxisDistance (

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT> bool
pcl::SampleConsensusModelCone<PointT, PointNT>::isModelValid (const Eigen::VectorXf &model_coefficients)
pcl::SampleConsensusModelCone<PointT, PointNT>::isModelValid (const Eigen::VectorXf &model_coefficients) const
{
if (!SampleConsensusModel<PointT>::isModelValid (model_coefficients))
return (false);
Expand Down
22 changes: 10 additions & 12 deletions sample_consensus/include/pcl/sample_consensus/impl/sac_model_cylinder.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,7 +55,7 @@ pcl::SampleConsensusModelCylinder<PointT, PointNT>::isSampleGood (const std::vec
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT> bool
pcl::SampleConsensusModelCylinder<PointT, PointNT>::computeModelCoefficients (
const std::vector<int> &samples, Eigen::VectorXf &model_coefficients)
const std::vector<int> &samples, Eigen::VectorXf &model_coefficients) const
{
// Need 2 samples
if (samples.size () != 2)
Expand Down Expand Up @@ -129,7 +129,7 @@ pcl::SampleConsensusModelCylinder<PointT, PointNT>::computeModelCoefficients (
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelCylinder<PointT, PointNT>::getDistancesToModel (
const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
const Eigen::VectorXf &model_coefficients, std::vector<double> &distances) const
{
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
Expand Down Expand Up @@ -224,7 +224,7 @@ pcl::SampleConsensusModelCylinder<PointT, PointNT>::selectWithinDistance (
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT> int
pcl::SampleConsensusModelCylinder<PointT, PointNT>::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))
Expand Down Expand Up @@ -264,7 +264,7 @@ pcl::SampleConsensusModelCylinder<PointT, PointNT>::countWithinDistance (
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelCylinder<PointT, PointNT>::optimizeModelCoefficients (
const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const
{
optimized_coefficients = model_coefficients;

Expand All @@ -281,9 +281,7 @@ pcl::SampleConsensusModelCylinder<PointT, PointNT>::optimizeModelCoefficients (
return;
}

tmp_inliers_ = &inliers;

OptimizationFunctor functor (static_cast<int> (inliers.size ()), this);
OptimizationFunctor functor (this, inliers);
Eigen::NumericalDiff<OptimizationFunctor > num_diff (functor);
Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, float> lm (num_diff);
int info = lm.minimize (optimized_coefficients);
Expand All @@ -303,7 +301,7 @@ pcl::SampleConsensusModelCylinder<PointT, PointNT>::optimizeModelCoefficients (
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelCylinder<PointT, PointNT>::projectPoints (
const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields)
const std::vector<int> &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)
Expand Down Expand Up @@ -389,7 +387,7 @@ pcl::SampleConsensusModelCylinder<PointT, PointNT>::projectPoints (
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT> bool
pcl::SampleConsensusModelCylinder<PointT, PointNT>::doSamplesVerifyModel (
const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, const double threshold)
const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const
{
// Needs a valid model coefficients
if (model_coefficients.size () != 7)
Expand All @@ -414,7 +412,7 @@ pcl::SampleConsensusModelCylinder<PointT, PointNT>::doSamplesVerifyModel (
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT> double
pcl::SampleConsensusModelCylinder<PointT, PointNT>::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);
Expand All @@ -424,7 +422,7 @@ pcl::SampleConsensusModelCylinder<PointT, PointNT>::pointToLineDistance (
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelCylinder<PointT, PointNT>::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);
Expand All @@ -441,7 +439,7 @@ pcl::SampleConsensusModelCylinder<PointT, PointNT>::projectPointToCylinder (

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT> bool
pcl::SampleConsensusModelCylinder<PointT, PointNT>::isModelValid (const Eigen::VectorXf &model_coefficients)
pcl::SampleConsensusModelCylinder<PointT, PointNT>::isModelValid (const Eigen::VectorXf &model_coefficients) const
{
if (!SampleConsensusModel<PointT>::isModelValid (model_coefficients))
return (false);
Expand Down
Loading

0 comments on commit 6770ab3

Please sign in to comment.