Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
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
60namespace 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:
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&
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
358 {
359 return sample_size_;
360 }
361
362 /** \brief Return the number of coefficients in the model. */
363 inline unsigned int
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
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
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. */
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.
shared_ptr< PointCloud< PointT > > Ptr
shared_ptr< const PointCloud< PointT > > ConstPtr
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
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
const std::string & getClassName() const
Get a string representation of the name of this class.
Definition sac_model.h:350
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:86
Defines functions, macros and traits for allocating and using memory.
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
Definition types.h:112
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.