Point Cloud Library (PCL) 1.13.0
sac.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 <pcl/sample_consensus/sac_model.h>
44#include <pcl/pcl_base.h>
45
46#include <boost/random/mersenne_twister.hpp> // for mt19937
47#include <boost/random/uniform_01.hpp> // for uniform_01
48
49#include <ctime>
50#include <memory>
51#include <set>
52
53namespace pcl
54{
55 /** \brief SampleConsensus represents the base class. All sample consensus methods must inherit from this class.
56 * \author Radu Bogdan Rusu
57 * \ingroup sample_consensus
58 */
59 template <typename T>
61 {
62 using SampleConsensusModelPtr = typename SampleConsensusModel<T>::Ptr;
63
64 private:
65 /** \brief Constructor for base SAC. */
66 SampleConsensus () = default;
67
68 public:
69 using Ptr = shared_ptr<SampleConsensus<T> >;
70 using ConstPtr = shared_ptr<const SampleConsensus<T> >;
71
72
73 /** \brief Constructor for base SAC.
74 * \param[in] model a Sample Consensus model
75 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
76 */
77 SampleConsensus (const SampleConsensusModelPtr &model, bool random = false)
78 : sac_model_ (model)
79 , probability_ (0.99)
80 , iterations_ (0)
81 , threshold_ (std::numeric_limits<double>::max ())
82 , max_iterations_ (1000)
83 , threads_ (-1)
84 , rng_ (new boost::uniform_01<boost::mt19937> (rng_alg_))
85 {
86 // Create a random number generator object
87 if (random)
88 rng_->base ().seed (static_cast<unsigned> (std::time (nullptr)));
89 else
90 rng_->base ().seed (12345u);
91 };
92
93 /** \brief Constructor for base SAC.
94 * \param[in] model a Sample Consensus model
95 * \param[in] threshold distance to model threshold
96 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
97 */
98 SampleConsensus (const SampleConsensusModelPtr &model,
99 double threshold,
100 bool random = false)
101 : sac_model_ (model)
102 , probability_ (0.99)
103 , iterations_ (0)
104 , threshold_ (threshold)
105 , max_iterations_ (1000)
106 , threads_ (-1)
107 , rng_ (new boost::uniform_01<boost::mt19937> (rng_alg_))
108 {
109 // Create a random number generator object
110 if (random)
111 rng_->base ().seed (static_cast<unsigned> (std::time (nullptr)));
112 else
113 rng_->base ().seed (12345u);
114 };
115
116 /** \brief Set the Sample Consensus model to use.
117 * \param[in] model a Sample Consensus model
118 */
119 void
120 setSampleConsensusModel (const SampleConsensusModelPtr &model)
121 {
122 sac_model_ = model;
123 }
124
125 /** \brief Get the Sample Consensus model used. */
126 SampleConsensusModelPtr
128 {
129 return (sac_model_);
130 }
131
132 /** \brief Destructor for base SAC. */
133 virtual ~SampleConsensus () = default;
134
135 /** \brief Set the distance to model threshold.
136 * \param[in] threshold distance to model threshold
137 */
138 inline void
139 setDistanceThreshold (double threshold) { threshold_ = threshold; }
140
141 /** \brief Get the distance to model threshold, as set by the user. */
142 inline double
143 getDistanceThreshold () const { return (threshold_); }
144
145 /** \brief Set the maximum number of iterations.
146 * \param[in] max_iterations maximum number of iterations
147 */
148 inline void
149 setMaxIterations (int max_iterations) { max_iterations_ = max_iterations; }
150
151 /** \brief Get the maximum number of iterations, as set by the user. */
152 inline int
153 getMaxIterations () const { return (max_iterations_); }
154
155 /** \brief Set the desired probability of choosing at least one sample free from outliers.
156 * \param[in] probability the desired probability of choosing at least one sample free from outliers
157 * \note internally, the probability is set to 99% (0.99) by default.
158 */
159 inline void
160 setProbability (double probability) { probability_ = probability; }
161
162 /** \brief Obtain the probability of choosing at least one sample free from outliers, as set by the user. */
163 inline double
164 getProbability () const { return (probability_); }
165
166 /** \brief Set the number of threads to use or turn off parallelization.
167 * \param[in] nr_threads the number of hardware threads to use (0 sets the value automatically, a negative number turns parallelization off)
168 * \note Not all SAC methods have a parallel implementation. Some will ignore this setting.
169 */
170 inline void
171 setNumberOfThreads (const int nr_threads = -1) { threads_ = nr_threads; }
172
173 /** \brief Get the number of threads, as set by the user. */
174 inline int
175 getNumberOfThreads () const { return (threads_); }
176
177 /** \brief Compute the actual model. Pure virtual. */
178 virtual bool
179 computeModel (int debug_verbosity_level = 0) = 0;
180
181 /** \brief Refine the model found.
182 * This loops over the model coefficients and optimizes them together
183 * with the set of inliers, until the change in the set of inliers is
184 * minimal.
185 * \param[in] sigma standard deviation multiplier for considering a sample as inlier (Mahalanobis distance)
186 * \param[in] max_iterations the maxim number of iterations to try to refine in case the inliers keep on changing
187 */
188 virtual bool
189 refineModel (const double sigma = 3.0, const unsigned int max_iterations = 1000)
190 {
191 if (!sac_model_)
192 {
193 PCL_ERROR ("[pcl::SampleConsensus::refineModel] Critical error: NULL model!\n");
194 return (false);
195 }
196
197 double inlier_distance_threshold_sqr = threshold_ * threshold_,
198 error_threshold = threshold_;
199 double sigma_sqr = sigma * sigma;
200 unsigned int refine_iterations = 0;
201 bool inlier_changed = false, oscillating = false;
202 Indices new_inliers, prev_inliers = inliers_;
203 std::vector<std::size_t> inliers_sizes;
204 Eigen::VectorXf new_model_coefficients = model_coefficients_;
205 do
206 {
207 // Optimize the model coefficients
208 sac_model_->optimizeModelCoefficients (prev_inliers, new_model_coefficients, new_model_coefficients);
209 inliers_sizes.push_back (prev_inliers.size ());
210
211 // Select the new inliers based on the optimized coefficients and new threshold
212 sac_model_->selectWithinDistance (new_model_coefficients, error_threshold, new_inliers);
213 PCL_DEBUG ("[pcl::SampleConsensus::refineModel] Number of inliers found (before/after): %lu/%lu, with an error threshold of %g.\n", prev_inliers.size (), new_inliers.size (), error_threshold);
214
215 if (new_inliers.empty ())
216 {
217 refine_iterations++;
218 if (refine_iterations >= max_iterations)
219 break;
220 continue;
221 //return (false);
222 }
223
224 // Estimate the variance and the new threshold
225 double variance = sac_model_->computeVariance ();
226 error_threshold = sqrt (std::min (inlier_distance_threshold_sqr, sigma_sqr * variance));
227
228 PCL_DEBUG ("[pcl::SampleConsensus::refineModel] New estimated error threshold: %g on iteration %d out of %d.\n", error_threshold, refine_iterations, max_iterations);
229 inlier_changed = false;
230 std::swap (prev_inliers, new_inliers);
231 // If the number of inliers changed, then we are still optimizing
232 if (new_inliers.size () != prev_inliers.size ())
233 {
234 // Check if the number of inliers is oscillating in between two values
235 if (inliers_sizes.size () >= 4)
236 {
237 if (inliers_sizes[inliers_sizes.size () - 1] == inliers_sizes[inliers_sizes.size () - 3] &&
238 inliers_sizes[inliers_sizes.size () - 2] == inliers_sizes[inliers_sizes.size () - 4])
239 {
240 oscillating = true;
241 break;
242 }
243 }
244 inlier_changed = true;
245 continue;
246 }
247
248 // Check the values of the inlier set
249 for (std::size_t i = 0; i < prev_inliers.size (); ++i)
250 {
251 // If the value of the inliers changed, then we are still optimizing
252 if (prev_inliers[i] != new_inliers[i])
253 {
254 inlier_changed = true;
255 break;
256 }
257 }
258 }
259 while (inlier_changed && ++refine_iterations < max_iterations);
260
261 // If the new set of inliers is empty, we didn't do a good job refining
262 if (new_inliers.empty ())
263 {
264 PCL_ERROR ("[pcl::SampleConsensus::refineModel] Refinement failed: got an empty set of inliers!\n");
265 return (false);
266 }
267
268 if (oscillating)
269 {
270 PCL_DEBUG ("[pcl::SampleConsensus::refineModel] Detected oscillations in the model refinement.\n");
271 return (true);
272 }
273
274 // If no inliers have been changed anymore, then the refinement was successful
275 if (!inlier_changed)
276 {
277 std::swap (inliers_, new_inliers);
278 model_coefficients_ = new_model_coefficients;
279 return (true);
280 }
281 return (false);
282 }
283
284 /** \brief Get a set of randomly selected indices.
285 * \param[in] indices the input indices vector
286 * \param[in] nr_samples the desired number of point indices to randomly select
287 * \param[out] indices_subset the resultant output set of randomly selected indices
288 */
289 inline void
291 std::size_t nr_samples,
292 std::set<index_t> &indices_subset)
293 {
294 indices_subset.clear ();
295 while (indices_subset.size () < nr_samples)
296 //indices_subset.insert ((*indices)[(index_t) (indices->size () * (rand () / (RAND_MAX + 1.0)))]);
297 indices_subset.insert ((*indices)[static_cast<index_t> (static_cast<double>(indices->size ()) * rnd ())]);
298 }
299
300 /** \brief Return the best model found so far.
301 * \param[out] model the resultant model
302 */
303 inline void
304 getModel (Indices &model) const { model = model_; }
305
306 /** \brief Return the best set of inliers found so far for this model.
307 * \param[out] inliers the resultant set of inliers
308 */
309 inline void
310 getInliers (Indices &inliers) const { inliers = inliers_; }
311
312 /** \brief Return the model coefficients of the best model found so far.
313 * \param[out] model_coefficients the resultant model coefficients, as documented in \ref sample_consensus
314 */
315 inline void
316 getModelCoefficients (Eigen::VectorXf &model_coefficients) const { model_coefficients = model_coefficients_; }
317
318 protected:
319 /** \brief The underlying data model used (i.e. what is it that we attempt to search for). */
320 SampleConsensusModelPtr sac_model_;
321
322 /** \brief The model found after the last computeModel () as point cloud indices. */
324
325 /** \brief The indices of the points that were chosen as inliers after the last computeModel () call. */
327
328 /** \brief The coefficients of our model computed directly from the model found. */
329 Eigen::VectorXf model_coefficients_;
330
331 /** \brief Desired probability of choosing at least one sample free from outliers. */
333
334 /** \brief Total number of internal loop iterations that we've done so far. */
336
337 /** \brief Distance to model threshold. */
339
340 /** \brief Maximum number of iterations before giving up. */
342
343 /** \brief The number of threads the scheduler should use, or a negative number if no parallelization is wanted. */
345
346 /** \brief Boost-based random number generator algorithm. */
347 boost::mt19937 rng_alg_;
348
349 /** \brief Boost-based random number generator distribution. */
350 std::shared_ptr<boost::uniform_01<boost::mt19937> > rng_;
351
352 /** \brief Boost-based random number generator. */
353 inline double
355 {
356 return ((*rng_) ());
357 }
358 };
359}
SampleConsensus represents the base class.
Definition: sac.h:61
double probability_
Desired probability of choosing at least one sample free from outliers.
Definition: sac.h:332
std::shared_ptr< boost::uniform_01< boost::mt19937 > > rng_
Boost-based random number generator distribution.
Definition: sac.h:350
SampleConsensusModelPtr getSampleConsensusModel() const
Get the Sample Consensus model used.
Definition: sac.h:127
shared_ptr< SampleConsensus< T > > Ptr
Definition: sac.h:69
double getDistanceThreshold() const
Get the distance to model threshold, as set by the user.
Definition: sac.h:143
boost::mt19937 rng_alg_
Boost-based random number generator algorithm.
Definition: sac.h:347
int getNumberOfThreads() const
Get the number of threads, as set by the user.
Definition: sac.h:175
Indices inliers_
The indices of the points that were chosen as inliers after the last computeModel () call.
Definition: sac.h:326
int getMaxIterations() const
Get the maximum number of iterations, as set by the user.
Definition: sac.h:153
void getInliers(Indices &inliers) const
Return the best set of inliers found so far for this model.
Definition: sac.h:310
int iterations_
Total number of internal loop iterations that we've done so far.
Definition: sac.h:335
void getRandomSamples(const IndicesPtr &indices, std::size_t nr_samples, std::set< index_t > &indices_subset)
Get a set of randomly selected indices.
Definition: sac.h:290
virtual bool computeModel(int debug_verbosity_level=0)=0
Compute the actual model.
Indices model_
The model found after the last computeModel () as point cloud indices.
Definition: sac.h:323
double rnd()
Boost-based random number generator.
Definition: sac.h:354
void setNumberOfThreads(const int nr_threads=-1)
Set the number of threads to use or turn off parallelization.
Definition: sac.h:171
void getModel(Indices &model) const
Return the best model found so far.
Definition: sac.h:304
void getModelCoefficients(Eigen::VectorXf &model_coefficients) const
Return the model coefficients of the best model found so far.
Definition: sac.h:316
Eigen::VectorXf model_coefficients_
The coefficients of our model computed directly from the model found.
Definition: sac.h:329
double threshold_
Distance to model threshold.
Definition: sac.h:338
SampleConsensusModelPtr sac_model_
The underlying data model used (i.e.
Definition: sac.h:320
int threads_
The number of threads the scheduler should use, or a negative number if no parallelization is wanted.
Definition: sac.h:344
SampleConsensus(const SampleConsensusModelPtr &model, double threshold, bool random=false)
Constructor for base SAC.
Definition: sac.h:98
double getProbability() const
Obtain the probability of choosing at least one sample free from outliers, as set by the user.
Definition: sac.h:164
int max_iterations_
Maximum number of iterations before giving up.
Definition: sac.h:341
void setSampleConsensusModel(const SampleConsensusModelPtr &model)
Set the Sample Consensus model to use.
Definition: sac.h:120
void setProbability(double probability)
Set the desired probability of choosing at least one sample free from outliers.
Definition: sac.h:160
virtual ~SampleConsensus()=default
Destructor for base SAC.
void setDistanceThreshold(double threshold)
Set the distance to model threshold.
Definition: sac.h:139
SampleConsensus(const SampleConsensusModelPtr &model, bool random=false)
Constructor for base SAC.
Definition: sac.h:77
shared_ptr< const SampleConsensus< T > > ConstPtr
Definition: sac.h:70
virtual bool refineModel(const double sigma=3.0, const unsigned int max_iterations=1000)
Refine the model found.
Definition: sac.h:189
void setMaxIterations(int max_iterations)
Set the maximum number of iterations.
Definition: sac.h:149
SampleConsensusModel represents the base model class.
Definition: sac_model.h:70
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