Point Cloud Library (PCL)
1.12.1-dev
|
47 #include <boost/random/mersenne_twister.hpp>
48 #include <boost/random/uniform_int.hpp>
49 #include <boost/random/variate_generator.hpp>
52 #include <pcl/console/print.h>
53 #include <pcl/point_cloud.h>
55 #include <pcl/sample_consensus/model_types.h>
57 #include <pcl/search/search.h>
61 template<
class T>
class ProgressiveSampleConsensus;
68 template <
typename Po
intT>
77 using Ptr = shared_ptr<SampleConsensusModel<PointT> >;
78 using ConstPtr = shared_ptr<const SampleConsensusModel<PointT> >;
86 ,
radius_min_ (-std::numeric_limits<double>::max ())
90 ,
rng_dist_ (new
boost::uniform_int<> (0, std::numeric_limits<int>::max ()))
95 rng_alg_.seed (
static_cast<unsigned> (std::time(
nullptr)));
109 ,
radius_min_ (-std::numeric_limits<double>::max ())
110 ,
radius_max_ (std::numeric_limits<double>::max ())
113 ,
rng_dist_ (new
boost::uniform_int<> (0, std::numeric_limits<int>::max ()))
117 rng_alg_.seed (
static_cast<unsigned> (std::time (
nullptr)));
138 ,
radius_min_ (-std::numeric_limits<double>::max ())
139 ,
radius_max_ (std::numeric_limits<double>::max ())
142 ,
rng_dist_ (new
boost::uniform_int<> (0, std::numeric_limits<int>::max ()))
146 rng_alg_.seed (
static_cast<unsigned> (std::time(
nullptr)));
152 PCL_ERROR(
"[pcl::SampleConsensusModel] Invalid index vector given with size "
153 "%zu while the input PointCloud has size %zu!\n",
155 static_cast<std::size_t
>(
input_->size()));
178 PCL_ERROR (
"[pcl::SampleConsensusModel::getSamples] Can not select %lu unique points out of %lu!\n",
179 samples.size (),
indices_->size ());
182 iterations = std::numeric_limits<int>::max() - 1;
199 PCL_DEBUG (
"[pcl::SampleConsensusModel::getSamples] Selected %lu samples.\n", samples.size ());
203 PCL_DEBUG (
"[pcl::SampleConsensusModel::getSamples] WARNING: Could not select %d sample points in %d iterations!\n",
getSampleSize (),
max_sample_checks_);
217 Eigen::VectorXf &model_coefficients)
const = 0;
231 const Eigen::VectorXf &model_coefficients,
232 Eigen::VectorXf &optimized_coefficients)
const = 0;
241 std::vector<double> &distances)
const = 0;
253 const double threshold,
267 const double threshold)
const = 0;
279 const Eigen::VectorXf &model_coefficients,
281 bool copy_data_fields =
true)
const = 0;
293 const Eigen::VectorXf &model_coefficients,
294 const double threshold)
const = 0;
309 for (std::size_t i = 0; i < cloud->size (); ++i)
316 inline PointCloudConstPtr
348 inline const std::string&
403 PCL_ERROR (
"[pcl::SampleConsensusModel::setModelConstraints] The given function is empty (i.e. does not contain a callable target)!\n");
438 std::vector<double> dists (error_sqr_dists);
439 const std::size_t medIdx = dists.size () >> 1;
440 std::nth_element (dists.begin (), dists.begin () + medIdx, dists.end ());
441 double median_error_sqr = dists[medIdx];
442 return (2.1981 * median_error_sqr);
454 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");
455 return (std::numeric_limits<double>::quiet_NaN ());
468 std::size_t sample_size = sample.size ();
470 for (std::size_t i = 0; i < sample_size; ++i)
485 std::size_t sample_size = sample.size ();
492 std::vector<float> sqr_dists;
502 if (indices.size () < sample_size - 1)
505 for(std::size_t i = 1; i < sample_size; ++i)
510 for (std::size_t i = 0; i < sample_size-1; ++i)
511 std::swap (indices[i], indices[i + (
rnd () % (indices.size () - i))]);
512 for (std::size_t i = 1; i < sample_size; ++i)
531 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_);
536 PCL_DEBUG (
"[pcl::%s::isModelValid] The user defined isModelValid function returned false.\n",
getClassName ().c_str ());
582 std::shared_ptr<boost::variate_generator< boost::mt19937&, boost::uniform_int<> > >
rng_gen_;
610 template <
typename Po
intT,
typename Po
intNT>
617 using Ptr = shared_ptr<SampleConsensusModelFromNormals<PointT, PointNT> >;
618 using ConstPtr = shared_ptr<const SampleConsensusModelFromNormals<PointT, PointNT> >;
634 if (w < 0.0 || w > 1.0)
636 PCL_ERROR (
"[pcl::SampleConsensusModel::setNormalDistanceWeight] w is %g, but should be in [0; 1]. Weight will not be set.", w);
677 template<
typename _Scalar,
int NX=Eigen::Dynamic,
int NY=Eigen::Dynamic>
687 using ValueType = Eigen::Matrix<Scalar,ValuesAtCompileTime,1>;
688 using InputType = Eigen::Matrix<Scalar,InputsAtCompileTime,1>;
689 using JacobianType = Eigen::Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime>;
697 Functor (
int m_data_points) : m_data_points_ (m_data_points) {}
703 values ()
const {
return (m_data_points_); }
706 const int m_data_points_;
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.
virtual ~SampleConsensusModel()
Destructor for base SampleConsensusModel.
SampleConsensusModel(const PointCloudConstPtr &cloud, const Indices &indices, bool random=false)
Constructor for base SampleConsensusModel.
Eigen::Matrix< Scalar, ValuesAtCompileTime, InputsAtCompileTime > JacobianType
shared_ptr< Indices > IndicesPtr
std::shared_ptr< boost::variate_generator< boost::mt19937 &, boost::uniform_int<> > > rng_gen_
Boost-based random number generator.
SearchPtr samples_radius_search_
The search object for picking subsequent samples using radius search.
virtual bool isSampleGood(const Indices &samples) const =0
Check if a sample of indices results in a good sample of points indices.
void setSamplesMaxDist(const double &radius, SearchPtr search)
Set the maximum distance allowed when drawing random samples.
const std::string & getClassName() const
Get a string representation of the name of this class.
Defines basic non-point types used by PCL.
std::shared_ptr< boost::uniform_int<> > rng_dist_
Boost-based random number generator distribution.
void setInputNormals(const PointCloudNConstPtr &normals)
Provide 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...
virtual void getSamples(int &iterations, Indices &samples)
Get a set of random data samples and return them as point indices.
PointCloudNConstPtr getInputNormals() const
Get a pointer to the normals of the input XYZ point cloud dataset.
PointCloudConstPtr getInputCloud() const
Get a pointer to the input point cloud dataset.
Eigen::Matrix< Scalar, InputsAtCompileTime, 1 > InputType
double getNormalDistanceWeight() const
Get the normal angular distance weight.
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.
unsigned int sample_size_
The size of a sample from which the model is computed.
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.
unsigned int model_size_
The number of coefficients in the model.
ProgressiveSampleConsensus represents an implementation of the PROSAC (PROgressive SAmple Consensus) ...
void getSamplesMaxDist(double &radius) const
Get maximum distance allowed when drawing random samples.
PointCloud represents the base class in PCL for storing collections of 3D points.
void setIndices(const Indices &indices)
Provide the vector of indices that represents the input data.
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
typename pcl::search::Search< WeightSACPointType >::Ptr SearchPtr
A point structure representing Euclidean xyz coordinates, and the RGB color.
virtual ~SampleConsensusModelFromNormals()
Destructor.
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.
int values() const
Get the number of values.
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.
PointCloudNConstPtr normals_
A pointer to the input dataset that contains the point normals of the XYZ dataset.
Functor(int m_data_points)
Constructor.
virtual SacModel getModelType() const =0
Return a unique id for each type of model employed.
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.
Base functor all the models that need non linear optimization must define their own one and implement...
unsigned int getModelSize() const
Return the number of coefficients in the model.
virtual bool isModelValid(const Eigen::VectorXf &model_coefficients) const
Check whether a model is valid given the user constraints.
void setNormalDistanceWeight(const double w)
Set the normal angular distance weight.
double samples_radius_
The maximum distance of subsequent samples from the first (radius search)
void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
IndicesPtr indices_
A pointer to the vector of point indices to use.
shared_ptr< const SampleConsensusModel< WeightSACPointType > > ConstPtr
typename pcl::PointCloud< PointNT >::Ptr PointCloudNPtr
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...
SampleConsensusModelFromNormals()
Empty constructor for base SampleConsensusModelFromNormals.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
typename pcl::PointCloud< PointNT >::ConstPtr PointCloudNConstPtr
double radius_min_
The minimum and maximum radius limits for the model.
shared_ptr< pcl::search::Search< PointT > > Ptr
Eigen::Matrix< Scalar, ValuesAtCompileTime, 1 > ValueType
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
void drawIndexSample(Indices &sample)
Fills a sample array with random samples from the indices_ vector.
int rnd()
Boost-based random number generator.
shared_ptr< SampleConsensusModel< WeightSACPointType > > Ptr
std::function< bool(const Eigen::VectorXf &)> custom_model_constraints_
A user defined function that takes model coefficients and returns whether the model is acceptable or ...
shared_ptr< const SampleConsensusModelFromNormals< pcl::PointXYZRGB, PointNT > > ConstPtr
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...
std::string model_name_
The model name.
unsigned int getSampleSize() const
Return the size of a sample from which the model is computed.
void setModelConstraints(std::function< bool(const Eigen::VectorXf &)> function)
This can be used to impose any kind of constraint on the model, e.g.
IndicesAllocator<> Indices
Type used for indices in PCL.
typename PointCloud::ConstPtr PointCloudConstPtr
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.
SampleConsensusModel(bool random=false)
Empty constructor for base SampleConsensusModel.
std::vector< double > error_sqr_dists_
A vector holding the distances to the computed model.
shared_ptr< PointCloud< PointT > > Ptr
Indices shuffled_indices_
Data containing a shuffled version of the indices.
shared_ptr< const PointCloud< PointT > > ConstPtr
void drawIndexSampleRadius(Indices &sample)
Fills a sample array with one random sample from the indices_ vector and other random samples that ar...
Functor()
Empty Constructor.
double computeVariance(const std::vector< double > &error_sqr_dists) const
Compute the variance of the errors to the model.
typename PointCloud::Ptr PointCloudPtr
double computeVariance() const
Compute the variance of the errors to the model from the internally estimated vector of distances.
SampleConsensusModel represents the base model class.
SampleConsensusModel(const PointCloudConstPtr &cloud, bool random=false)
Constructor for base SampleConsensusModel.
PointCloudConstPtr input_
A boost shared pointer to the point cloud data array.
double normal_distance_weight_
The relative weight (between 0 and 1) to give to the angular distance (0 to pi/2) between point norma...
boost::mt19937 rng_alg_
Boost-based random number generator algorithm.
Defines functions, macros and traits for allocating and using memory.
IndicesPtr getIndices() const
Get a pointer to the vector of indices used.
shared_ptr< SampleConsensusModelFromNormals< pcl::PointXYZRGB, PointNT > > Ptr