49 #include <pcl/memory.h>
50 #include <pcl/pcl_macros.h>
51 #include <pcl/pcl_base.h>
52 #include <pcl/console/print.h>
53 #include <pcl/point_cloud.h>
54 #include <pcl/types.h>
55 #include <pcl/sample_consensus/boost.h>
56 #include <pcl/sample_consensus/model_types.h>
58 #include <pcl/search/search.h>
62 template<
class T>
class ProgressiveSampleConsensus;
69 template <
typename Po
intT>
78 using Ptr = shared_ptr<SampleConsensusModel<PointT> >;
79 using ConstPtr = shared_ptr<const SampleConsensusModel<PointT> >;
95 rng_alg_.seed (static_cast<unsigned> (std::time(
nullptr)));
116 rng_alg_.seed (static_cast<unsigned> (std::time (
nullptr)));
144 rng_alg_.seed (static_cast<unsigned> (std::time(
nullptr)));
150 PCL_ERROR (
"[pcl::SampleConsensusModel] Invalid index vector given with size %lu while the input PointCloud has size %lu!\n",
indices_->size (),
input_->points.size ());
173 PCL_ERROR (
"[pcl::SampleConsensusModel::getSamples] Can not select %lu unique points out of %lu!\n",
174 samples.size (),
indices_->size ());
177 iterations = INT_MAX - 1;
194 PCL_DEBUG (
"[pcl::SampleConsensusModel::getSamples] Selected %lu samples.\n", samples.size ());
198 PCL_DEBUG (
"[pcl::SampleConsensusModel::getSamples] WARNING: Could not select %d sample points in %d iterations!\n",
getSampleSize (), max_sample_checks_);
212 Eigen::VectorXf &model_coefficients)
const = 0;
226 const Eigen::VectorXf &model_coefficients,
227 Eigen::VectorXf &optimized_coefficients)
const = 0;
236 std::vector<double> &distances)
const = 0;
248 const double threshold,
262 const double threshold)
const = 0;
274 const Eigen::VectorXf &model_coefficients,
276 bool copy_data_fields =
true)
const = 0;
288 const Eigen::VectorXf &model_coefficients,
289 const double threshold)
const = 0;
303 indices_->resize (cloud->points.size ());
304 for (std::size_t i = 0; i < cloud->points.size (); ++i)
311 inline PointCloudConstPtr
343 inline const std::string&
418 std::vector<double> dists (error_sqr_dists);
419 const std::size_t medIdx = dists.size () >> 1;
420 std::nth_element (dists.begin (), dists.begin () + medIdx, dists.end ());
421 double median_error_sqr = dists[medIdx];
422 return (2.1981 * median_error_sqr);
434 PCL_ERROR (
"[pcl::SampleConsensusModel::computeVariance] The variance of the Sample Consensus model distances cannot be estimated, as the model has not been computed yet. Please compute the model first or at least run selectWithinDistance before continuing. Returning NAN!\n");
435 return (std::numeric_limits<double>::quiet_NaN ());
448 std::size_t sample_size = sample.size ();
450 for (std::size_t i = 0; i < sample_size; ++i)
465 std::size_t sample_size = sample.size ();
472 std::vector<float> sqr_dists;
482 if (indices.size () < sample_size - 1)
485 for(std::size_t i = 1; i < sample_size; ++i)
490 for (std::size_t i = 0; i < sample_size-1; ++i)
491 std::swap (indices[i], indices[i + (
rnd () % (indices.size () - i))]);
492 for (std::size_t i = 1; i < sample_size; ++i)
511 PCL_ERROR (
"[pcl::%s::isModelValid] Invalid number of model coefficients given (is %lu, should be %lu)!\n",
getClassName ().c_str (), model_coefficients.size (),
model_size_);
557 std::shared_ptr<boost::variate_generator< boost::mt19937&, boost::uniform_int<> > >
rng_gen_;
572 return ((*rng_gen_) ());
581 template <
typename Po
intT,
typename Po
intNT>
588 using Ptr = shared_ptr<SampleConsensusModelFromNormals<PointT, PointNT> >;
589 using ConstPtr = shared_ptr<const SampleConsensusModelFromNormals<PointT, PointNT> >;
643 template<
typename _Scalar,
int NX=Eigen::Dynamic,
int NY=Eigen::Dynamic>
653 using ValueType = Eigen::Matrix<Scalar,ValuesAtCompileTime,1>;
654 using InputType = Eigen::Matrix<Scalar,InputsAtCompileTime,1>;
655 using JacobianType = Eigen::Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime>;
663 Functor (
int m_data_points) : m_data_points_ (m_data_points) {}
669 values ()
const {
return (m_data_points_); }
672 const int m_data_points_;
virtual void optimizeModelCoefficients(const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const =0
Recompute the model coefficients using the given inlier set and return them to the user...
double normal_distance_weight_
The relative weight (between 0 and 1) to give to the angular distance (0 to pi/2) between point norma...
shared_ptr< PointCloud< PointT > > Ptr
double radius_min_
The minimum and maximum radius limits for the model.
void setIndices(const Indices &indices)
Provide the vector of indices that represents the input data.
SampleConsensusModelFromNormals()
Empty constructor for base SampleConsensusModelFromNormals.
unsigned int getModelSize() const
Return the number of coefficients in the model.
virtual SacModel getModelType() const =0
Return a unique id for each type of model employed.
shared_ptr< Indices > IndicesPtr
std::shared_ptr< boost::variate_generator< boost::mt19937 &, boost::uniform_int<> > > rng_gen_
Boost-based random number generator.
std::vector< index_t > Indices
Type used for indices in PCL.
int values() const
Get the number of values.
SampleConsensusModel(bool random=false)
Empty constructor for base SampleConsensusModel.
RandomSampleConsensus represents an implementation of the RANSAC (RAndom SAmple Consensus) algorithm...
unsigned int model_size_
The number of coefficients in the model.
PointCloudConstPtr getInputCloud() const
Get a pointer to the input point cloud dataset.
Eigen::Matrix< Scalar, InputsAtCompileTime, 1 > InputType
SampleConsensusModel(const PointCloudConstPtr &cloud, bool random=false)
Constructor for base SampleConsensusModel.
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
Indices shuffled_indices_
Data containing a shuffled version of the indices.
Base functor all the models that need non linear optimization must define their own one and implement...
double samples_radius_
The maximum distance of subsequent samples from the first (radius search)
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
virtual bool computeModelCoefficients(const Indices &samples, Eigen::VectorXf &model_coefficients) const =0
Check whether the given index samples can form a valid model, compute the model coefficients from the...
typename PointCloud::Ptr PointCloudPtr
const std::string & getClassName() const
Get a string representation of the name of this class.
void setSamplesMaxDist(const double &radius, SearchPtr search)
Set the maximum distance allowed when drawing random samples.
virtual bool isModelValid(const Eigen::VectorXf &model_coefficients) const
Check whether a model is valid given the user constraints.
static const unsigned int max_sample_checks_
The maximum number of samples to try until we get a good one.
virtual void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const =0
Compute all distances from the cloud data to a given model.
PointCloudConstPtr input_
A boost shared pointer to the point cloud data array.
void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
virtual void getSamples(int &iterations, Indices &samples)
Get a set of random data samples and return them as point indices.
virtual void selectWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers)=0
Select all the points which respect the given model coefficients as inliers.
void getRadiusLimits(double &min_radius, double &max_radius) const
Get the minimum and maximum allowable radius limits for the model as set by the user.
typename pcl::PointCloud< PointNT >::ConstPtr PointCloudNConstPtr
SampleConsensusModel represents the base model class.
int rnd()
Boost-based random number generator.
Functor(int m_data_points)
Constructor.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
std::string model_name_
The model name.
Eigen::Matrix< Scalar, ValuesAtCompileTime, 1 > ValueType
void getSamplesMaxDist(double &radius) const
Get maximum distance allowed when drawing random samples.
shared_ptr< SampleConsensusModelFromNormals< pcl::PointXYZRGB, PointNT > > Ptr
shared_ptr< const SampleConsensusModelFromNormals< pcl::PointXYZRGB, PointNT > > ConstPtr
double computeVariance() const
Compute the variance of the errors to the model from the internally estimated vector of distances...
void setNormalDistanceWeight(const double w)
Set the normal angular distance weight.
PointCloudNConstPtr getInputNormals() const
Get a pointer to the normals of the input XYZ point cloud dataset.
double computeVariance(const std::vector< double > &error_sqr_dists) const
Compute the variance of the errors to the model.
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input dataset that contains the point normals of the XYZ dataset...
typename PointCloud::ConstPtr PointCloudConstPtr
Functor()
Empty Constructor.
PointCloudNConstPtr normals_
A pointer to the input dataset that contains the point normals of the XYZ dataset.
SampleConsensusModelFromNormals represents the base model class for models that require the use of su...
PointCloud represents the base class in PCL for storing collections of 3D points. ...
virtual std::size_t countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold) const =0
Count all the points which respect the given model coefficients as inliers.
void drawIndexSampleRadius(Indices &sample)
Fills a sample array with one random sample from the indices_ vector and other random samples that ar...
std::shared_ptr< boost::uniform_int<> > rng_dist_
Boost-based random number generator distribution.
shared_ptr< const SampleConsensusModel< WeightSACPointType > > ConstPtr
virtual bool doSamplesVerifyModel(const std::set< index_t > &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const =0
Verify whether a subset of indices verifies a given set of model coefficients.
typename pcl::search::Search< WeightSACPointType >::Ptr SearchPtr
virtual bool isSampleGood(const Indices &samples) const =0
Check if a sample of indices results in a good sample of points indices.
boost::mt19937 rng_alg_
Boost-based random number generator algorithm.
Eigen::Matrix< Scalar, ValuesAtCompileTime, InputsAtCompileTime > JacobianType
virtual void projectPoints(const Indices &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields=true) const =0
Create a new point cloud with inliers projected onto the model.
IndicesPtr indices_
A pointer to the vector of point indices to use.
shared_ptr< const PointCloud< PointT > > ConstPtr
void setRadiusLimits(const double &min_radius, const double &max_radius)
Set the minimum and maximum allowable radius limits for the model (applicable to models that estimate...
std::vector< double > error_sqr_dists_
A vector holding the distances to the computed model.
shared_ptr< pcl::search::Search< PointT > > Ptr
double getNormalDistanceWeight() const
Get the normal angular distance weight.
virtual ~SampleConsensusModelFromNormals()
Destructor.
A point structure representing Euclidean xyz coordinates, and the RGB color.
shared_ptr< SampleConsensusModel< WeightSACPointType > > Ptr
void drawIndexSample(Indices &sample)
Fills a sample array with random samples from the indices_ vector.
SampleConsensusModel(const PointCloudConstPtr &cloud, const Indices &indices, bool random=false)
Constructor for base SampleConsensusModel.
SearchPtr samples_radius_search_
The search object for picking subsequent samples using radius search.
IndicesPtr getIndices() const
Get a pointer to the vector of indices used.
typename pcl::PointCloud< PointNT >::Ptr PointCloudNPtr
unsigned int getSampleSize() const
Return the size of a sample from which the model is computed.
unsigned int sample_size_
The size of a sample from which the model is computed.
virtual ~SampleConsensusModel()
Destructor for base SampleConsensusModel.