51 #include <pcl/pcl_base.h>
52 #include <pcl/console/print.h>
53 #include <pcl/point_cloud.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> >;
87 ,
radius_min_ (-std::numeric_limits<double>::max ())
91 ,
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 ()))
116 rng_alg_.seed (
static_cast<unsigned> (std::time (
nullptr)));
137 ,
radius_min_ (-std::numeric_limits<double>::max ())
138 ,
radius_max_ (std::numeric_limits<double>::max ())
141 ,
rng_dist_ (new
boost::uniform_int<> (0, std::numeric_limits<int>::max ()))
144 rng_alg_.seed (
static_cast<unsigned> (std::time(
nullptr)));
150 PCL_ERROR(
"[pcl::SampleConsensusModel] Invalid index vector given with size "
151 "%zu while the input PointCloud has size %zu!\n",
153 static_cast<std::size_t
>(
input_->size()));
176 PCL_ERROR (
"[pcl::SampleConsensusModel::getSamples] Can not select %lu unique points out of %lu!\n",
177 samples.size (),
indices_->size ());
180 iterations = INT_MAX - 1;
197 PCL_DEBUG (
"[pcl::SampleConsensusModel::getSamples] Selected %lu samples.\n", samples.size ());
201 PCL_DEBUG (
"[pcl::SampleConsensusModel::getSamples] WARNING: Could not select %d sample points in %d iterations!\n",
getSampleSize (),
max_sample_checks_);
215 Eigen::VectorXf &model_coefficients)
const = 0;
229 const Eigen::VectorXf &model_coefficients,
230 Eigen::VectorXf &optimized_coefficients)
const = 0;
239 std::vector<double> &distances)
const = 0;
251 const double threshold,
265 const double threshold)
const = 0;
277 const Eigen::VectorXf &model_coefficients,
279 bool copy_data_fields =
true)
const = 0;
291 const Eigen::VectorXf &model_coefficients,
292 const double threshold)
const = 0;
307 for (std::size_t i = 0; i < cloud->size (); ++i)
314 inline PointCloudConstPtr
346 inline const std::string&
421 std::vector<double> dists (error_sqr_dists);
422 const std::size_t medIdx = dists.size () >> 1;
423 std::nth_element (dists.begin (), dists.begin () + medIdx, dists.end ());
424 double median_error_sqr = dists[medIdx];
425 return (2.1981 * median_error_sqr);
437 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");
438 return (std::numeric_limits<double>::quiet_NaN ());
451 std::size_t sample_size = sample.size ();
453 for (std::size_t i = 0; i < sample_size; ++i)
468 std::size_t sample_size = sample.size ();
475 std::vector<float> sqr_dists;
485 if (indices.size () < sample_size - 1)
488 for(std::size_t i = 1; i < sample_size; ++i)
493 for (std::size_t i = 0; i < sample_size-1; ++i)
494 std::swap (indices[i], indices[i + (
rnd () % (indices.size () - i))]);
495 for (std::size_t i = 1; i < sample_size; ++i)
514 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_);
560 std::shared_ptr<boost::variate_generator< boost::mt19937&, boost::uniform_int<> > >
rng_gen_;
584 template <
typename Po
intT,
typename Po
intNT>
591 using Ptr = shared_ptr<SampleConsensusModelFromNormals<PointT, PointNT> >;
592 using ConstPtr = shared_ptr<const SampleConsensusModelFromNormals<PointT, PointNT> >;
646 template<
typename _Scalar,
int NX=Eigen::Dynamic,
int NY=Eigen::Dynamic>
656 using ValueType = Eigen::Matrix<Scalar,ValuesAtCompileTime,1>;
657 using InputType = Eigen::Matrix<Scalar,InputsAtCompileTime,1>;
658 using JacobianType = Eigen::Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime>;
666 Functor (
int m_data_points) : m_data_points_ (m_data_points) {}
672 values ()
const {
return (m_data_points_); }
675 const int m_data_points_;
PointCloud represents the base class in PCL for storing collections of 3D points.
shared_ptr< PointCloud< PointT > > Ptr
shared_ptr< const PointCloud< PointT > > ConstPtr
RandomSampleConsensus represents an implementation of the RANSAC (RAndom SAmple Consensus) algorithm,...
SampleConsensusModelFromNormals represents the base model class for models that require the use of su...
virtual ~SampleConsensusModelFromNormals()
Destructor.
void setNormalDistanceWeight(const double w)
Set the normal angular distance weight.
PointCloudNConstPtr normals_
A pointer to the input dataset that contains the point normals of the XYZ dataset.
typename pcl::PointCloud< PointNT >::ConstPtr PointCloudNConstPtr
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input dataset that contains the point normals of the XYZ dataset.
double getNormalDistanceWeight() const
Get the normal angular distance weight.
SampleConsensusModelFromNormals()
Empty constructor for base SampleConsensusModelFromNormals.
shared_ptr< const SampleConsensusModelFromNormals< PointT, PointNT > > ConstPtr
typename pcl::PointCloud< PointNT >::Ptr PointCloudNPtr
double normal_distance_weight_
The relative weight (between 0 and 1) to give to the angular distance (0 to pi/2) between point norma...
PointCloudNConstPtr getInputNormals() const
Get a pointer to the normals of the input XYZ point cloud dataset.
shared_ptr< SampleConsensusModelFromNormals< PointT, PointNT > > Ptr
SampleConsensusModel represents the base model class.
SampleConsensusModel(const PointCloudConstPtr &cloud, const Indices &indices, bool random=false)
Constructor for base SampleConsensusModel.
virtual void getSamples(int &iterations, Indices &samples)
Get a set of random data samples and return them as point indices.
static const unsigned int max_sample_checks_
The maximum number of samples to try until we get a good one.
virtual bool isSampleGood(const Indices &samples) const =0
Check if a sample of indices results in a good sample of points indices.
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...
virtual SacModel getModelType() const =0
Return a unique id for each type of model employed.
void drawIndexSampleRadius(Indices &sample)
Fills a sample array with one random sample from the indices_ vector and other random samples that ar...
unsigned int getModelSize() const
Return the number of coefficients in the model.
double radius_min_
The minimum and maximum radius limits for the model.
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...
void getSamplesMaxDist(double &radius) const
Get maximum distance allowed when drawing random samples.
PointCloudConstPtr getInputCloud() const
Get a pointer to the input point cloud dataset.
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.
shared_ptr< SampleConsensusModel< PointT > > Ptr
const std::string & getClassName() const
Get a string representation of the name of this class.
SearchPtr samples_radius_search_
The search object for picking subsequent samples using radius search.
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.
double computeVariance(const std::vector< double > &error_sqr_dists) const
Compute the variance of the errors to the model.
unsigned int sample_size_
The size of a sample from which the model is computed.
typename PointCloud::ConstPtr PointCloudConstPtr
IndicesPtr getIndices() const
Get a pointer to the vector of indices used.
std::shared_ptr< boost::variate_generator< boost::mt19937 &, boost::uniform_int<> > > rng_gen_
Boost-based random number generator.
IndicesPtr indices_
A pointer to the vector of point indices to use.
double computeVariance() const
Compute the variance of the errors to the model from the internally estimated vector of distances.
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.
Indices shuffled_indices_
Data containing a shuffled version of the indices.
boost::mt19937 rng_alg_
Boost-based random number generator algorithm.
PointCloudConstPtr input_
A boost shared pointer to the point cloud data array.
virtual bool isModelValid(const Eigen::VectorXf &model_coefficients) const
Check whether a model is valid given the user constraints.
void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
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::shared_ptr< boost::uniform_int<> > rng_dist_
Boost-based random number generator distribution.
void setIndices(const Indices &indices)
Provide the vector of indices that represents the input data.
SampleConsensusModel(const PointCloudConstPtr &cloud, bool random=false)
Constructor for base SampleConsensusModel.
double samples_radius_
The maximum distance of subsequent samples from the first (radius search)
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
std::string model_name_
The model name.
unsigned int model_size_
The number of coefficients in the model.
int rnd()
Boost-based random number generator.
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::search::Search< PointT >::Ptr SearchPtr
void drawIndexSample(Indices &sample)
Fills a sample array with random samples from the indices_ vector.
typename PointCloud::Ptr PointCloudPtr
shared_ptr< const SampleConsensusModel< PointT > > ConstPtr
std::vector< double > error_sqr_dists_
A vector holding the distances to the computed model.
void setSamplesMaxDist(const double &radius, SearchPtr search)
Set the maximum distance allowed when drawing random samples.
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.
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 getSampleSize() const
Return the size of a sample from which the model is computed.
shared_ptr< pcl::search::Search< PointT > > Ptr
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Defines functions, macros and traits for allocating and using memory.
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
IndicesAllocator<> Indices
Type used for indices in PCL.
shared_ptr< Indices > IndicesPtr
Defines all the PCL and non-PCL macros used.
Base functor all the models that need non linear optimization must define their own one and implement...
int values() const
Get the number of values.
Functor()
Empty Constructor.
Eigen::Matrix< Scalar, InputsAtCompileTime, 1 > InputType
Eigen::Matrix< Scalar, ValuesAtCompileTime, 1 > ValueType
Eigen::Matrix< Scalar, ValuesAtCompileTime, InputsAtCompileTime > JacobianType
Functor(int m_data_points)
Constructor.
A point structure representing Euclidean xyz coordinates, and the RGB color.
Defines basic non-point types used by PCL.