41 #ifndef PCL_SAMPLE_CONSENSUS_MODEL_H_
42 #define PCL_SAMPLE_CONSENSUS_MODEL_H_
49 #include <pcl/console/print.h>
50 #include <pcl/point_cloud.h>
51 #include <pcl/sample_consensus/boost.h>
52 #include <pcl/sample_consensus/model_types.h>
54 #include <pcl/search/search.h>
58 template<
class T>
class ProgressiveSampleConsensus;
65 template <
typename Po
intT>
74 typedef boost::shared_ptr<SampleConsensusModel>
Ptr;
75 typedef boost::shared_ptr<const SampleConsensusModel>
ConstPtr;
84 ,
radius_min_ (-std::numeric_limits<double>::max ())
90 ,
rng_dist_ (new boost::uniform_int<> (0, std::numeric_limits<int>::max ()))
96 rng_alg_.seed (static_cast<unsigned> (std::time(0)));
111 ,
radius_min_ (-std::numeric_limits<double>::max ())
112 ,
radius_max_ (std::numeric_limits<double>::max ())
117 ,
rng_dist_ (new boost::uniform_int<> (0, std::numeric_limits<int>::max ()))
122 rng_alg_.seed (static_cast<unsigned> (std::time (0)));
139 const std::vector<int> &indices,
142 ,
indices_ (new std::vector<int> (indices))
143 ,
radius_min_ (-std::numeric_limits<double>::max ())
144 ,
radius_max_ (std::numeric_limits<double>::max ())
149 ,
rng_dist_ (new boost::uniform_int<> (0, std::numeric_limits<int>::max ()))
154 rng_alg_.seed (static_cast<unsigned> (std::time(0)));
160 PCL_ERROR (
"[pcl::SampleConsensusModel] Invalid index vector given with size %zu while the input PointCloud has size %zu!\n",
indices_->size (),
input_->points.size ());
183 PCL_ERROR (
"[pcl::SampleConsensusModel::getSamples] Can not select %zu unique points out of %zu!\n",
184 samples.size (),
indices_->size ());
187 iterations = INT_MAX - 1;
204 PCL_DEBUG (
"[pcl::SampleConsensusModel::getSamples] Selected %zu samples.\n", samples.size ());
208 PCL_DEBUG (
"[pcl::SampleConsensusModel::getSamples] WARNING: Could not select %d sample points in %d iterations!\n",
getSampleSize (), max_sample_checks_);
221 Eigen::VectorXf &model_coefficients) = 0;
235 const Eigen::VectorXf &model_coefficients,
236 Eigen::VectorXf &optimized_coefficients) = 0;
245 std::vector<double> &distances) = 0;
257 const double threshold,
258 std::vector<int> &inliers) = 0;
271 const double threshold) = 0;
283 const Eigen::VectorXf &model_coefficients,
285 bool copy_data_fields =
true) = 0;
297 const Eigen::VectorXf &model_coefficients,
298 const double threshold) = 0;
308 indices_.reset (
new std::vector<int> ());
312 indices_->resize (cloud->points.size ());
313 for (
size_t i = 0; i < cloud->points.size (); ++i)
314 (*
indices_)[i] =
static_cast<int> (i);
320 inline PointCloudConstPtr
327 setIndices (
const boost::shared_ptr <std::vector<int> > &indices)
339 indices_.reset (
new std::vector<int> (indices));
344 inline boost::shared_ptr <std::vector<int> >
415 std::vector<double> dists (error_sqr_dists);
416 std::sort (dists.begin (), dists.end ());
417 double median_error_sqr = dists[dists.size () >> 1];
418 return (2.1981 * median_error_sqr);
430 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");
431 return (std::numeric_limits<double>::quiet_NaN ());
443 size_t sample_size = sample.size ();
445 for (
unsigned int i = 0; i < sample_size; ++i)
460 size_t sample_size = sample.size ();
466 std::vector<int> indices;
467 std::vector<float> sqr_dists;
477 if (indices.size () < sample_size - 1)
480 for(
unsigned int i = 1; i < sample_size; ++i)
485 for (
unsigned int i = 0; i < sample_size-1; ++i)
486 std::swap (indices[i], indices[i + (
rnd () % (indices.size () - i))]);
487 for (
unsigned int i = 1; i < sample_size; ++i)
498 isModelValid (
const Eigen::VectorXf &model_coefficients) = 0;
505 isSampleGood (
const std::vector<int> &samples)
const = 0;
537 boost::shared_ptr<boost::variate_generator< boost::mt19937&, boost::uniform_int<> > >
rng_gen_;
549 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
555 template <
typename Po
intT,
typename Po
intNT>
562 typedef boost::shared_ptr<SampleConsensusModelFromNormals>
Ptr;
563 typedef boost::shared_ptr<const SampleConsensusModelFromNormals>
ConstPtr;
617 template<
typename _Scalar,
int NX=Eigen::Dynamic,
int NY=Eigen::Dynamic>
627 typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,1>
ValueType;
628 typedef Eigen::Matrix<Scalar,InputsAtCompileTime,1>
InputType;
629 typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime>
JacobianType;
637 Functor (
int m_data_points) : m_data_points_ (m_data_points) {}
643 values ()
const {
return (m_data_points_); }
646 const int m_data_points_;
650 #endif //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_H_
double radius_min_
The minimum and maximum radius limits for the model.
std::vector< double > error_sqr_dists_
A vector holding the distances to the computed model.
virtual void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances)=0
Compute all distances from the cloud data to a given model.
pcl::PointCloud< PointNT >::Ptr PointCloudNPtr
void drawIndexSample(std::vector< int > &sample)
Fills a sample array with random samples from the indices_ vector.
SampleConsensusModel(const PointCloudConstPtr &cloud, bool random=false)
Constructor for base SampleConsensusModel.
PointCloudConstPtr getInputCloud() const
Get a pointer to the input point cloud dataset.
boost::shared_ptr< std::vector< int > > getIndices() const
Get a pointer to the vector of indices used.
PointCloudConstPtr input_
A boost shared pointer to the point cloud data array.
std::vector< int > shuffled_indices_
Data containing a shuffled version of the indices.
virtual bool isSampleGood(const std::vector< int > &samples) const =0
Check if a sample of indices results in a good sample of points indices.
RandomSampleConsensus represents an implementation of the RANSAC (RAndom SAmple Consensus) algorithm...
Eigen::Matrix< Scalar, ValuesAtCompileTime, InputsAtCompileTime > JacobianType
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...
boost::shared_ptr< boost::uniform_int<> > rng_dist_
Boost-based random number generator distribution.
static const unsigned int max_sample_checks_
The maximum number of samples to try until we get a good one.
static const std::map< pcl::SacModel, unsigned int > SAC_SAMPLE_SIZE(sample_size_pairs, sample_size_pairs+sizeof(sample_size_pairs)/sizeof(SampleSizeModel))
boost::shared_ptr< const SampleConsensusModelFromNormals > ConstPtr
PointCloudNConstPtr getInputNormals()
Get a pointer to the normals of the input XYZ point cloud dataset.
virtual void optimizeModelCoefficients(const std::vector< int > &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)=0
Recompute the model coefficients using the given inlier set and return them to the user...
SearchPtr samples_radius_search_
The search object for picking subsequent samples using radius search.
An exception that is thrown when a sample consensus model doesn't have the correct number of samples ...
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
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.
boost::mt19937 rng_alg_
Boost-based random number generator algorithm.
Eigen::Matrix< Scalar, InputsAtCompileTime, 1 > InputType
Base functor all the models that need non linear optimization must define their own one and implement...
Functor(int m_data_points)
Constructor.
void drawIndexSampleRadius(std::vector< int > &sample)
Fills a sample array with one random sample from the indices_ vector and other random samples that ar...
int values() const
Get the number of values.
SampleConsensusModelFromNormals()
Empty constructor for base SampleConsensusModelFromNormals.
void setIndices(const std::vector< int > &indices)
Provide the vector of indices that represents the input data.
pcl::PointCloud< PointNT >::ConstPtr PointCloudNConstPtr
void getRadiusLimits(double &min_radius, double &max_radius)
Get the minimum and maximum allowable radius limits for the model as set by the user.
SampleConsensusModel(const PointCloudConstPtr &cloud, const std::vector< int > &indices, bool random=false)
Constructor for base SampleConsensusModel.
double normal_distance_weight_
The relative weight (between 0 and 1) to give to the angular distance (0 to pi/2) between point norma...
SampleConsensusModelFromNormals represents the base model class for models that require the use of su...
Eigen::Matrix< Scalar, ValuesAtCompileTime, 1 > ValueType
void getSamplesMaxDist(double &radius)
Get maximum distance allowed when drawing random samples.
void setNormalDistanceWeight(const double w)
Set the normal angular distance weight.
pcl::PointCloud< PointT >::Ptr PointCloudPtr
boost::shared_ptr< pcl::search::Search< PointT > > Ptr
boost::shared_ptr< PointCloud< PointT > > Ptr
virtual ~SampleConsensusModel()
Destructor for base SampleConsensusModel.
double computeVariance()
Compute the variance of the errors to the model from the internally estimated vector of distances...
virtual SacModel getModelType() const =0
Return an unique id for each type of model employed.
PointCloudNConstPtr normals_
A pointer to the input dataset that contains the point normals of the XYZ dataset.
double getNormalDistanceWeight()
Get the normal angular distance weight.
void setSamplesMaxDist(const double &radius, SearchPtr search)
Set the maximum distance allowed when drawing random samples.
virtual bool isModelValid(const Eigen::VectorXf &model_coefficients)=0
Check whether a model is valid given the user constraints.
int rnd()
Boost-based random number generator.
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
boost::shared_ptr< SampleConsensusModelFromNormals > Ptr
virtual bool doSamplesVerifyModel(const std::set< int > &indices, const Eigen::VectorXf &model_coefficients, const double threshold)=0
Verify whether a subset of indices verifies a given set of model coefficients.
pcl::PointCloud< PointT > PointCloud
virtual int countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold)=0
Count all the points which respect the given model coefficients as inliers.
virtual void projectPoints(const std::vector< int > &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields=true)=0
Create a new point cloud with inliers projected onto the model.
virtual bool computeModelCoefficients(const std::vector< int > &samples, Eigen::VectorXf &model_coefficients)=0
Check whether the given index samples can form a valid model, compute the model coefficients from the...
pcl::PointCloud< PointT >::ConstPtr PointCloudConstPtr
Functor()
Empty Construtor.
void setIndices(const boost::shared_ptr< std::vector< int > > &indices)
Provide a pointer to the vector of indices that represents the input data.
boost::shared_ptr< SampleConsensusModel > Ptr
pcl::search::Search< PointT >::Ptr SearchPtr
double computeVariance(const std::vector< double > &error_sqr_dists)
Compute the variance of the errors to the model.
A point structure representing Euclidean xyz coordinates, and the RGB color.
double samples_radius_
The maximum distance of subsequent samples from the first (radius search)
boost::shared_ptr< const SampleConsensusModel > ConstPtr
SampleConsensusModel represents the base model class.
SampleConsensusModel(bool random=false)
Empty constructor for base SampleConsensusModel.
unsigned int getSampleSize() const
Return the size of a sample from which a model is computed.
virtual ~SampleConsensusModelFromNormals()
Destructor.
boost::shared_ptr< std::vector< int > > indices_
A pointer to the vector of point indices to use.
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input dataset that contains the point normals of the XYZ dataset...
boost::shared_ptr< boost::variate_generator< boost::mt19937 &, boost::uniform_int<> > > rng_gen_
Boost-based random number generator.
virtual void getSamples(int &iterations, std::vector< int > &samples)
Get a set of random data samples and return them as point indices.