Point Cloud Library (PCL)  1.8.1
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 #ifndef PCL_SAMPLE_CONSENSUS_MODEL_H_
42 #define PCL_SAMPLE_CONSENSUS_MODEL_H_
43 
44 #include <cfloat>
45 #include <ctime>
46 #include <limits.h>
47 #include <set>
48 
49 #include <pcl/console/print.h>
50 #include <pcl/point_cloud.h>
51 #include <pcl/sample_consensus/boost.h>
52 #include <pcl/sample_consensus/model_types.h>
53 
54 #include <pcl/search/search.h>
55 
56 namespace pcl
57 {
58  template<class T> class ProgressiveSampleConsensus;
59 
60  /** \brief @b SampleConsensusModel represents the base model class. All sample consensus models must inherit
61  * from this class.
62  * \author Radu B. Rusu
63  * \ingroup sample_consensus
64  */
65  template <typename PointT>
67  {
68  public:
73 
74  typedef boost::shared_ptr<SampleConsensusModel> Ptr;
75  typedef boost::shared_ptr<const SampleConsensusModel> ConstPtr;
76 
77  protected:
78  /** \brief Empty constructor for base SampleConsensusModel.
79  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
80  */
81  SampleConsensusModel (bool random = false)
82  : input_ ()
83  , indices_ ()
84  , radius_min_ (-std::numeric_limits<double>::max ())
85  , radius_max_ (std::numeric_limits<double>::max ())
86  , samples_radius_ (0.)
89  , rng_alg_ ()
90  , rng_dist_ (new boost::uniform_int<> (0, std::numeric_limits<int>::max ()))
91  , rng_gen_ ()
92  , error_sqr_dists_ ()
93  {
94  // Create a random number generator object
95  if (random)
96  rng_alg_.seed (static_cast<unsigned> (std::time(0)));
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  , indices_ ()
111  , radius_min_ (-std::numeric_limits<double>::max ())
112  , radius_max_ (std::numeric_limits<double>::max ())
113  , samples_radius_ (0.)
115  , shuffled_indices_ ()
116  , rng_alg_ ()
117  , rng_dist_ (new boost::uniform_int<> (0, std::numeric_limits<int>::max ()))
118  , rng_gen_ ()
119  , error_sqr_dists_ ()
120  {
121  if (random)
122  rng_alg_.seed (static_cast<unsigned> (std::time (0)));
123  else
124  rng_alg_.seed (12345u);
125 
126  // Sets the input cloud and creates a vector of "fake" indices
127  setInputCloud (cloud);
128 
129  // Create a random number generator object
130  rng_gen_.reset (new boost::variate_generator<boost::mt19937&, boost::uniform_int<> > (rng_alg_, *rng_dist_));
131  }
132 
133  /** \brief Constructor for base SampleConsensusModel.
134  * \param[in] cloud the input point cloud dataset
135  * \param[in] indices a vector of point indices to be used from \a cloud
136  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
137  */
138  SampleConsensusModel (const PointCloudConstPtr &cloud,
139  const std::vector<int> &indices,
140  bool random = false)
141  : input_ (cloud)
142  , indices_ (new std::vector<int> (indices))
143  , radius_min_ (-std::numeric_limits<double>::max ())
144  , radius_max_ (std::numeric_limits<double>::max ())
145  , samples_radius_ (0.)
147  , shuffled_indices_ ()
148  , rng_alg_ ()
149  , rng_dist_ (new boost::uniform_int<> (0, std::numeric_limits<int>::max ()))
150  , rng_gen_ ()
151  , error_sqr_dists_ ()
152  {
153  if (random)
154  rng_alg_.seed (static_cast<unsigned> (std::time(0)));
155  else
156  rng_alg_.seed (12345u);
157 
158  if (indices_->size () > input_->points.size ())
159  {
160  PCL_ERROR ("[pcl::SampleConsensusModel] Invalid index vector given with size %lu while the input PointCloud has size %lu!\n", indices_->size (), input_->points.size ());
161  indices_->clear ();
162  }
164 
165  // Create a random number generator object
166  rng_gen_.reset (new boost::variate_generator<boost::mt19937&, boost::uniform_int<> > (rng_alg_, *rng_dist_));
167  };
168 
169  /** \brief Destructor for base SampleConsensusModel. */
170  virtual ~SampleConsensusModel () {};
171 
172  /** \brief Get a set of random data samples and return them as point
173  * indices.
174  * \param[out] iterations the internal number of iterations used by SAC methods
175  * \param[out] samples the resultant model samples
176  */
177  virtual void
178  getSamples (int &iterations, std::vector<int> &samples)
179  {
180  // We're assuming that indices_ have already been set in the constructor
181  if (indices_->size () < getSampleSize ())
182  {
183  PCL_ERROR ("[pcl::SampleConsensusModel::getSamples] Can not select %lu unique points out of %lu!\n",
184  samples.size (), indices_->size ());
185  // one of these will make it stop :)
186  samples.clear ();
187  iterations = INT_MAX - 1;
188  return;
189  }
190 
191  // Get a second point which is different than the first
192  samples.resize (getSampleSize ());
193  for (unsigned int iter = 0; iter < max_sample_checks_; ++iter)
194  {
195  // Choose the random indices
196  if (samples_radius_ < std::numeric_limits<double>::epsilon ())
198  else
200 
201  // If it's a good sample, stop here
202  if (isSampleGood (samples))
203  {
204  PCL_DEBUG ("[pcl::SampleConsensusModel::getSamples] Selected %lu samples.\n", samples.size ());
205  return;
206  }
207  }
208  PCL_DEBUG ("[pcl::SampleConsensusModel::getSamples] WARNING: Could not select %d sample points in %d iterations!\n", getSampleSize (), max_sample_checks_);
209  samples.clear ();
210  }
211 
212  /** \brief Check whether the given index samples can form a valid model,
213  * compute the model coefficients from these samples and store them
214  * in model_coefficients. Pure virtual.
215  * \param[in] samples the point indices found as possible good candidates
216  * for creating a valid model
217  * \param[out] model_coefficients the computed model coefficients
218  */
219  virtual bool
220  computeModelCoefficients (const std::vector<int> &samples,
221  Eigen::VectorXf &model_coefficients) = 0;
222 
223  /** \brief Recompute the model coefficients using the given inlier set
224  * and return them to the user. Pure virtual.
225  *
226  * @note: these are the coefficients of the model after refinement
227  * (e.g., after a least-squares optimization)
228  *
229  * \param[in] inliers the data inliers supporting the model
230  * \param[in] model_coefficients the initial guess for the model coefficients
231  * \param[out] optimized_coefficients the resultant recomputed coefficients after non-linear optimization
232  */
233  virtual void
234  optimizeModelCoefficients (const std::vector<int> &inliers,
235  const Eigen::VectorXf &model_coefficients,
236  Eigen::VectorXf &optimized_coefficients) = 0;
237 
238  /** \brief Compute all distances from the cloud data to a given model. Pure virtual.
239  *
240  * \param[in] model_coefficients the coefficients of a model that we need to compute distances to
241  * \param[out] distances the resultant estimated distances
242  */
243  virtual void
244  getDistancesToModel (const Eigen::VectorXf &model_coefficients,
245  std::vector<double> &distances) = 0;
246 
247  /** \brief Select all the points which respect the given model
248  * coefficients as inliers. Pure virtual.
249  *
250  * \param[in] model_coefficients the coefficients of a model that we need to compute distances to
251  * \param[in] threshold a maximum admissible distance threshold for determining the inliers from
252  * the outliers
253  * \param[out] inliers the resultant model inliers
254  */
255  virtual void
256  selectWithinDistance (const Eigen::VectorXf &model_coefficients,
257  const double threshold,
258  std::vector<int> &inliers) = 0;
259 
260  /** \brief Count all the points which respect the given model
261  * coefficients as inliers. Pure virtual.
262  *
263  * \param[in] model_coefficients the coefficients of a model that we need to
264  * compute distances to
265  * \param[in] threshold a maximum admissible distance threshold for
266  * determining the inliers from the outliers
267  * \return the resultant number of inliers
268  */
269  virtual int
270  countWithinDistance (const Eigen::VectorXf &model_coefficients,
271  const double threshold) = 0;
272 
273  /** \brief Create a new point cloud with inliers projected onto the model. Pure virtual.
274  * \param[in] inliers the data inliers that we want to project on the model
275  * \param[in] model_coefficients the coefficients of a model
276  * \param[out] projected_points the resultant projected points
277  * \param[in] copy_data_fields set to true (default) if we want the \a
278  * projected_points cloud to be an exact copy of the input dataset minus
279  * the point projections on the plane model
280  */
281  virtual void
282  projectPoints (const std::vector<int> &inliers,
283  const Eigen::VectorXf &model_coefficients,
284  PointCloud &projected_points,
285  bool copy_data_fields = true) = 0;
286 
287  /** \brief Verify whether a subset of indices verifies a given set of
288  * model coefficients. Pure virtual.
289  *
290  * \param[in] indices the data indices that need to be tested against the model
291  * \param[in] model_coefficients the set of model coefficients
292  * \param[in] threshold a maximum admissible distance threshold for
293  * determining the inliers from the outliers
294  */
295  virtual bool
296  doSamplesVerifyModel (const std::set<int> &indices,
297  const Eigen::VectorXf &model_coefficients,
298  const double threshold) = 0;
299 
300  /** \brief Provide a pointer to the input dataset
301  * \param[in] cloud the const boost shared pointer to a PointCloud message
302  */
303  inline virtual void
304  setInputCloud (const PointCloudConstPtr &cloud)
305  {
306  input_ = cloud;
307  if (!indices_)
308  indices_.reset (new std::vector<int> ());
309  if (indices_->empty ())
310  {
311  // Prepare a set of indices to be used (entire cloud)
312  indices_->resize (cloud->points.size ());
313  for (size_t i = 0; i < cloud->points.size (); ++i)
314  (*indices_)[i] = static_cast<int> (i);
315  }
317  }
318 
319  /** \brief Get a pointer to the input point cloud dataset. */
320  inline PointCloudConstPtr
321  getInputCloud () const { return (input_); }
322 
323  /** \brief Provide a pointer to the vector of indices that represents the input data.
324  * \param[in] indices a pointer to the vector of indices that represents the input data.
325  */
326  inline void
327  setIndices (const boost::shared_ptr <std::vector<int> > &indices)
328  {
329  indices_ = indices;
331  }
332 
333  /** \brief Provide the vector of indices that represents the input data.
334  * \param[out] indices the vector of indices that represents the input data.
335  */
336  inline void
337  setIndices (const std::vector<int> &indices)
338  {
339  indices_.reset (new std::vector<int> (indices));
340  shuffled_indices_ = indices;
341  }
342 
343  /** \brief Get a pointer to the vector of indices used. */
344  inline boost::shared_ptr <std::vector<int> >
345  getIndices () const { return (indices_); }
346 
347  /** \brief Return an unique id for each type of model employed. */
348  virtual SacModel
349  getModelType () const = 0;
350 
351  /** \brief Get a string representation of the name of this class. */
352  inline const std::string&
353  getClassName () const
354  {
355  return (model_name_);
356  }
357 
358  /** \brief Return the size of a sample from which the model is computed. */
359  inline unsigned int
360  getSampleSize () const
361  {
362  return sample_size_;
363  }
364 
365  /** \brief Return the number of coefficients in the model. */
366  inline unsigned int
367  getModelSize () const
368  {
369  return model_size_;
370  }
371 
372  /** \brief Set the minimum and maximum allowable radius limits for the
373  * model (applicable to models that estimate a radius)
374  * \param[in] min_radius the minimum radius model
375  * \param[in] max_radius the maximum radius model
376  * \todo change this to set limits on the entire model
377  */
378  inline void
379  setRadiusLimits (const double &min_radius, const double &max_radius)
380  {
381  radius_min_ = min_radius;
382  radius_max_ = max_radius;
383  }
384 
385  /** \brief Get the minimum and maximum allowable radius limits for the
386  * model as set by the user.
387  *
388  * \param[out] min_radius the resultant minimum radius model
389  * \param[out] max_radius the resultant maximum radius model
390  */
391  inline void
392  getRadiusLimits (double &min_radius, double &max_radius)
393  {
394  min_radius = radius_min_;
395  max_radius = radius_max_;
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)
415  {
416  radius = samples_radius_;
417  }
418 
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)
426  {
427  std::vector<double> dists (error_sqr_dists);
428  const 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
455  drawIndexSample (std::vector<int> &sample)
456  {
457  size_t sample_size = sample.size ();
458  size_t index_size = shuffled_indices_.size ();
459  for (unsigned int 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_.begin (), shuffled_indices_.begin () + 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
472  drawIndexSampleRadius (std::vector<int> &sample)
473  {
474  size_t sample_size = sample.size ();
475  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  std::vector<int> 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(unsigned int i = 1; i < sample_size; ++i)
496  }
497  else
498  {
499  for (unsigned int i = 0; i < sample_size-1; ++i)
500  std::swap (indices[i], indices[i + (rnd () % (indices.size () - i))]);
501  for (unsigned int i = 1; i < sample_size; ++i)
502  shuffled_indices_[i] = indices[i-1];
503  }
504 
505  std::copy (shuffled_indices_.begin (), shuffled_indices_.begin () + 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)
517  {
518  if (model_coefficients.size () != model_size_)
519  {
520  PCL_ERROR ("[pcl::%s::isModelValid] Invalid number of model coefficients given (%lu)!\n", getClassName ().c_str (), model_coefficients.size ());
521  return (false);
522  }
523  return (true);
524  }
525 
526  /** \brief Check if a sample of indices results in a good sample of points
527  * indices. Pure virtual.
528  * \param[in] samples the resultant index samples
529  */
530  virtual bool
531  isSampleGood (const std::vector<int> &samples) const = 0;
532 
533  /** \brief The model name. */
534  std::string model_name_;
535 
536  /** \brief A boost shared pointer to the point cloud data array. */
537  PointCloudConstPtr input_;
538 
539  /** \brief A pointer to the vector of point indices to use. */
540  boost::shared_ptr <std::vector<int> > indices_;
541 
542  /** The maximum number of samples to try until we get a good one */
543  static const unsigned int max_sample_checks_ = 1000;
544 
545  /** \brief The minimum and maximum radius limits for the model.
546  * Applicable to all models that estimate a radius.
547  */
549 
550  /** \brief The maximum distance of subsequent samples from the first (radius search) */
552 
553  /** \brief The search object for picking subsequent samples using radius search */
555 
556  /** Data containing a shuffled version of the indices. This is used and modified when drawing samples. */
557  std::vector<int> shuffled_indices_;
558 
559  /** \brief Boost-based random number generator algorithm. */
560  boost::mt19937 rng_alg_;
561 
562  /** \brief Boost-based random number generator distribution. */
563  boost::shared_ptr<boost::uniform_int<> > rng_dist_;
564 
565  /** \brief Boost-based random number generator. */
566  boost::shared_ptr<boost::variate_generator< boost::mt19937&, boost::uniform_int<> > > rng_gen_;
567 
568  /** \brief A vector holding the distances to the computed model. Used internally. */
569  std::vector<double> error_sqr_dists_;
570 
571  /** \brief The size of a sample from which the model is computed. Every subclass should initialize this appropriately. */
572  unsigned int sample_size_;
573 
574  /** \brief The number of coefficients in the model. Every subclass should initialize this appropriately. */
575  unsigned int model_size_;
576 
577  /** \brief Boost-based random number generator. */
578  inline int
579  rnd ()
580  {
581  return ((*rng_gen_) ());
582  }
583  public:
584  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
585  };
586 
587  /** \brief @b SampleConsensusModelFromNormals represents the base model class
588  * for models that require the use of surface normals for estimation.
589  */
590  template <typename PointT, typename PointNT>
591  class SampleConsensusModelFromNormals //: public SampleConsensusModel<PointT>
592  {
593  public:
596 
597  typedef boost::shared_ptr<SampleConsensusModelFromNormals> Ptr;
598  typedef boost::shared_ptr<const SampleConsensusModelFromNormals> ConstPtr;
599 
600  /** \brief Empty constructor for base SampleConsensusModelFromNormals. */
601  SampleConsensusModelFromNormals () : normal_distance_weight_ (0.0), normals_ () {};
602 
603  /** \brief Destructor. */
605 
606  /** \brief Set the normal angular distance weight.
607  * \param[in] w the relative weight (between 0 and 1) to give to the angular
608  * distance (0 to pi/2) between point normals and the plane normal.
609  * (The Euclidean distance will have weight 1-w.)
610  */
611  inline void
612  setNormalDistanceWeight (const double w)
613  {
614  normal_distance_weight_ = w;
615  }
616 
617  /** \brief Get the normal angular distance weight. */
618  inline double
619  getNormalDistanceWeight () { return (normal_distance_weight_); }
620 
621  /** \brief Provide a pointer to the input dataset that contains the point
622  * normals of the XYZ dataset.
623  *
624  * \param[in] normals the const boost shared pointer to a PointCloud message
625  */
626  inline void
627  setInputNormals (const PointCloudNConstPtr &normals)
628  {
629  normals_ = normals;
630  }
631 
632  /** \brief Get a pointer to the normals of the input XYZ point cloud dataset. */
633  inline PointCloudNConstPtr
634  getInputNormals () { return (normals_); }
635 
636  protected:
637  /** \brief The relative weight (between 0 and 1) to give to the angular
638  * distance (0 to pi/2) between point normals and the plane normal.
639  */
641 
642  /** \brief A pointer to the input dataset that contains the point normals
643  * of the XYZ dataset.
644  */
645  PointCloudNConstPtr normals_;
646  };
647 
648  /** Base functor all the models that need non linear optimization must
649  * define their own one and implement operator() (const Eigen::VectorXd& x, Eigen::VectorXd& fvec)
650  * or operator() (const Eigen::VectorXf& x, Eigen::VectorXf& fvec) dependening on the choosen _Scalar
651  */
652  template<typename _Scalar, int NX=Eigen::Dynamic, int NY=Eigen::Dynamic>
653  struct Functor
654  {
655  typedef _Scalar Scalar;
656  enum
657  {
658  InputsAtCompileTime = NX,
659  ValuesAtCompileTime = NY
660  };
661 
662  typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,1> ValueType;
663  typedef Eigen::Matrix<Scalar,InputsAtCompileTime,1> InputType;
664  typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType;
665 
666  /** \brief Empty Constructor. */
667  Functor () : m_data_points_ (ValuesAtCompileTime) {}
668 
669  /** \brief Constructor
670  * \param[in] m_data_points number of data points to evaluate.
671  */
672  Functor (int m_data_points) : m_data_points_ (m_data_points) {}
673 
674  virtual ~Functor () {}
675 
676  /** \brief Get the number of values. */
677  int
678  values () const { return (m_data_points_); }
679 
680  private:
681  const int m_data_points_;
682  };
683 }
684 
685 #endif //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_H_
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:640
double radius_min_
The minimum and maximum radius limits for the model.
Definition: sac_model.h:548
double computeVariance()
Compute the variance of the errors to the model from the internally estimated vector of distances...
Definition: sac_model.h:439
boost::shared_ptr< const SampleConsensusModelFromNormals > ConstPtr
Definition: sac_model.h:598
pcl::PointCloud< PointNT >::ConstPtr PointCloudNConstPtr
Definition: sac_model.h:594
SampleConsensusModelFromNormals()
Empty constructor for base SampleConsensusModelFromNormals.
Definition: sac_model.h:601
void setIndices(const std::vector< int > &indices)
Provide the vector of indices that represents the input data.
Definition: sac_model.h:337
virtual ~Functor()
Definition: sac_model.h:674
boost::shared_ptr< boost::uniform_int<> > rng_dist_
Boost-based random number generator distribution.
Definition: sac_model.h:563
virtual SacModel getModelType() const =0
Return an unique id for each type of model employed.
unsigned int getSampleSize() const
Return the size of a sample from which the model is computed.
Definition: sac_model.h:360
virtual void optimizeModelCoefficients(const std::vector< int > &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)=0
Recompute the model coefficients using the given inlier set and return them to the user...
virtual void selectWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold, std::vector< int > &inliers)=0
Select all the points which respect the given model coefficients as inliers.
SampleConsensusModel(bool random=false)
Empty constructor for base SampleConsensusModel.
Definition: sac_model.h:81
RandomSampleConsensus represents an implementation of the RANSAC (RAndom SAmple Consensus) algorithm...
Definition: prosac.h:56
unsigned int model_size_
The number of coefficients in the model.
Definition: sac_model.h:575
void setIndices(const boost::shared_ptr< std::vector< int > > &indices)
Provide a pointer to the vector of indices that represents the input data.
Definition: sac_model.h:327
SampleConsensusModel(const PointCloudConstPtr &cloud, bool random=false)
Constructor for base SampleConsensusModel.
Definition: sac_model.h:108
pcl::PointCloud< PointNT >::Ptr PointCloudNPtr
Definition: sac_model.h:595
Base functor all the models that need non linear optimization must define their own one and implement...
Definition: sac_model.h:653
Eigen::Matrix< Scalar, ValuesAtCompileTime, 1 > ValueType
Definition: sac_model.h:662
boost::shared_ptr< std::vector< int > > indices_
A pointer to the vector of point indices to use.
Definition: sac_model.h:540
double samples_radius_
The maximum distance of subsequent samples from the first (radius search)
Definition: sac_model.h:551
boost::shared_ptr< std::vector< int > > getIndices() const
Get a pointer to the vector of indices used.
Definition: sac_model.h:345
int values() const
Get the number of values.
Definition: sac_model.h:678
virtual bool isModelValid(const Eigen::VectorXf &model_coefficients)
Check whether a model is valid given the user constraints.
Definition: sac_model.h:516
virtual bool isSampleGood(const std::vector< int > &samples) const =0
Check if a sample of indices results in a good sample of points indices.
Eigen::Matrix< Scalar, ValuesAtCompileTime, InputsAtCompileTime > JacobianType
Definition: sac_model.h:664
void setSamplesMaxDist(const double &radius, SearchPtr search)
Set the maximum distance allowed when drawing random samples.
Definition: sac_model.h:403
pcl::search::Search< PointT >::Ptr SearchPtr
Definition: sac_model.h:72
static const unsigned int max_sample_checks_
The maximum number of samples to try until we get a good one.
Definition: sac_model.h:543
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
PointCloudConstPtr input_
A boost shared pointer to the point cloud data array.
Definition: sac_model.h:537
virtual bool doSamplesVerifyModel(const std::set< int > &indices, const Eigen::VectorXf &model_coefficients, const double threshold)=0
Verify whether a subset of indices verifies a given set of model coefficients.
pcl::PointCloud< PointT > PointCloud
Definition: sac_model.h:69
virtual int countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold)=0
Count all the points which respect the given model coefficients as inliers.
SampleConsensusModel represents the base model class.
Definition: sac_model.h:66
int rnd()
Boost-based random number generator.
Definition: sac_model.h:579
const std::string & getClassName() const
Get a string representation of the name of this class.
Definition: sac_model.h:353
Functor(int m_data_points)
Constructor.
Definition: sac_model.h:672
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: sac_model.h:304
std::string model_name_
The model name.
Definition: sac_model.h:534
pcl::PointCloud< PointT >::Ptr PointCloudPtr
Definition: sac_model.h:71
boost::shared_ptr< pcl::search::Search< PointT > > Ptr
Definition: search.h:81
double computeVariance(const std::vector< double > &error_sqr_dists)
Compute the variance of the errors to the model.
Definition: sac_model.h:425
void setNormalDistanceWeight(const double w)
Set the normal angular distance weight.
Definition: sac_model.h:612
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:627
std::vector< int > shuffled_indices_
Data containing a shuffled version of the indices.
Definition: sac_model.h:557
void getRadiusLimits(double &min_radius, double &max_radius)
Get the minimum and maximum allowable radius limits for the model as set by the user.
Definition: sac_model.h:392
Functor()
Empty Constructor.
Definition: sac_model.h:667
void drawIndexSampleRadius(std::vector< int > &sample)
Fills a sample array with one random sample from the indices_ vector and other random samples that ar...
Definition: sac_model.h:472
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:429
PointCloudNConstPtr normals_
A pointer to the input dataset that contains the point normals of the XYZ dataset.
Definition: sac_model.h:645
void drawIndexSample(std::vector< int > &sample)
Fills a sample array with random samples from the indices_ vector.
Definition: sac_model.h:455
virtual bool computeModelCoefficients(const std::vector< int > &samples, Eigen::VectorXf &model_coefficients)=0
Check whether the given index samples can form a valid model, compute the model coefficients from the...
SampleConsensusModelFromNormals represents the base model class for models that require the use of su...
Definition: sac_model.h:591
virtual void projectPoints(const std::vector< int > &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields=true)=0
Create a new point cloud with inliers projected onto the model.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
SacModel
Definition: model_types.h:48
boost::shared_ptr< const SampleConsensusModel > ConstPtr
Definition: sac_model.h:75
pcl::PointCloud< PointT >::ConstPtr PointCloudConstPtr
Definition: sac_model.h:70
boost::shared_ptr< boost::variate_generator< boost::mt19937 &, boost::uniform_int<> > > rng_gen_
Boost-based random number generator.
Definition: sac_model.h:566
void getSamplesMaxDist(double &radius)
Get maximum distance allowed when drawing random samples.
Definition: sac_model.h:414
SampleConsensusModel(const PointCloudConstPtr &cloud, const std::vector< int > &indices, bool random=false)
Constructor for base SampleConsensusModel.
Definition: sac_model.h:138
virtual void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances)=0
Compute all distances from the cloud data to a given model.
PointCloudNConstPtr getInputNormals()
Get a pointer to the normals of the input XYZ point cloud dataset.
Definition: sac_model.h:634
boost::shared_ptr< SampleConsensusModel > Ptr
Definition: sac_model.h:74
boost::mt19937 rng_alg_
Boost-based random number generator algorithm.
Definition: sac_model.h:560
Eigen::Matrix< Scalar, InputsAtCompileTime, 1 > InputType
Definition: sac_model.h:663
boost::shared_ptr< SampleConsensusModelFromNormals > Ptr
Definition: sac_model.h:597
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:379
std::vector< double > error_sqr_dists_
A vector holding the distances to the computed model.
Definition: sac_model.h:569
_Scalar Scalar
Definition: sac_model.h:655
virtual ~SampleConsensusModelFromNormals()
Destructor.
Definition: sac_model.h:604
double getNormalDistanceWeight()
Get the normal angular distance weight.
Definition: sac_model.h:619
A point structure representing Euclidean xyz coordinates, and the RGB color.
PointCloudConstPtr getInputCloud() const
Get a pointer to the input point cloud dataset.
Definition: sac_model.h:321
SearchPtr samples_radius_search_
The search object for picking subsequent samples using radius search.
Definition: sac_model.h:554
virtual void getSamples(int &iterations, std::vector< int > &samples)
Get a set of random data samples and return them as point indices.
Definition: sac_model.h:178
unsigned int sample_size_
The size of a sample from which the model is computed.
Definition: sac_model.h:572
unsigned int getModelSize() const
Return the number of coefficients in the model.
Definition: sac_model.h:367
virtual ~SampleConsensusModel()
Destructor for base SampleConsensusModel.
Definition: sac_model.h:170