Point Cloud Library (PCL)  1.10.1-dev
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 #pragma once
42 
43 #include <cfloat>
44 #include <ctime>
45 #include <climits>
46 #include <memory>
47 #include <set>
48 
49 #include <pcl/memory.h>
50 #include <pcl/pcl_macros.h>
51 #include <pcl/pcl_base.h>
52 #include <pcl/console/print.h>
53 #include <pcl/point_cloud.h>
54 #include <pcl/sample_consensus/boost.h>
55 #include <pcl/sample_consensus/model_types.h>
56 
57 #include <pcl/search/search.h>
58 
59 namespace pcl
60 {
61  template<class T> class ProgressiveSampleConsensus;
62 
63  /** \brief @b SampleConsensusModel represents the base model class. All sample consensus models must inherit
64  * from this class.
65  * \author Radu B. Rusu
66  * \ingroup sample_consensus
67  */
68  template <typename PointT>
70  {
71  public:
74  using PointCloudPtr = typename PointCloud::Ptr;
76 
79 
80  protected:
81  /** \brief Empty constructor for base SampleConsensusModel.
82  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
83  */
84  SampleConsensusModel (bool random = false)
85  : input_ ()
86  , radius_min_ (-std::numeric_limits<double>::max ())
87  , radius_max_ (std::numeric_limits<double>::max ())
88  , samples_radius_ (0.)
90  , rng_dist_ (new boost::uniform_int<> (0, std::numeric_limits<int>::max ()))
91  {
92  // Create a random number generator object
93  if (random)
94  rng_alg_.seed (static_cast<unsigned> (std::time(nullptr)));
95  else
96  rng_alg_.seed (12345u);
97 
98  rng_gen_.reset (new boost::variate_generator<boost::mt19937&, boost::uniform_int<> > (rng_alg_, *rng_dist_));
99  }
100 
101  public:
102  /** \brief Constructor for base SampleConsensusModel.
103  * \param[in] cloud the input point cloud dataset
104  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
105  */
106  SampleConsensusModel (const PointCloudConstPtr &cloud, bool random = false)
107  : input_ ()
108  , radius_min_ (-std::numeric_limits<double>::max ())
109  , radius_max_ (std::numeric_limits<double>::max ())
110  , samples_radius_ (0.)
112  , rng_dist_ (new boost::uniform_int<> (0, std::numeric_limits<int>::max ()))
113  {
114  if (random)
115  rng_alg_.seed (static_cast<unsigned> (std::time (nullptr)));
116  else
117  rng_alg_.seed (12345u);
118 
119  // Sets the input cloud and creates a vector of "fake" indices
120  setInputCloud (cloud);
121 
122  // Create a random number generator object
123  rng_gen_.reset (new boost::variate_generator<boost::mt19937&, boost::uniform_int<> > (rng_alg_, *rng_dist_));
124  }
125 
126  /** \brief Constructor for base SampleConsensusModel.
127  * \param[in] cloud the input point cloud dataset
128  * \param[in] indices a vector of point indices to be used from \a cloud
129  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
130  */
132  const std::vector<int> &indices,
133  bool random = false)
134  : input_ (cloud)
135  , indices_ (new std::vector<int> (indices))
136  , radius_min_ (-std::numeric_limits<double>::max ())
137  , radius_max_ (std::numeric_limits<double>::max ())
138  , samples_radius_ (0.)
140  , rng_dist_ (new boost::uniform_int<> (0, std::numeric_limits<int>::max ()))
141  {
142  if (random)
143  rng_alg_.seed (static_cast<unsigned> (std::time(nullptr)));
144  else
145  rng_alg_.seed (12345u);
146 
147  if (indices_->size () > input_->points.size ())
148  {
149  PCL_ERROR ("[pcl::SampleConsensusModel] Invalid index vector given with size %lu while the input PointCloud has size %lu!\n", indices_->size (), input_->points.size ());
150  indices_->clear ();
151  }
153 
154  // Create a random number generator object
155  rng_gen_.reset (new boost::variate_generator<boost::mt19937&, boost::uniform_int<> > (rng_alg_, *rng_dist_));
156  };
157 
158  /** \brief Destructor for base SampleConsensusModel. */
159  virtual ~SampleConsensusModel () {};
160 
161  /** \brief Get a set of random data samples and return them as point
162  * indices.
163  * \param[out] iterations the internal number of iterations used by SAC methods
164  * \param[out] samples the resultant model samples
165  */
166  virtual void
167  getSamples (int &iterations, std::vector<int> &samples)
168  {
169  // We're assuming that indices_ have already been set in the constructor
170  if (indices_->size () < getSampleSize ())
171  {
172  PCL_ERROR ("[pcl::SampleConsensusModel::getSamples] Can not select %lu unique points out of %lu!\n",
173  samples.size (), indices_->size ());
174  // one of these will make it stop :)
175  samples.clear ();
176  iterations = INT_MAX - 1;
177  return;
178  }
179 
180  // Get a second point which is different than the first
181  samples.resize (getSampleSize ());
182  for (unsigned int iter = 0; iter < max_sample_checks_; ++iter)
183  {
184  // Choose the random indices
185  if (samples_radius_ < std::numeric_limits<double>::epsilon ())
187  else
189 
190  // If it's a good sample, stop here
191  if (isSampleGood (samples))
192  {
193  PCL_DEBUG ("[pcl::SampleConsensusModel::getSamples] Selected %lu samples.\n", samples.size ());
194  return;
195  }
196  }
197  PCL_DEBUG ("[pcl::SampleConsensusModel::getSamples] WARNING: Could not select %d sample points in %d iterations!\n", getSampleSize (), max_sample_checks_);
198  samples.clear ();
199  }
200 
201  /** \brief Check whether the given index samples can form a valid model,
202  * compute the model coefficients from these samples and store them
203  * in model_coefficients. Pure virtual.
204  * Implementations of this function must be thread-safe.
205  * \param[in] samples the point indices found as possible good candidates
206  * for creating a valid model
207  * \param[out] model_coefficients the computed model coefficients
208  */
209  virtual bool
210  computeModelCoefficients (const std::vector<int> &samples,
211  Eigen::VectorXf &model_coefficients) const = 0;
212 
213  /** \brief Recompute the model coefficients using the given inlier set
214  * and return them to the user. Pure virtual.
215  *
216  * @note: these are the coefficients of the model after refinement
217  * (e.g., after a least-squares optimization)
218  *
219  * \param[in] inliers the data inliers supporting the model
220  * \param[in] model_coefficients the initial guess for the model coefficients
221  * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization
222  */
223  virtual void
224  optimizeModelCoefficients (const std::vector<int> &inliers,
225  const Eigen::VectorXf &model_coefficients,
226  Eigen::VectorXf &optimized_coefficients) const = 0;
227 
228  /** \brief Compute all distances from the cloud data to a given model. Pure virtual.
229  *
230  * \param[in] model_coefficients the coefficients of a model that we need to compute distances to
231  * \param[out] distances the resultant estimated distances
232  */
233  virtual void
234  getDistancesToModel (const Eigen::VectorXf &model_coefficients,
235  std::vector<double> &distances) const = 0;
236 
237  /** \brief Select all the points which respect the given model
238  * coefficients as inliers. Pure virtual.
239  *
240  * \param[in] model_coefficients the coefficients of a model that we need to compute distances to
241  * \param[in] threshold a maximum admissible distance threshold for determining the inliers from
242  * the outliers
243  * \param[out] inliers the resultant model inliers
244  */
245  virtual void
246  selectWithinDistance (const Eigen::VectorXf &model_coefficients,
247  const double threshold,
248  std::vector<int> &inliers) = 0;
249 
250  /** \brief Count all the points which respect the given model
251  * coefficients as inliers. Pure virtual.
252  * Implementations of this function must be thread-safe.
253  * \param[in] model_coefficients the coefficients of a model that we need to
254  * compute distances to
255  * \param[in] threshold a maximum admissible distance threshold for
256  * determining the inliers from the outliers
257  * \return the resultant number of inliers
258  */
259  virtual std::size_t
260  countWithinDistance (const Eigen::VectorXf &model_coefficients,
261  const double threshold) const = 0;
262 
263  /** \brief Create a new point cloud with inliers projected onto the model. Pure virtual.
264  * \param[in] inliers the data inliers that we want to project on the model
265  * \param[in] model_coefficients the coefficients of a model
266  * \param[out] projected_points the resultant projected points
267  * \param[in] copy_data_fields set to true (default) if we want the \a
268  * projected_points cloud to be an exact copy of the input dataset minus
269  * the point projections on the plane model
270  */
271  virtual void
272  projectPoints (const std::vector<int> &inliers,
273  const Eigen::VectorXf &model_coefficients,
274  PointCloud &projected_points,
275  bool copy_data_fields = true) const = 0;
276 
277  /** \brief Verify whether a subset of indices verifies a given set of
278  * model coefficients. Pure virtual.
279  *
280  * \param[in] indices the data indices that need to be tested against the model
281  * \param[in] model_coefficients the set of model coefficients
282  * \param[in] threshold a maximum admissible distance threshold for
283  * determining the inliers from the outliers
284  */
285  virtual bool
286  doSamplesVerifyModel (const std::set<int> &indices,
287  const Eigen::VectorXf &model_coefficients,
288  const double threshold) const = 0;
289 
290  /** \brief Provide a pointer to the input dataset
291  * \param[in] cloud the const boost shared pointer to a PointCloud message
292  */
293  inline virtual void
295  {
296  input_ = cloud;
297  if (!indices_)
298  indices_.reset (new std::vector<int> ());
299  if (indices_->empty ())
300  {
301  // Prepare a set of indices to be used (entire cloud)
302  indices_->resize (cloud->points.size ());
303  for (std::size_t i = 0; i < cloud->points.size (); ++i)
304  (*indices_)[i] = static_cast<int> (i);
305  }
307  }
308 
309  /** \brief Get a pointer to the input point cloud dataset. */
310  inline PointCloudConstPtr
311  getInputCloud () const { return (input_); }
312 
313  /** \brief Provide a pointer to the vector of indices that represents the input data.
314  * \param[in] indices a pointer to the vector of indices that represents the input data.
315  */
316  inline void
317  setIndices (const IndicesPtr &indices)
318  {
319  indices_ = indices;
321  }
322 
323  /** \brief Provide the vector of indices that represents the input data.
324  * \param[out] indices the vector of indices that represents the input data.
325  */
326  inline void
327  setIndices (const std::vector<int> &indices)
328  {
329  indices_.reset (new std::vector<int> (indices));
330  shuffled_indices_ = indices;
331  }
332 
333  /** \brief Get a pointer to the vector of indices used. */
334  inline IndicesPtr
335  getIndices () const { return (indices_); }
336 
337  /** \brief Return a unique id for each type of model employed. */
338  virtual SacModel
339  getModelType () const = 0;
340 
341  /** \brief Get a string representation of the name of this class. */
342  inline const std::string&
343  getClassName () const
344  {
345  return (model_name_);
346  }
347 
348  /** \brief Return the size of a sample from which the model is computed. */
349  inline unsigned int
350  getSampleSize () const
351  {
352  return sample_size_;
353  }
354 
355  /** \brief Return the number of coefficients in the model. */
356  inline unsigned int
357  getModelSize () const
358  {
359  return model_size_;
360  }
361 
362  /** \brief Set the minimum and maximum allowable radius limits for the
363  * model (applicable to models that estimate a radius)
364  * \param[in] min_radius the minimum radius model
365  * \param[in] max_radius the maximum radius model
366  * \todo change this to set limits on the entire model
367  */
368  inline void
369  setRadiusLimits (const double &min_radius, const double &max_radius)
370  {
371  radius_min_ = min_radius;
372  radius_max_ = max_radius;
373  }
374 
375  /** \brief Get the minimum and maximum allowable radius limits for the
376  * model as set by the user.
377  *
378  * \param[out] min_radius the resultant minimum radius model
379  * \param[out] max_radius the resultant maximum radius model
380  */
381  inline void
382  getRadiusLimits (double &min_radius, double &max_radius) const
383  {
384  min_radius = radius_min_;
385  max_radius = radius_max_;
386  }
387 
388  /** \brief Set the maximum distance allowed when drawing random samples
389  * \param[in] radius the maximum distance (L2 norm)
390  * \param search
391  */
392  inline void
393  setSamplesMaxDist (const double &radius, SearchPtr search)
394  {
395  samples_radius_ = radius;
396  samples_radius_search_ = search;
397  }
398 
399  /** \brief Get maximum distance allowed when drawing random samples
400  *
401  * \param[out] radius the maximum distance (L2 norm)
402  */
403  inline void
404  getSamplesMaxDist (double &radius) const
405  {
406  radius = samples_radius_;
407  }
408 
410 
411  /** \brief Compute the variance of the errors to the model.
412  * \param[in] error_sqr_dists a vector holding the distances
413  */
414  inline double
415  computeVariance (const std::vector<double> &error_sqr_dists) const
416  {
417  std::vector<double> dists (error_sqr_dists);
418  const std::size_t medIdx = dists.size () >> 1;
419  std::nth_element (dists.begin (), dists.begin () + medIdx, dists.end ());
420  double median_error_sqr = dists[medIdx];
421  return (2.1981 * median_error_sqr);
422  }
423 
424  /** \brief Compute the variance of the errors to the model from the internally
425  * estimated vector of distances. The model must be computed first (or at least
426  * selectWithinDistance must be called).
427  */
428  inline double
430  {
431  if (error_sqr_dists_.empty ())
432  {
433  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");
434  return (std::numeric_limits<double>::quiet_NaN ());
435  }
437  }
438 
439  protected:
440 
441  /** \brief Fills a sample array with random samples from the indices_ vector
442  * \param[out] sample the set of indices of target_ to analyze
443  */
444  inline void
445  drawIndexSample (std::vector<int> &sample)
446  {
447  std::size_t sample_size = sample.size ();
448  std::size_t index_size = shuffled_indices_.size ();
449  for (std::size_t i = 0; i < sample_size; ++i)
450  // The 1/(RAND_MAX+1.0) trick is when the random numbers are not uniformly distributed and for small modulo
451  // elements, that does not matter (and nowadays, random number generators are good)
452  //std::swap (shuffled_indices_[i], shuffled_indices_[i + (rand () % (index_size - i))]);
453  std::swap (shuffled_indices_[i], shuffled_indices_[i + (rnd () % (index_size - i))]);
454  std::copy (shuffled_indices_.begin (), shuffled_indices_.begin () + sample_size, sample.begin ());
455  }
456 
457  /** \brief Fills a sample array with one random sample from the indices_ vector
458  * and other random samples that are closer than samples_radius_
459  * \param[out] sample the set of indices of target_ to analyze
460  */
461  inline void
462  drawIndexSampleRadius (std::vector<int> &sample)
463  {
464  std::size_t sample_size = sample.size ();
465  std::size_t index_size = shuffled_indices_.size ();
466 
467  std::swap (shuffled_indices_[0], shuffled_indices_[0 + (rnd () % (index_size - 0))]);
468  //const PointT& pt0 = (*input_)[shuffled_indices_[0]];
469 
470  std::vector<int> indices;
471  std::vector<float> sqr_dists;
472 
473  // If indices have been set when the search object was constructed,
474  // radiusSearch() expects an index into the indices vector as its
475  // first parameter. This can't be determined efficiently, so we use
476  // the point instead of the index.
477  // Returned indices are converted automatically.
478  samples_radius_search_->radiusSearch (input_->at(shuffled_indices_[0]),
479  samples_radius_, indices, sqr_dists );
480 
481  if (indices.size () < sample_size - 1)
482  {
483  // radius search failed, make an invalid model
484  for(std::size_t i = 1; i < sample_size; ++i)
486  }
487  else
488  {
489  for (std::size_t i = 0; i < sample_size-1; ++i)
490  std::swap (indices[i], indices[i + (rnd () % (indices.size () - i))]);
491  for (std::size_t i = 1; i < sample_size; ++i)
492  shuffled_indices_[i] = indices[i-1];
493  }
494 
495  std::copy (shuffled_indices_.begin (), shuffled_indices_.begin () + sample_size, sample.begin ());
496  }
497 
498  /** \brief Check whether a model is valid given the user constraints.
499  *
500  * Default implementation verifies that the number of coefficients in the supplied model is as expected for this
501  * SAC model type. Specific SAC models should extend this function by checking the user constraints (if any).
502  *
503  * \param[in] model_coefficients the set of model coefficients
504  */
505  virtual bool
506  isModelValid (const Eigen::VectorXf &model_coefficients) const
507  {
508  if (model_coefficients.size () != model_size_)
509  {
510  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_);
511  return (false);
512  }
513  return (true);
514  }
515 
516  /** \brief Check if a sample of indices results in a good sample of points
517  * indices. Pure virtual.
518  * \param[in] samples the resultant index samples
519  */
520  virtual bool
521  isSampleGood (const std::vector<int> &samples) const = 0;
522 
523  /** \brief The model name. */
524  std::string model_name_;
525 
526  /** \brief A boost shared pointer to the point cloud data array. */
528 
529  /** \brief A pointer to the vector of point indices to use. */
531 
532  /** The maximum number of samples to try until we get a good one */
533  static const unsigned int max_sample_checks_ = 1000;
534 
535  /** \brief The minimum and maximum radius limits for the model.
536  * Applicable to all models that estimate a radius.
537  */
539 
540  /** \brief The maximum distance of subsequent samples from the first (radius search) */
542 
543  /** \brief The search object for picking subsequent samples using radius search */
545 
546  /** Data containing a shuffled version of the indices. This is used and modified when drawing samples. */
547  std::vector<int> shuffled_indices_;
548 
549  /** \brief Boost-based random number generator algorithm. */
550  boost::mt19937 rng_alg_;
551 
552  /** \brief Boost-based random number generator distribution. */
553  std::shared_ptr<boost::uniform_int<> > rng_dist_;
554 
555  /** \brief Boost-based random number generator. */
556  std::shared_ptr<boost::variate_generator< boost::mt19937&, boost::uniform_int<> > > rng_gen_;
557 
558  /** \brief A vector holding the distances to the computed model. Used internally. */
559  std::vector<double> error_sqr_dists_;
560 
561  /** \brief The size of a sample from which the model is computed. Every subclass should initialize this appropriately. */
562  unsigned int sample_size_;
563 
564  /** \brief The number of coefficients in the model. Every subclass should initialize this appropriately. */
565  unsigned int model_size_;
566 
567  /** \brief Boost-based random number generator. */
568  inline int
569  rnd ()
570  {
571  return ((*rng_gen_) ());
572  }
573  public:
575  };
576 
577  /** \brief @b SampleConsensusModelFromNormals represents the base model class
578  * for models that require the use of surface normals for estimation.
579  */
580  template <typename PointT, typename PointNT>
581  class SampleConsensusModelFromNormals //: public SampleConsensusModel<PointT>
582  {
583  public:
586 
589 
590  /** \brief Empty constructor for base SampleConsensusModelFromNormals. */
591  SampleConsensusModelFromNormals () : normal_distance_weight_ (0.0), normals_ () {};
592 
593  /** \brief Destructor. */
595 
596  /** \brief Set the normal angular distance weight.
597  * \param[in] w the relative weight (between 0 and 1) to give to the angular
598  * distance (0 to pi/2) between point normals and the plane normal.
599  * (The Euclidean distance will have weight 1-w.)
600  */
601  inline void
602  setNormalDistanceWeight (const double w)
603  {
604  normal_distance_weight_ = w;
605  }
606 
607  /** \brief Get the normal angular distance weight. */
608  inline double
609  getNormalDistanceWeight () const { return (normal_distance_weight_); }
610 
611  /** \brief Provide a pointer to the input dataset that contains the point
612  * normals of the XYZ dataset.
613  *
614  * \param[in] normals the const boost shared pointer to a PointCloud message
615  */
616  inline void
618  {
619  normals_ = normals;
620  }
621 
622  /** \brief Get a pointer to the normals of the input XYZ point cloud dataset. */
623  inline PointCloudNConstPtr
624  getInputNormals () const { return (normals_); }
625 
626  protected:
627  /** \brief The relative weight (between 0 and 1) to give to the angular
628  * distance (0 to pi/2) between point normals and the plane normal.
629  */
631 
632  /** \brief A pointer to the input dataset that contains the point normals
633  * of the XYZ dataset.
634  */
636  };
637 
638  /** Base functor all the models that need non linear optimization must
639  * define their own one and implement operator() (const Eigen::VectorXd& x, Eigen::VectorXd& fvec)
640  * or operator() (const Eigen::VectorXf& x, Eigen::VectorXf& fvec) depending on the chosen _Scalar
641  */
642  template<typename _Scalar, int NX=Eigen::Dynamic, int NY=Eigen::Dynamic>
643  struct Functor
644  {
645  using Scalar = _Scalar;
646  enum
647  {
648  InputsAtCompileTime = NX,
649  ValuesAtCompileTime = NY
650  };
651 
652  using ValueType = Eigen::Matrix<Scalar,ValuesAtCompileTime,1>;
653  using InputType = Eigen::Matrix<Scalar,InputsAtCompileTime,1>;
654  using JacobianType = Eigen::Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime>;
655 
656  /** \brief Empty Constructor. */
657  Functor () : m_data_points_ (ValuesAtCompileTime) {}
658 
659  /** \brief Constructor
660  * \param[in] m_data_points number of data points to evaluate.
661  */
662  Functor (int m_data_points) : m_data_points_ (m_data_points) {}
663 
664  virtual ~Functor () {}
665 
666  /** \brief Get the number of values. */
667  int
668  values () const { return (m_data_points_); }
669 
670  private:
671  const int m_data_points_;
672  };
673 }
virtual bool isModelValid(const Eigen::VectorXf &model_coefficients) const
Check whether a model is valid given the user constraints.
Definition: sac_model.h:506
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:630
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:414
IndicesPtr getIndices() const
Get a pointer to the vector of indices used.
Definition: sac_model.h:335
Defines functions, macros and traits for allocating and using memory.
double radius_min_
The minimum and maximum radius limits for the model.
Definition: sac_model.h:538
SampleConsensusModelFromNormals()
Empty constructor for base SampleConsensusModelFromNormals.
Definition: sac_model.h:591
void setIndices(const std::vector< int > &indices)
Provide the vector of indices that represents the input data.
Definition: sac_model.h:327
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.
Definition: sac_model.h:382
virtual ~Functor()
Definition: sac_model.h:664
virtual SacModel getModelType() const =0
Return a unique id for each type of model employed.
unsigned int getSampleSize() const
Return the size of a sample from which the model is computed.
Definition: sac_model.h:350
shared_ptr< Indices > IndicesPtr
Definition: pcl_base.h:62
std::shared_ptr< boost::variate_generator< boost::mt19937 &, boost::uniform_int<> > > rng_gen_
Boost-based random number generator.
Definition: sac_model.h:556
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.
SampleConsensusModel(bool random=false)
Empty constructor for base SampleConsensusModel.
Definition: sac_model.h:84
RandomSampleConsensus represents an implementation of the RANSAC (RAndom SAmple Consensus) algorithm...
Definition: prosac.h:55
unsigned int model_size_
The number of coefficients in the model.
Definition: sac_model.h:565
Eigen::Matrix< Scalar, InputsAtCompileTime, 1 > InputType
Definition: sac_model.h:653
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.
SampleConsensusModel(const PointCloudConstPtr &cloud, bool random=false)
Constructor for base SampleConsensusModel.
Definition: sac_model.h:106
Base functor all the models that need non linear optimization must define their own one and implement...
Definition: sac_model.h:643
double samples_radius_
The maximum distance of subsequent samples from the first (radius search)
Definition: sac_model.h:541
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: memory.h:65
int values() const
Get the number of values.
Definition: sac_model.h:668
virtual bool isSampleGood(const std::vector< int > &samples) const =0
Check if a sample of indices results in a good sample of points indices.
double computeVariance(const std::vector< double > &error_sqr_dists) const
Compute the variance of the errors to the model.
Definition: sac_model.h:415
void setSamplesMaxDist(const double &radius, SearchPtr search)
Set the maximum distance allowed when drawing random samples.
Definition: sac_model.h:393
static const unsigned int max_sample_checks_
The maximum number of samples to try until we get a good one.
Definition: sac_model.h:533
PointCloudConstPtr input_
A boost shared pointer to the point cloud data array.
Definition: sac_model.h:527
double getNormalDistanceWeight() const
Get the normal angular distance weight.
Definition: sac_model.h:609
void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
Definition: sac_model.h:317
typename pcl::PointCloud< PointNT >::ConstPtr PointCloudNConstPtr
Definition: sac_model.h:584
double computeVariance() const
Compute the variance of the errors to the model from the internally estimated vector of distances...
Definition: sac_model.h:429
SampleConsensusModel represents the base model class.
Definition: sac_model.h:69
int rnd()
Boost-based random number generator.
Definition: sac_model.h:569
const std::string & getClassName() const
Get a string representation of the name of this class.
Definition: sac_model.h:343
Functor(int m_data_points)
Constructor.
Definition: sac_model.h:662
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: sac_model.h:294
std::string model_name_
The model name.
Definition: sac_model.h:524
Eigen::Matrix< Scalar, ValuesAtCompileTime, 1 > ValueType
Definition: sac_model.h:652
shared_ptr< SampleConsensusModelFromNormals< pcl::PointXYZRGB, PointNT > > Ptr
Definition: sac_model.h:587
shared_ptr< const SampleConsensusModelFromNormals< pcl::PointXYZRGB, PointNT > > ConstPtr
Definition: sac_model.h:588
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...
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 setNormalDistanceWeight(const double w)
Set the normal angular distance weight.
Definition: sac_model.h:602
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:617
std::vector< int > shuffled_indices_
Data containing a shuffled version of the indices.
Definition: sac_model.h:547
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: sac_model.h:73
Functor()
Empty Constructor.
Definition: sac_model.h:657
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:462
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...
PointCloudNConstPtr normals_
A pointer to the input dataset that contains the point normals of the XYZ dataset.
Definition: sac_model.h:635
void drawIndexSample(std::vector< int > &sample)
Fills a sample array with random samples from the indices_ vector.
Definition: sac_model.h:445
SampleConsensusModelFromNormals represents the base model class for models that require the use of su...
Definition: sac_model.h:581
PointCloud represents the base class in PCL for storing collections of 3D points. ...
SacModel
Definition: model_types.h:45
std::shared_ptr< boost::uniform_int<> > rng_dist_
Boost-based random number generator distribution.
Definition: sac_model.h:553
shared_ptr< const SampleConsensusModel< WeightSACPointType > > ConstPtr
Definition: sac_model.h:78
typename pcl::search::Search< WeightSACPointType >::Ptr SearchPtr
Definition: sac_model.h:75
SampleConsensusModel(const PointCloudConstPtr &cloud, const std::vector< int > &indices, bool random=false)
Constructor for base SampleConsensusModel.
Definition: sac_model.h:131
boost::mt19937 rng_alg_
Boost-based random number generator algorithm.
Definition: sac_model.h:550
Eigen::Matrix< Scalar, ValuesAtCompileTime, InputsAtCompileTime > JacobianType
Definition: sac_model.h:654
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition: sac_model.h:530
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:415
void getSamplesMaxDist(double &radius) const
Get maximum distance allowed when drawing random samples.
Definition: sac_model.h:404
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:369
std::vector< double > error_sqr_dists_
A vector holding the distances to the computed model.
Definition: sac_model.h:559
shared_ptr< pcl::search::Search< PointT > > Ptr
Definition: search.h:80
virtual ~SampleConsensusModelFromNormals()
Destructor.
Definition: sac_model.h:594
A point structure representing Euclidean xyz coordinates, and the RGB color.
shared_ptr< SampleConsensusModel< WeightSACPointType > > Ptr
Definition: sac_model.h:77
PointCloudConstPtr getInputCloud() const
Get a pointer to the input point cloud dataset.
Definition: sac_model.h:311
SearchPtr samples_radius_search_
The search object for picking subsequent samples using radius search.
Definition: sac_model.h:544
boost::shared_ptr< T > shared_ptr
Alias for boost::shared_ptr.
Definition: memory.h:81
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.
typename pcl::PointCloud< PointNT >::Ptr PointCloudNPtr
Definition: sac_model.h:585
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:167
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.
Defines all the PCL and non-PCL macros used.
PointCloudNConstPtr getInputNormals() const
Get a pointer to the normals of the input XYZ point cloud dataset.
Definition: sac_model.h:624
unsigned int sample_size_
The size of a sample from which the model is computed.
Definition: sac_model.h:562
unsigned int getModelSize() const
Return the number of coefficients in the model.
Definition: sac_model.h:357
virtual ~SampleConsensusModel()
Destructor for base SampleConsensusModel.
Definition: sac_model.h:159