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