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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 8fe78517f Sun Jul 14 19:43:28 2019 +0200 at lun oct 28 02:10:00 CET 2019