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