Point Cloud Library (PCL)  1.7.2
sac_model.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  * $Id$
38  *
39  */
40 
41 #ifndef PCL_SAMPLE_CONSENSUS_MODEL_H_
42 #define PCL_SAMPLE_CONSENSUS_MODEL_H_
43 
44 #include <cfloat>
45 #include <ctime>
46 #include <limits.h>
47 #include <set>
48 
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>
53 
54 #include <pcl/search/search.h>
55 
56 namespace pcl
57 {
58  template<class T> class ProgressiveSampleConsensus;
59 
60  /** \brief @b SampleConsensusModel represents the base model class. All sample consensus models must inherit
61  * from this class.
62  * \author Radu B. Rusu
63  * \ingroup sample_consensus
64  */
65  template <typename PointT>
67  {
68  public:
73 
74  typedef boost::shared_ptr<SampleConsensusModel> Ptr;
75  typedef boost::shared_ptr<const SampleConsensusModel> ConstPtr;
76 
77  protected:
78  /** \brief Empty constructor for base SampleConsensusModel.
79  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
80  */
81  SampleConsensusModel (bool random = false)
82  : input_ ()
83  , indices_ ()
84  , radius_min_ (-std::numeric_limits<double>::max ())
85  , radius_max_ (std::numeric_limits<double>::max ())
86  , samples_radius_ (0.)
89  , rng_alg_ ()
90  , rng_dist_ (new boost::uniform_int<> (0, std::numeric_limits<int>::max ()))
91  , rng_gen_ ()
92  , error_sqr_dists_ ()
93  {
94  // Create a random number generator object
95  if (random)
96  rng_alg_.seed (static_cast<unsigned> (std::time(0)));
97  else
98  rng_alg_.seed (12345u);
99 
100  rng_gen_.reset (new boost::variate_generator<boost::mt19937&, boost::uniform_int<> > (rng_alg_, *rng_dist_));
101  }
102 
103  public:
104  /** \brief Constructor for base SampleConsensusModel.
105  * \param[in] cloud the input point cloud dataset
106  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
107  */
108  SampleConsensusModel (const PointCloudConstPtr &cloud, bool random = false)
109  : input_ ()
110  , indices_ ()
111  , radius_min_ (-std::numeric_limits<double>::max ())
112  , radius_max_ (std::numeric_limits<double>::max ())
113  , samples_radius_ (0.)
115  , shuffled_indices_ ()
116  , rng_alg_ ()
117  , rng_dist_ (new boost::uniform_int<> (0, std::numeric_limits<int>::max ()))
118  , rng_gen_ ()
119  , error_sqr_dists_ ()
120  {
121  if (random)
122  rng_alg_.seed (static_cast<unsigned> (std::time (0)));
123  else
124  rng_alg_.seed (12345u);
125 
126  // Sets the input cloud and creates a vector of "fake" indices
127  setInputCloud (cloud);
128 
129  // Create a random number generator object
130  rng_gen_.reset (new boost::variate_generator<boost::mt19937&, boost::uniform_int<> > (rng_alg_, *rng_dist_));
131  }
132 
133  /** \brief Constructor for base SampleConsensusModel.
134  * \param[in] cloud the input point cloud dataset
135  * \param[in] indices a vector of point indices to be used from \a cloud
136  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
137  */
138  SampleConsensusModel (const PointCloudConstPtr &cloud,
139  const std::vector<int> &indices,
140  bool random = false)
141  : input_ (cloud)
142  , indices_ (new std::vector<int> (indices))
143  , radius_min_ (-std::numeric_limits<double>::max ())
144  , radius_max_ (std::numeric_limits<double>::max ())
145  , samples_radius_ (0.)
147  , shuffled_indices_ ()
148  , rng_alg_ ()
149  , rng_dist_ (new boost::uniform_int<> (0, std::numeric_limits<int>::max ()))
150  , rng_gen_ ()
151  , error_sqr_dists_ ()
152  {
153  if (random)
154  rng_alg_.seed (static_cast<unsigned> (std::time(0)));
155  else
156  rng_alg_.seed (12345u);
157 
158  if (indices_->size () > input_->points.size ())
159  {
160  PCL_ERROR ("[pcl::SampleConsensusModel] Invalid index vector given with size %lu while the input PointCloud has size %lu!\n", indices_->size (), input_->points.size ());
161  indices_->clear ();
162  }
164 
165  // Create a random number generator object
166  rng_gen_.reset (new boost::variate_generator<boost::mt19937&, boost::uniform_int<> > (rng_alg_, *rng_dist_));
167  };
168 
169  /** \brief Destructor for base SampleConsensusModel. */
170  virtual ~SampleConsensusModel () {};
171 
172  /** \brief Get a set of random data samples and return them as point
173  * indices.
174  * \param[out] iterations the internal number of iterations used by SAC methods
175  * \param[out] samples the resultant model samples
176  */
177  virtual void
178  getSamples (int &iterations, std::vector<int> &samples)
179  {
180  // We're assuming that indices_ have already been set in the constructor
181  if (indices_->size () < getSampleSize ())
182  {
183  PCL_ERROR ("[pcl::SampleConsensusModel::getSamples] Can not select %lu unique points out of %lu!\n",
184  samples.size (), indices_->size ());
185  // one of these will make it stop :)
186  samples.clear ();
187  iterations = INT_MAX - 1;
188  return;
189  }
190 
191  // Get a second point which is different than the first
192  samples.resize (getSampleSize ());
193  for (unsigned int iter = 0; iter < max_sample_checks_; ++iter)
194  {
195  // Choose the random indices
196  if (samples_radius_ < std::numeric_limits<double>::epsilon ())
198  else
200 
201  // If it's a good sample, stop here
202  if (isSampleGood (samples))
203  {
204  PCL_DEBUG ("[pcl::SampleConsensusModel::getSamples] Selected %lu samples.\n", samples.size ());
205  return;
206  }
207  }
208  PCL_DEBUG ("[pcl::SampleConsensusModel::getSamples] WARNING: Could not select %d sample points in %d iterations!\n", getSampleSize (), max_sample_checks_);
209  samples.clear ();
210  }
211 
212  /** \brief Check whether the given index samples can form a valid model,
213  * compute the model coefficients from these samples and store them
214  * in model_coefficients. Pure virtual.
215  * \param[in] samples the point indices found as possible good candidates
216  * for creating a valid model
217  * \param[out] model_coefficients the computed model coefficients
218  */
219  virtual bool
220  computeModelCoefficients (const std::vector<int> &samples,
221  Eigen::VectorXf &model_coefficients) = 0;
222 
223  /** \brief Recompute the model coefficients using the given inlier set
224  * and return them to the user. Pure virtual.
225  *
226  * @note: these are the coefficients of the model after refinement
227  * (e.g., after a least-squares optimization)
228  *
229  * \param[in] inliers the data inliers supporting the model
230  * \param[in] model_coefficients the initial guess for the model coefficients
231  * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization
232  */
233  virtual void
234  optimizeModelCoefficients (const std::vector<int> &inliers,
235  const Eigen::VectorXf &model_coefficients,
236  Eigen::VectorXf &optimized_coefficients) = 0;
237 
238  /** \brief Compute all distances from the cloud data to a given model. Pure virtual.
239  *
240  * \param[in] model_coefficients the coefficients of a model that we need to compute distances to
241  * \param[out] distances the resultant estimated distances
242  */
243  virtual void
244  getDistancesToModel (const Eigen::VectorXf &model_coefficients,
245  std::vector<double> &distances) = 0;
246 
247  /** \brief Select all the points which respect the given model
248  * coefficients as inliers. Pure virtual.
249  *
250  * \param[in] model_coefficients the coefficients of a model that we need to compute distances to
251  * \param[in] threshold a maximum admissible distance threshold for determining the inliers from
252  * the outliers
253  * \param[out] inliers the resultant model inliers
254  */
255  virtual void
256  selectWithinDistance (const Eigen::VectorXf &model_coefficients,
257  const double threshold,
258  std::vector<int> &inliers) = 0;
259 
260  /** \brief Count all the points which respect the given model
261  * coefficients as inliers. Pure virtual.
262  *
263  * \param[in] model_coefficients the coefficients of a model that we need to
264  * compute distances to
265  * \param[in] threshold a maximum admissible distance threshold for
266  * determining the inliers from the outliers
267  * \return the resultant number of inliers
268  */
269  virtual int
270  countWithinDistance (const Eigen::VectorXf &model_coefficients,
271  const double threshold) = 0;
272 
273  /** \brief Create a new point cloud with inliers projected onto the model. Pure virtual.
274  * \param[in] inliers the data inliers that we want to project on the model
275  * \param[in] model_coefficients the coefficients of a model
276  * \param[out] projected_points the resultant projected points
277  * \param[in] copy_data_fields set to true (default) if we want the \a
278  * projected_points cloud to be an exact copy of the input dataset minus
279  * the point projections on the plane model
280  */
281  virtual void
282  projectPoints (const std::vector<int> &inliers,
283  const Eigen::VectorXf &model_coefficients,
284  PointCloud &projected_points,
285  bool copy_data_fields = true) = 0;
286 
287  /** \brief Verify whether a subset of indices verifies a given set of
288  * model coefficients. Pure virtual.
289  *
290  * \param[in] indices the data indices that need to be tested against the model
291  * \param[in] model_coefficients the set of model coefficients
292  * \param[in] threshold a maximum admissible distance threshold for
293  * determining the inliers from the outliers
294  */
295  virtual bool
296  doSamplesVerifyModel (const std::set<int> &indices,
297  const Eigen::VectorXf &model_coefficients,
298  const double threshold) = 0;
299 
300  /** \brief Provide a pointer to the input dataset
301  * \param[in] cloud the const boost shared pointer to a PointCloud message
302  */
303  inline virtual void
304  setInputCloud (const PointCloudConstPtr &cloud)
305  {
306  input_ = cloud;
307  if (!indices_)
308  indices_.reset (new std::vector<int> ());
309  if (indices_->empty ())
310  {
311  // Prepare a set of indices to be used (entire cloud)
312  indices_->resize (cloud->points.size ());
313  for (size_t i = 0; i < cloud->points.size (); ++i)
314  (*indices_)[i] = static_cast<int> (i);
315  }
317  }
318 
319  /** \brief Get a pointer to the input point cloud dataset. */
320  inline PointCloudConstPtr
321  getInputCloud () const { return (input_); }
322 
323  /** \brief Provide a pointer to the vector of indices that represents the input data.
324  * \param[in] indices a pointer to the vector of indices that represents the input data.
325  */
326  inline void
327  setIndices (const boost::shared_ptr <std::vector<int> > &indices)
328  {
329  indices_ = indices;
331  }
332 
333  /** \brief Provide the vector of indices that represents the input data.
334  * \param[out] indices the vector of indices that represents the input data.
335  */
336  inline void
337  setIndices (const std::vector<int> &indices)
338  {
339  indices_.reset (new std::vector<int> (indices));
340  shuffled_indices_ = indices;
341  }
342 
343  /** \brief Get a pointer to the vector of indices used. */
344  inline boost::shared_ptr <std::vector<int> >
345  getIndices () const { return (indices_); }
346 
347  /** \brief Return an unique id for each type of model employed. */
348  virtual SacModel
349  getModelType () const = 0;
350 
351  /** \brief Return the size of a sample from which a model is computed */
352  inline unsigned int
353  getSampleSize () const
354  {
355  std::map<pcl::SacModel, unsigned int>::const_iterator it = SAC_SAMPLE_SIZE.find (getModelType ());
356  if (it == SAC_SAMPLE_SIZE.end ())
357  throw InvalidSACModelTypeException ("No sample size defined for given model type!\n");
358  return (it->second);
359  }
360 
361  /** \brief Set the minimum and maximum allowable radius limits for the
362  * model (applicable to models that estimate a radius)
363  * \param[in] min_radius the minimum radius model
364  * \param[in] max_radius the maximum radius model
365  * \todo change this to set limits on the entire model
366  */
367  inline void
368  setRadiusLimits (const double &min_radius, const double &max_radius)
369  {
370  radius_min_ = min_radius;
371  radius_max_ = max_radius;
372  }
373 
374  /** \brief Get the minimum and maximum allowable radius limits for the
375  * model as set by the user.
376  *
377  * \param[out] min_radius the resultant minimum radius model
378  * \param[out] max_radius the resultant maximum radius model
379  */
380  inline void
381  getRadiusLimits (double &min_radius, double &max_radius)
382  {
383  min_radius = radius_min_;
384  max_radius = radius_max_;
385  }
386 
387  /** \brief Set the maximum distance allowed when drawing random samples
388  * \param[in] radius the maximum distance (L2 norm)
389  * \param search
390  */
391  inline void
392  setSamplesMaxDist (const double &radius, SearchPtr search)
393  {
394  samples_radius_ = radius;
395  samples_radius_search_ = search;
396  }
397 
398  /** \brief Get maximum distance allowed when drawing random samples
399  *
400  * \param[out] radius the maximum distance (L2 norm)
401  */
402  inline void
403  getSamplesMaxDist (double &radius)
404  {
405  radius = samples_radius_;
406  }
407 
409 
410  /** \brief Compute the variance of the errors to the model.
411  * \param[in] error_sqr_dists a vector holding the distances
412  */
413  inline double
414  computeVariance (const std::vector<double> &error_sqr_dists)
415  {
416  std::vector<double> dists (error_sqr_dists);
417  std::sort (dists.begin (), dists.end ());
418  double median_error_sqr = dists[dists.size () >> 1];
419  return (2.1981 * median_error_sqr);
420  }
421 
422  /** \brief Compute the variance of the errors to the model from the internally
423  * estimated vector of distances. The model must be computed first (or at least
424  * selectWithinDistance must be called).
425  */
426  inline double
428  {
429  if (error_sqr_dists_.empty ())
430  {
431  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");
432  return (std::numeric_limits<double>::quiet_NaN ());
433  }
435  }
436 
437  protected:
438  /** \brief Fills a sample array with random samples from the indices_ vector
439  * \param[out] sample the set of indices of target_ to analyze
440  */
441  inline void
442  drawIndexSample (std::vector<int> &sample)
443  {
444  size_t sample_size = sample.size ();
445  size_t index_size = shuffled_indices_.size ();
446  for (unsigned int i = 0; i < sample_size; ++i)
447  // The 1/(RAND_MAX+1.0) trick is when the random numbers are not uniformly distributed and for small modulo
448  // elements, that does not matter (and nowadays, random number generators are good)
449  //std::swap (shuffled_indices_[i], shuffled_indices_[i + (rand () % (index_size - i))]);
450  std::swap (shuffled_indices_[i], shuffled_indices_[i + (rnd () % (index_size - i))]);
451  std::copy (shuffled_indices_.begin (), shuffled_indices_.begin () + sample_size, sample.begin ());
452  }
453 
454  /** \brief Fills a sample array with one random sample from the indices_ vector
455  * and other random samples that are closer than samples_radius_
456  * \param[out] sample the set of indices of target_ to analyze
457  */
458  inline void
459  drawIndexSampleRadius (std::vector<int> &sample)
460  {
461  size_t sample_size = sample.size ();
462  size_t index_size = shuffled_indices_.size ();
463 
464  std::swap (shuffled_indices_[0], shuffled_indices_[0 + (rnd () % (index_size - 0))]);
465  //const PointT& pt0 = (*input_)[shuffled_indices_[0]];
466 
467  std::vector<int> indices;
468  std::vector<float> sqr_dists;
469 
470  // If indices have been set when the search object was constructed,
471  // radiusSearch() expects an index into the indices vector as its
472  // first parameter. This can't be determined efficiently, so we use
473  // the point instead of the index.
474  // Returned indices are converted automatically.
475  samples_radius_search_->radiusSearch (input_->at(shuffled_indices_[0]),
476  samples_radius_, indices, sqr_dists );
477 
478  if (indices.size () < sample_size - 1)
479  {
480  // radius search failed, make an invalid model
481  for(unsigned int i = 1; i < sample_size; ++i)
483  }
484  else
485  {
486  for (unsigned int i = 0; i < sample_size-1; ++i)
487  std::swap (indices[i], indices[i + (rnd () % (indices.size () - i))]);
488  for (unsigned int i = 1; i < sample_size; ++i)
489  shuffled_indices_[i] = indices[i-1];
490  }
491 
492  std::copy (shuffled_indices_.begin (), shuffled_indices_.begin () + sample_size, sample.begin ());
493  }
494 
495  /** \brief Check whether a model is valid given the user constraints.
496  * \param[in] model_coefficients the set of model coefficients
497  */
498  virtual inline bool
499  isModelValid (const Eigen::VectorXf &model_coefficients) = 0;
500 
501  /** \brief Check if a sample of indices results in a good sample of points
502  * indices. Pure virtual.
503  * \param[in] samples the resultant index samples
504  */
505  virtual bool
506  isSampleGood (const std::vector<int> &samples) const = 0;
507 
508  /** \brief A boost shared pointer to the point cloud data array. */
509  PointCloudConstPtr input_;
510 
511  /** \brief A pointer to the vector of point indices to use. */
512  boost::shared_ptr <std::vector<int> > indices_;
513 
514  /** The maximum number of samples to try until we get a good one */
515  static const unsigned int max_sample_checks_ = 1000;
516 
517  /** \brief The minimum and maximum radius limits for the model.
518  * Applicable to all models that estimate a radius.
519  */
521 
522  /** \brief The maximum distance of subsequent samples from the first (radius search) */
524 
525  /** \brief The search object for picking subsequent samples using radius search */
527 
528  /** Data containing a shuffled version of the indices. This is used and modified when drawing samples. */
529  std::vector<int> shuffled_indices_;
530 
531  /** \brief Boost-based random number generator algorithm. */
532  boost::mt19937 rng_alg_;
533 
534  /** \brief Boost-based random number generator distribution. */
535  boost::shared_ptr<boost::uniform_int<> > rng_dist_;
536 
537  /** \brief Boost-based random number generator. */
538  boost::shared_ptr<boost::variate_generator< boost::mt19937&, boost::uniform_int<> > > rng_gen_;
539 
540  /** \brief A vector holding the distances to the computed model. Used internally. */
541  std::vector<double> error_sqr_dists_;
542 
543  /** \brief Boost-based random number generator. */
544  inline int
545  rnd ()
546  {
547  return ((*rng_gen_) ());
548  }
549  public:
550  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
551  };
552 
553  /** \brief @b SampleConsensusModelFromNormals represents the base model class
554  * for models that require the use of surface normals for estimation.
555  */
556  template <typename PointT, typename PointNT>
557  class SampleConsensusModelFromNormals //: public SampleConsensusModel<PointT>
558  {
559  public:
562 
563  typedef boost::shared_ptr<SampleConsensusModelFromNormals> Ptr;
564  typedef boost::shared_ptr<const SampleConsensusModelFromNormals> ConstPtr;
565 
566  /** \brief Empty constructor for base SampleConsensusModelFromNormals. */
567  SampleConsensusModelFromNormals () : normal_distance_weight_ (0.0), normals_ () {};
568 
569  /** \brief Destructor. */
571 
572  /** \brief Set the normal angular distance weight.
573  * \param[in] w the relative weight (between 0 and 1) to give to the angular
574  * distance (0 to pi/2) between point normals and the plane normal.
575  * (The Euclidean distance will have weight 1-w.)
576  */
577  inline void
578  setNormalDistanceWeight (const double w)
579  {
580  normal_distance_weight_ = w;
581  }
582 
583  /** \brief Get the normal angular distance weight. */
584  inline double
585  getNormalDistanceWeight () { return (normal_distance_weight_); }
586 
587  /** \brief Provide a pointer to the input dataset that contains the point
588  * normals of the XYZ dataset.
589  *
590  * \param[in] normals the const boost shared pointer to a PointCloud message
591  */
592  inline void
593  setInputNormals (const PointCloudNConstPtr &normals)
594  {
595  normals_ = normals;
596  }
597 
598  /** \brief Get a pointer to the normals of the input XYZ point cloud dataset. */
599  inline PointCloudNConstPtr
600  getInputNormals () { return (normals_); }
601 
602  protected:
603  /** \brief The relative weight (between 0 and 1) to give to the angular
604  * distance (0 to pi/2) between point normals and the plane normal.
605  */
607 
608  /** \brief A pointer to the input dataset that contains the point normals
609  * of the XYZ dataset.
610  */
611  PointCloudNConstPtr normals_;
612  };
613 
614  /** Base functor all the models that need non linear optimization must
615  * define their own one and implement operator() (const Eigen::VectorXd& x, Eigen::VectorXd& fvec)
616  * or operator() (const Eigen::VectorXf& x, Eigen::VectorXf& fvec) dependening on the choosen _Scalar
617  */
618  template<typename _Scalar, int NX=Eigen::Dynamic, int NY=Eigen::Dynamic>
619  struct Functor
620  {
621  typedef _Scalar Scalar;
622  enum
623  {
624  InputsAtCompileTime = NX,
625  ValuesAtCompileTime = NY
626  };
627 
628  typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,1> ValueType;
629  typedef Eigen::Matrix<Scalar,InputsAtCompileTime,1> InputType;
630  typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType;
631 
632  /** \brief Empty Construtor. */
633  Functor () : m_data_points_ (ValuesAtCompileTime) {}
634 
635  /** \brief Constructor
636  * \param[in] m_data_points number of data points to evaluate.
637  */
638  Functor (int m_data_points) : m_data_points_ (m_data_points) {}
639 
640  virtual ~Functor () {}
641 
642  /** \brief Get the number of values. */
643  int
644  values () const { return (m_data_points_); }
645 
646  private:
647  const int m_data_points_;
648  };
649 }
650 
651 #endif //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_H_
An exception that is thrown when a sample consensus model doesn&#39;t have the correct number of samples ...
Definition: exceptions.h:166
double normal_distance_weight_
The relative weight (between 0 and 1) to give to the angular distance (0 to pi/2) between point norma...
Definition: sac_model.h:606
double radius_min_
The minimum and maximum radius limits for the model.
Definition: sac_model.h:520
double computeVariance()
Compute the variance of the errors to the model from the internally estimated vector of distances...
Definition: sac_model.h:427
boost::shared_ptr< const SampleConsensusModelFromNormals > ConstPtr
Definition: sac_model.h:564
pcl::PointCloud< PointNT >::ConstPtr PointCloudNConstPtr
Definition: sac_model.h:560
SampleConsensusModelFromNormals()
Empty constructor for base SampleConsensusModelFromNormals.
Definition: sac_model.h:567
void setIndices(const std::vector< int > &indices)
Provide the vector of indices that represents the input data.
Definition: sac_model.h:337
virtual ~Functor()
Definition: sac_model.h:640
boost::shared_ptr< boost::uniform_int<> > rng_dist_
Boost-based random number generator distribution.
Definition: sac_model.h:535
virtual SacModel getModelType() const =0
Return an unique id for each type of model employed.
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...
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.
int values() const
Get the number of values.
Definition: sac_model.h:644
SampleConsensusModel(bool random=false)
Empty constructor for base SampleConsensusModel.
Definition: sac_model.h:81
RandomSampleConsensus represents an implementation of the RANSAC (RAndom SAmple Consensus) algorithm...
Definition: prosac.h:56
PointCloudConstPtr getInputCloud() const
Get a pointer to the input point cloud dataset.
Definition: sac_model.h:321
void setIndices(const boost::shared_ptr< std::vector< int > > &indices)
Provide a pointer to the vector of indices that represents the input data.
Definition: sac_model.h:327
SampleConsensusModel(const PointCloudConstPtr &cloud, bool random=false)
Constructor for base SampleConsensusModel.
Definition: sac_model.h:108
pcl::PointCloud< PointNT >::Ptr PointCloudNPtr
Definition: sac_model.h:561
Base functor all the models that need non linear optimization must define their own one and implement...
Definition: sac_model.h:619
Eigen::Matrix< Scalar, ValuesAtCompileTime, 1 > ValueType
Definition: sac_model.h:628
boost::shared_ptr< std::vector< int > > indices_
A pointer to the vector of point indices to use.
Definition: sac_model.h:512
double samples_radius_
The maximum distance of subsequent samples from the first (radius search)
Definition: sac_model.h:523
virtual bool isSampleGood(const std::vector< int > &samples) const =0
Check if a sample of indices results in a good sample of points indices.
Eigen::Matrix< Scalar, ValuesAtCompileTime, InputsAtCompileTime > JacobianType
Definition: sac_model.h:630
void setSamplesMaxDist(const double &radius, SearchPtr search)
Set the maximum distance allowed when drawing random samples.
Definition: sac_model.h:392
pcl::search::Search< PointT >::Ptr SearchPtr
Definition: sac_model.h:72
static const unsigned int max_sample_checks_
The maximum number of samples to try until we get a good one.
Definition: sac_model.h:515
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
PointCloudConstPtr input_
A boost shared pointer to the point cloud data array.
Definition: sac_model.h:509
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
Definition: sac_model.h:69
virtual int countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold)=0
Count all the points which respect the given model coefficients as inliers.
SampleConsensusModel represents the base model class.
Definition: sac_model.h:66
int rnd()
Boost-based random number generator.
Definition: sac_model.h:545
static const std::map< pcl::SacModel, unsigned int > SAC_SAMPLE_SIZE(sample_size_pairs, sample_size_pairs+sizeof(sample_size_pairs)/sizeof(SampleSizeModel))
Functor(int m_data_points)
Constructor.
Definition: sac_model.h:638
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: sac_model.h:304
pcl::PointCloud< PointT >::Ptr PointCloudPtr
Definition: sac_model.h:71
boost::shared_ptr< pcl::search::Search< PointT > > Ptr
Definition: search.h:81
double computeVariance(const std::vector< double > &error_sqr_dists)
Compute the variance of the errors to the model.
Definition: sac_model.h:414
void setNormalDistanceWeight(const double w)
Set the normal angular distance weight.
Definition: sac_model.h:578
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input dataset that contains the point normals of the XYZ dataset...
Definition: sac_model.h:593
std::vector< int > shuffled_indices_
Data containing a shuffled version of the indices.
Definition: sac_model.h:529
void getRadiusLimits(double &min_radius, double &max_radius)
Get the minimum and maximum allowable radius limits for the model as set by the user.
Definition: sac_model.h:381
Functor()
Empty Construtor.
Definition: sac_model.h:633
virtual bool isModelValid(const Eigen::VectorXf &model_coefficients)=0
Check whether a model is valid given the user constraints.
void drawIndexSampleRadius(std::vector< int > &sample)
Fills a sample array with one random sample from the indices_ vector and other random samples that ar...
Definition: sac_model.h:459
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
PointCloudNConstPtr normals_
A pointer to the input dataset that contains the point normals of the XYZ dataset.
Definition: sac_model.h:611
void drawIndexSample(std::vector< int > &sample)
Fills a sample array with random samples from the indices_ vector.
Definition: sac_model.h:442
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...
SampleConsensusModelFromNormals represents the base model class for models that require the use of su...
Definition: sac_model.h:557
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.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
SacModel
Definition: model_types.h:48
boost::shared_ptr< const SampleConsensusModel > ConstPtr
Definition: sac_model.h:75
pcl::PointCloud< PointT >::ConstPtr PointCloudConstPtr
Definition: sac_model.h:70
boost::shared_ptr< boost::variate_generator< boost::mt19937 &, boost::uniform_int<> > > rng_gen_
Boost-based random number generator.
Definition: sac_model.h:538
void getSamplesMaxDist(double &radius)
Get maximum distance allowed when drawing random samples.
Definition: sac_model.h:403
SampleConsensusModel(const PointCloudConstPtr &cloud, const std::vector< int > &indices, bool random=false)
Constructor for base SampleConsensusModel.
Definition: sac_model.h:138
virtual void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances)=0
Compute all distances from the cloud data to a given model.
PointCloudNConstPtr getInputNormals()
Get a pointer to the normals of the input XYZ point cloud dataset.
Definition: sac_model.h:600
boost::shared_ptr< SampleConsensusModel > Ptr
Definition: sac_model.h:74
boost::mt19937 rng_alg_
Boost-based random number generator algorithm.
Definition: sac_model.h:532
Eigen::Matrix< Scalar, InputsAtCompileTime, 1 > InputType
Definition: sac_model.h:629
boost::shared_ptr< SampleConsensusModelFromNormals > Ptr
Definition: sac_model.h:563
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...
Definition: sac_model.h:368
boost::shared_ptr< std::vector< int > > getIndices() const
Get a pointer to the vector of indices used.
Definition: sac_model.h:345
std::vector< double > error_sqr_dists_
A vector holding the distances to the computed model.
Definition: sac_model.h:541
_Scalar Scalar
Definition: sac_model.h:621
virtual ~SampleConsensusModelFromNormals()
Destructor.
Definition: sac_model.h:570
double getNormalDistanceWeight()
Get the normal angular distance weight.
Definition: sac_model.h:585
A point structure representing Euclidean xyz coordinates, and the RGB color.
SearchPtr samples_radius_search_
The search object for picking subsequent samples using radius search.
Definition: sac_model.h:526
virtual void getSamples(int &iterations, std::vector< int > &samples)
Get a set of random data samples and return them as point indices.
Definition: sac_model.h:178
unsigned int getSampleSize() const
Return the size of a sample from which a model is computed.
Definition: sac_model.h:353
virtual ~SampleConsensusModel()
Destructor for base SampleConsensusModel.
Definition: sac_model.h:170