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