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