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