Point Cloud Library (PCL)
1.10.0
|
50 #include <pcl/pcl_base.h>
51 #include <pcl/console/print.h>
52 #include <pcl/point_cloud.h>
53 #include <pcl/sample_consensus/boost.h>
54 #include <pcl/sample_consensus/model_types.h>
56 #include <pcl/search/search.h>
60 template<
class T>
class ProgressiveSampleConsensus;
67 template <
typename Po
intT>
85 ,
radius_min_ (-std::numeric_limits<double>::max ())
89 ,
rng_dist_ (new
boost::uniform_int<> (0, std::numeric_limits<int>::max ()))
93 rng_alg_.seed (
static_cast<unsigned> (std::time(
nullptr)));
107 ,
radius_min_ (-std::numeric_limits<double>::max ())
108 ,
radius_max_ (std::numeric_limits<double>::max ())
111 ,
rng_dist_ (new
boost::uniform_int<> (0, std::numeric_limits<int>::max ()))
114 rng_alg_.seed (
static_cast<unsigned> (std::time (
nullptr)));
131 const std::vector<int> &indices,
134 ,
indices_ (new std::vector<int> (indices))
135 ,
radius_min_ (-std::numeric_limits<double>::max ())
136 ,
radius_max_ (std::numeric_limits<double>::max ())
139 ,
rng_dist_ (new
boost::uniform_int<> (0, std::numeric_limits<int>::max ()))
142 rng_alg_.seed (
static_cast<unsigned> (std::time(
nullptr)));
148 PCL_ERROR (
"[pcl::SampleConsensusModel] Invalid index vector given with size %lu while the input PointCloud has size %lu!\n",
indices_->size (),
input_->points.size ());
171 PCL_ERROR (
"[pcl::SampleConsensusModel::getSamples] Can not select %lu unique points out of %lu!\n",
172 samples.size (),
indices_->size ());
175 iterations = INT_MAX - 1;
192 PCL_DEBUG (
"[pcl::SampleConsensusModel::getSamples] Selected %lu samples.\n", samples.size ());
196 PCL_DEBUG (
"[pcl::SampleConsensusModel::getSamples] WARNING: Could not select %d sample points in %d iterations!\n",
getSampleSize (),
max_sample_checks_);
210 Eigen::VectorXf &model_coefficients)
const = 0;
224 const Eigen::VectorXf &model_coefficients,
225 Eigen::VectorXf &optimized_coefficients)
const = 0;
234 std::vector<double> &distances)
const = 0;
246 const double threshold,
247 std::vector<int> &inliers) = 0;
260 const double threshold)
const = 0;
272 const Eigen::VectorXf &model_coefficients,
274 bool copy_data_fields =
true)
const = 0;
286 const Eigen::VectorXf &model_coefficients,
287 const double threshold)
const = 0;
297 indices_.reset (
new std::vector<int> ());
301 indices_->resize (cloud->points.size ());
302 for (std::size_t i = 0; i < cloud->points.size (); ++i)
303 (*
indices_)[i] =
static_cast<int> (i);
309 inline PointCloudConstPtr
328 indices_.reset (
new std::vector<int> (indices));
341 inline const std::string&
416 std::vector<double> dists (error_sqr_dists);
417 const std::size_t medIdx = dists.size () >> 1;
418 std::nth_element (dists.begin (), dists.begin () + medIdx, dists.end ());
419 double median_error_sqr = dists[medIdx];
420 return (2.1981 * median_error_sqr);
432 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");
433 return (std::numeric_limits<double>::quiet_NaN ());
446 std::size_t sample_size = sample.size ();
448 for (std::size_t i = 0; i < sample_size; ++i)
463 std::size_t sample_size = sample.size ();
469 std::vector<int> indices;
470 std::vector<float> sqr_dists;
480 if (indices.size () < sample_size - 1)
483 for(std::size_t i = 1; i < sample_size; ++i)
488 for (std::size_t i = 0; i < sample_size-1; ++i)
489 std::swap (indices[i], indices[i + (
rnd () % (indices.size () - i))]);
490 for (std::size_t i = 1; i < sample_size; ++i)
509 PCL_ERROR (
"[pcl::%s::isModelValid] Invalid number of model coefficients given (%lu)!\n",
getClassName ().c_str (), model_coefficients.size ());
520 isSampleGood (
const std::vector<int> &samples)
const = 0;
555 std::shared_ptr<boost::variate_generator< boost::mt19937&, boost::uniform_int<> > >
rng_gen_;
579 template <
typename Po
intT,
typename Po
intNT>
641 template<
typename _Scalar,
int NX=Eigen::Dynamic,
int NY=Eigen::Dynamic>
651 using ValueType = Eigen::Matrix<Scalar,ValuesAtCompileTime,1>;
652 using InputType = Eigen::Matrix<Scalar,InputsAtCompileTime,1>;
653 using JacobianType = Eigen::Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime>;
661 Functor (
int m_data_points) : m_data_points_ (m_data_points) {}
667 values ()
const {
return (m_data_points_); }
670 const int m_data_points_;
virtual bool computeModelCoefficients(const std::vector< int > &samples, Eigen::VectorXf &model_coefficients) const =0
Check whether the given index samples can form a valid model, compute the model coefficients from the...
Defines all the PCL and non-PCL macros used.
virtual ~SampleConsensusModel()
Destructor for base SampleConsensusModel.
This file defines compatibility wrappers for low level I/O functions.
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.
void drawIndexSampleRadius(std::vector< int > &sample)
Fills a sample array with one random sample from the indices_ vector and other random samples that ar...
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.
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...
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 optimizeModelCoefficients(const std::vector< int > &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.
unsigned int sample_size_
The size of a sample from which the model is computed.
virtual void projectPoints(const std::vector< int > &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.
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.
RandomSampleConsensus represents an implementation of the RANSAC (RAndom SAmple Consensus) algorithm,...
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.
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 selectWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold, std::vector< int > &inliers)=0
Select all the points which respect the given model coefficients as inliers.
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.
SampleConsensusModel(const PointCloudConstPtr &cloud, const std::vector< int > &indices, bool random=false)
Constructor for base SampleConsensusModel.
virtual void getSamples(int &iterations, std::vector< int > &samples)
Get a set of random data samples and return them as point indices.
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.
virtual bool isSampleGood(const std::vector< int > &samples) const =0
Check if a sample of indices results in a good sample of points indices.
int rnd()
Boost-based random number generator.
shared_ptr< SampleConsensusModel< WeightSACPointType > > Ptr
shared_ptr< const SampleConsensusModelFromNormals< pcl::PointXYZRGB, PointNT > > ConstPtr
std::string model_name_
The model name.
unsigned int getSampleSize() const
Return the size of a sample from which the model is computed.
typename PointCloud::ConstPtr PointCloudConstPtr
void drawIndexSample(std::vector< int > &sample)
Fills a sample array with random samples from the indices_ vector.
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
shared_ptr< const PointCloud< PointT > > ConstPtr
virtual bool doSamplesVerifyModel(const std::set< int > &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const =0
Verify whether a subset of indices verifies a given set of model coefficients.
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.
std::vector< int > shuffled_indices_
Data containing a shuffled version of the indices.
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.
IndicesPtr getIndices() const
Get a pointer to the vector of indices used.
shared_ptr< SampleConsensusModelFromNormals< pcl::PointXYZRGB, PointNT > > Ptr
boost::shared_ptr< T > shared_ptr
Alias for boost::shared_ptr.
void setIndices(const std::vector< int > &indices)
Provide the vector of indices that represents the input data.