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