MRPT  2.0.1
model_search_impl.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #pragma once
11 
12 #ifndef math_modelsearch_h
13 #include "model_search.h"
14 #endif
15 
16 #include <mrpt/core/exceptions.h> // ASSERT_()
17 #include <mrpt/core/round.h>
18 #include <algorithm> // std::max(),...
19 #include <cmath>
20 #include <limits>
21 #include <string>
22 
23 namespace mrpt::math
24 {
25 //----------------------------------------------------------------------
26 //! Run the ransac algorithm searching for a single model
27 template <typename TModelFit>
29  const TModelFit& p_state, size_t p_kernelSize,
30  const typename TModelFit::Real& p_fitnessThreshold,
31  typename TModelFit::Model& p_bestModel, std::vector<size_t>& p_inliers)
32 {
33  size_t bestScore = std::string::npos; // npos will mean "none"
34  size_t iter = 0;
35  size_t softIterLimit = 1; // will be updated by the size of inliers
36  size_t hardIterLimit = 100; // a fixed iteration step
37  p_inliers.clear();
38  size_t nSamples = p_state.getSampleCount();
39  std::vector<size_t> ind(p_kernelSize);
40 
41  while (iter < softIterLimit && iter < hardIterLimit)
42  {
43  bool degenerate = true;
44  typename TModelFit::Model currentModel;
45  size_t i = 0;
46  while (degenerate)
47  {
48  pickRandomIndex(nSamples, p_kernelSize, ind);
49  degenerate = !p_state.fitModel(ind, currentModel);
50  i++;
51  if (i > 100) return false;
52  }
53 
54  std::vector<size_t> inliers;
55 
56  for (size_t j = 0; j < nSamples; j++)
57  {
58  if (p_state.testSample(j, currentModel) < p_fitnessThreshold)
59  inliers.push_back(j);
60  }
61  ASSERT_(inliers.size() > 0);
62 
63  // Find the number of inliers to this model.
64  const size_t ninliers = inliers.size();
65  bool update_estim_num_iters =
66  (iter == 0); // Always update on the first iteration, regardless of
67  // the result (even for ninliers=0)
68 
69  if (ninliers > bestScore ||
70  (bestScore == std::string::npos && ninliers != 0))
71  {
72  bestScore = ninliers;
73  p_bestModel = currentModel;
74  p_inliers = inliers;
75  update_estim_num_iters = true;
76  }
77 
78  if (update_estim_num_iters)
79  {
80  // Update the estimation of maxIter to pick dataset with no outliers
81  // at propability p
82  double f = ninliers / static_cast<double>(nSamples);
83  double p = 1 - std::pow(f, static_cast<double>(p_kernelSize));
84  const double eps = std::numeric_limits<double>::epsilon();
85  p = std::max(eps, p); // Avoid division by -Inf
86  p = std::min(1 - eps, p); // Avoid division by 0.
87  softIterLimit = mrpt::round(log(1 - p) / log(p));
88  }
89 
90  iter++;
91  }
92 
93  return true;
94 }
95 
96 //----------------------------------------------------------------------
97 //! Run a generic programming version of ransac searching for a single model
98 template <typename TModelFit>
100  const TModelFit& p_state, size_t p_kernelSize,
101  const typename TModelFit::Real& p_fitnessThreshold, size_t p_populationSize,
102  size_t p_maxIteration, typename TModelFit::Model& p_bestModel,
103  std::vector<size_t>& p_inliers)
104 {
105  // a single specie of the population
106  using Species = TSpecies<TModelFit>;
107  std::vector<Species> storage;
108  std::vector<Species*> population;
109  std::vector<Species*> sortedPopulation;
110 
111  size_t sampleCount = p_state.getSampleCount();
112  int elderCnt = (int)p_populationSize / 3;
113  int siblingCnt = (p_populationSize - elderCnt) / 2;
114  int speciesAlive = 0;
115 
116  storage.resize(p_populationSize);
117  population.reserve(p_populationSize);
118  sortedPopulation.reserve(p_populationSize);
119  for (auto& d : storage)
120  {
121  pickRandomIndex(sampleCount, p_kernelSize, d.sample);
122  population.push_back(&d);
123  sortedPopulation.push_back(&d);
124  }
125 
126  size_t iter = 0;
127  while (iter < p_maxIteration)
128  {
129  if (iter > 0)
130  {
131  // generate new population: old, siblings, new
132  population.clear();
133  int i = 0;
134 
135  // copy the best elders
136  for (; i < elderCnt; i++)
137  {
138  population.push_back(sortedPopulation[i]);
139  }
140 
141  // mate elders to make siblings
142  int se = (int)speciesAlive; // dead species cannot mate
143  for (; i < elderCnt + siblingCnt; i++)
144  {
145  Species* sibling = sortedPopulation[--se];
146  population.push_back(sibling);
147 
148  // pick two parents, from the species not yet refactored
149  // better elders has more chance to mate as they are removed
150  // later from the list
151  int r1 = rand();
152  int r2 = rand();
153  int p1 = r1 % se;
154  int p2 =
155  (p1 > se / 2) ? (r2 % p1) : p1 + 1 + (r2 % (se - p1 - 1));
156  ASSERT_(p1 != p2 && p1 < se && p2 < se);
157  ASSERT_(se >= elderCnt);
158  Species* a = sortedPopulation[p1];
159  Species* b = sortedPopulation[p2];
160 
161  // merge the sample candidates
162  std::set<size_t> sampleSet;
163  sampleSet.insert(a->sample.begin(), a->sample.end());
164  sampleSet.insert(b->sample.begin(), b->sample.end());
165  // mutate - add a random sample that will be selected with some
166  // (non-zero) probability
167  sampleSet.insert(rand() % sampleCount);
168  pickRandomIndex(sampleSet, p_kernelSize, sibling->sample);
169  }
170 
171  // generate some new random species
172  for (; i < (int)p_populationSize; i++)
173  {
174  Species* s = sortedPopulation[i];
175  population.push_back(s);
176  pickRandomIndex(sampleCount, p_kernelSize, s->sample);
177  }
178  }
179 
180  // evaluate species
181  speciesAlive = 0;
182  for (typename std::vector<Species*>::iterator it = population.begin();
183  it != population.end(); it++)
184  {
185  Species& s = **it;
186  if (p_state.fitModel(s.sample, s.model))
187  {
188  s.fitness = 0;
189  for (size_t i = 0; i < p_state.getSampleCount(); i++)
190  {
191  typename TModelFit::Real f = p_state.testSample(i, s.model);
192  if (f < p_fitnessThreshold)
193  {
194  s.fitness += f;
195  s.inliers.push_back(i);
196  }
197  }
198  ASSERT_(s.inliers.size() > 0);
199 
200  s.fitness /= s.inliers.size();
201  // scale by the number of outliers
202  s.fitness *= (sampleCount - s.inliers.size());
203  speciesAlive++;
204  }
205  else
206  s.fitness =
207  std::numeric_limits<typename TModelFit::Real>::max();
208  }
209 
210  if (!speciesAlive)
211  {
212  // the world is dead, no model was found
213  return false;
214  }
215 
216  // sort by fitness (ascending)
217  std::sort(
218  sortedPopulation.begin(), sortedPopulation.end(), Species::compare);
219 
220  iter++;
221  }
222 
223  p_bestModel = sortedPopulation[0]->model;
224  p_inliers = sortedPopulation[0]->inliers;
225 
226  return !p_inliers.empty();
227 }
228 } // namespace mrpt::math
bool ransacSingleModel(const TModelFit &p_state, size_t p_kernelSize, const typename TModelFit::Real &p_fitnessThreshold, typename TModelFit::Model &p_bestModel, std::vector< size_t > &p_inliers)
Run the ransac algorithm searching for a single model.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
This base provides a set of functions for maths stuff.
const double eps
void pickRandomIndex(size_t p_size, size_t p_pick, std::vector< size_t > &p_ind)
Select random (unique) indices from the 0..p_size sequence.
bool geneticSingleModel(const TModelFit &p_state, size_t p_kernelSize, const typename TModelFit::Real &p_fitnessThreshold, size_t p_populationSize, size_t p_maxIteration, typename TModelFit::Model &p_bestModel, std::vector< size_t > &p_inliers)
Run a generic programming version of ransac searching for a single model.
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:24



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020