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