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