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