ransac.h
Go to the documentation of this file.
1 // *****************************************************************************
2 //
3 // Copyright (c) 2014, Southwest Research Institute® (SwRI®)
4 // All rights reserved.
5 //
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are met:
8 // * Redistributions of source code must retain the above copyright
9 // notice, this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // * Neither the name of Southwest Research Institute® (SwRI®) nor the
14 // names of its contributors may be used to endorse or promote products
15 // derived from this software without specific prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
21 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
24 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 //
28 // *****************************************************************************
29 
30 #ifndef MATH_UTIL_RANSAC_H_
31 #define MATH_UTIL_RANSAC_H_
32 
33 #include <cmath>
34 #include <limits>
35 #include <vector>
36 
37 #include <boost/make_shared.hpp>
38 #include <swri_math_util/random.h>
39 
40 namespace swri_math_util
41 {
42  template <class Model>
43  class Ransac
44  {
45  public:
46  typedef typename Model::M ModelType;
47  typedef typename Model::T DataType;
48 
49  explicit Ransac(RandomGeneratorPtr rng = RandomGeneratorPtr()) : rng_(rng) {}
50 
51  ModelType FitModel(
52  Model& model,
53  double max_error,
54  double confidence,
55  int32_t min_iterations,
56  int32_t max_iterations,
57  std::vector<uint32_t>& inliers,
58  int32_t& iterations)
59  {
60  int32_t breakout = std::numeric_limits<int32_t>::max();
61  ModelType best_fit;
62  inliers.clear();
63  int32_t max_inliers = 0;
64 
65  if (!model.ValidData())
66  {
67  return best_fit;
68  }
69 
70  if (!rng_)
71  {
72  rng_ = boost::make_shared<RandomGenerator>();
73  }
74 
75  std::vector<int32_t> indices;
76 
77  ModelType hypothesis;
78  for (iterations = 0; (iterations < max_iterations && iterations < breakout) || iterations < min_iterations; iterations++)
79  {
80  indices.clear();
81  rng_->GetUniformRandomSample(0, model.Size() - 1, Model::MIN_SIZE, indices);
82 
83  // Generate a hypothesis model from the random sample.
84  // If the sample is not degenerate, calculate the number of inliers.
85  if (model.GetModel(indices, hypothesis, max_error))
86  {
87  int32_t inlier_count = model.GetInlierCount(hypothesis, max_error);
88 
89  // Update the best fit hypothesis and inliers if this hypothesis has
90  // the most inliers so far.
91  if (inlier_count > max_inliers)
92  {
93  max_inliers = inlier_count;
94  Model::CopyTo(hypothesis, best_fit);
95 
96  // Recalculate breakout threshold to see if the fit is good enough.
97  double ratio = inlier_count / static_cast<double>(model.Size());
98  double p_no_outliers = 1.0 - std::pow(ratio, Model::MIN_SIZE);
99  if (p_no_outliers == 0)
100  {
101  breakout = 0;
102  }
103  else if (p_no_outliers < .9999)
104  {
105  breakout = std::log(1 - confidence) / std::log(p_no_outliers);
106  }
107  }
108  }
109  }
110 
111  if (max_inliers > 0)
112  {
113  model.GetInliers(best_fit, max_error, inliers);
114  }
115  return best_fit;
116  }
117 
118  private:
120  };
121 
122  template <class Model>
124  {
125  public:
126  typedef typename Model::M ModelType;
127  typedef typename Model::T DataType;
128 
130 
131  ModelType FitModel(
132  const DataType& data,
133  double max_error,
134  double confidence,
135  int32_t max_iterations,
136  int32_t batch_size,
137  std::vector<uint32_t>& inliers,
138  int32_t& iterations)
139  {
140  Model model(data, batch_size);
141  iterations = 0;
142  int32_t breakout = std::numeric_limits<int32_t>::max();
143  ModelType best_fit;
144  inliers.clear();
145  int32_t max_inliers = 0;
146 
147  if (!model.ValidData())
148  {
149  return best_fit;
150  }
151 
152  if (!rng_)
153  {
154  rng_ = boost::make_shared<RandomGenerator>();
155  }
156 
157  std::vector<int32_t> indices;
158 
159  ModelType hypothesis;
160  while (iterations < max_iterations && iterations < breakout)
161  {
162  int32_t valid = 0;
163  model.ClearSamples();
164  while (iterations < max_iterations && iterations < breakout && model.Samples() < batch_size)
165  {
166  iterations++;
167  indices.clear();
168  rng_->GetUniformRandomSample(0, model.Size() - 1, Model::MIN_SIZE, indices);
169  model.AddSample(indices, max_error);
170  }
171 
172  if (model.Samples() > 0)
173  {
174  int32_t inlier_count = model.ProcessSamples(hypothesis, max_error);
175  if (inlier_count > 0 && inlier_count > max_inliers)
176  {
177  max_inliers = inlier_count;
178  Model::CopyTo(hypothesis, best_fit);
179 
180  // Recalculate breakout threshold to see if the fit is good enough.
181  double ratio = inlier_count / static_cast<double>(model.Size());
182  double p_no_outliers = 1.0 - std::pow(ratio, Model::MIN_SIZE);
183  if (p_no_outliers == 0)
184  {
185  breakout = 0;
186  }
187  else if (p_no_outliers < .9999)
188  {
189  breakout = std::log(1 - confidence) / std::log(p_no_outliers);
190  }
191  }
192  }
193  }
194 
195  if (max_inliers > 0)
196  {
197  model.GetInliers(best_fit, max_error, inliers);
198  }
199  return best_fit;
200  }
201 
202  private:
204  };
205 }
206 
207 #endif // MATH_UTIL_RANSAC_H_
Model::T DataType
Definition: ransac.h:47
Model::M ModelType
Definition: ransac.h:46
Ransac(RandomGeneratorPtr rng=RandomGeneratorPtr())
Definition: ransac.h:49
ModelType FitModel(Model &model, double max_error, double confidence, int32_t min_iterations, int32_t max_iterations, std::vector< uint32_t > &inliers, int32_t &iterations)
Definition: ransac.h:51
ModelType FitModel(const DataType &data, double max_error, double confidence, int32_t max_iterations, int32_t batch_size, std::vector< uint32_t > &inliers, int32_t &iterations)
Definition: ransac.h:131
RandomGeneratorPtr rng_
Definition: ransac.h:119
RandomGeneratorPtr rng_
Definition: ransac.h:203
RansacBatch(RandomGeneratorPtr rng=RandomGeneratorPtr())
Definition: ransac.h:129
boost::shared_ptr< RandomGenerator > RandomGeneratorPtr
Definition: random.h:70


swri_math_util
Author(s): Marc Alban
autogenerated on Sat Jan 21 2023 03:13:11