Main MRPT website > C++ reference for MRPT 1.9.9
model_search_impl.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #ifndef math_modelsearch_impl_h
11 #define math_modelsearch_impl_h
12 
13 #ifndef math_modelsearch_h
14 #include "model_search.h"
15 #endif
16 
17 #include <limits>
18 
19 namespace mrpt
20 {
21 namespace math
22 {
23 //----------------------------------------------------------------------
24 //! Run the ransac algorithm searching for a single model
25 template <typename TModelFit>
27  const TModelFit& p_state, size_t p_kernelSize,
28  const typename TModelFit::Real& p_fitnessThreshold,
29  typename TModelFit::Model& p_bestModel, std::vector<size_t>& p_inliers)
30 {
31  size_t bestScore = std::string::npos; // npos will mean "none"
32  size_t iter = 0;
33  size_t softIterLimit = 1; // will be updated by the size of inliers
34  size_t hardIterLimit = 100; // a fixed iteration step
35  p_inliers.clear();
36  size_t nSamples = p_state.getSampleCount();
37  std::vector<size_t> ind(p_kernelSize);
38 
39  while (iter < softIterLimit && iter < hardIterLimit)
40  {
41  bool degenerate = true;
42  typename TModelFit::Model currentModel;
43  size_t i = 0;
44  while (degenerate)
45  {
46  pickRandomIndex(nSamples, p_kernelSize, ind);
47  degenerate = !p_state.fitModel(ind, currentModel);
48  i++;
49  if (i > 100) return false;
50  }
51 
52  std::vector<size_t> inliers;
53 
54  for (size_t j = 0; j < nSamples; j++)
55  {
56  if (p_state.testSample(j, currentModel) < p_fitnessThreshold)
57  inliers.push_back(j);
58  }
59  ASSERT_(inliers.size() > 0);
60 
61  // Find the number of inliers to this model.
62  const size_t ninliers = inliers.size();
63  bool update_estim_num_iters =
64  (iter == 0); // Always update on the first iteration, regardless of
65  // the result (even for ninliers=0)
66 
67  if (ninliers > bestScore ||
68  (bestScore == std::string::npos && ninliers != 0))
69  {
70  bestScore = ninliers;
71  p_bestModel = currentModel;
72  p_inliers = inliers;
73  update_estim_num_iters = true;
74  }
75 
76  if (update_estim_num_iters)
77  {
78  // Update the estimation of maxIter to pick dataset with no outliers
79  // at propability p
80  double f = ninliers / static_cast<double>(nSamples);
81  double p = 1 - pow(f, static_cast<double>(p_kernelSize));
82  const double eps = std::numeric_limits<double>::epsilon();
83  p = std::max(eps, p); // Avoid division by -Inf
84  p = std::min(1 - eps, p); // Avoid division by 0.
85  softIterLimit = log(1 - p) / log(p);
86  }
87 
88  iter++;
89  }
90 
91  return true;
92 }
93 
94 //----------------------------------------------------------------------
95 //! Run a generic programming version of ransac searching for a single model
96 template <typename TModelFit>
98  const TModelFit& p_state, size_t p_kernelSize,
99  const typename TModelFit::Real& p_fitnessThreshold, size_t p_populationSize,
100  size_t p_maxIteration, typename TModelFit::Model& p_bestModel,
101  std::vector<size_t>& p_inliers)
102 {
103  // a single specie of the population
104  using Species = TSpecies<TModelFit>;
105  std::vector<Species> storage;
106  std::vector<Species*> population;
107  std::vector<Species*> sortedPopulation;
108 
109  size_t sampleCount = p_state.getSampleCount();
110  int elderCnt = (int)p_populationSize / 3;
111  int siblingCnt = (p_populationSize - elderCnt) / 2;
112  int speciesAlive = 0;
113 
114  storage.resize(p_populationSize);
115  population.reserve(p_populationSize);
116  sortedPopulation.reserve(p_populationSize);
117  for (typename std::vector<Species>::iterator it = storage.begin();
118  it != storage.end(); it++)
119  {
120  pickRandomIndex(sampleCount, p_kernelSize, it->sample);
121  population.push_back(&*it);
122  sortedPopulation.push_back(&*it);
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 
228 } // namespace math
229 } // namespace mrpt
230 
231 #endif // math_modelsearch_h
Scalar * iterator
Definition: eigen_plugins.h:26
#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:3676
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
GLubyte GLubyte b
Definition: glext.h:6279
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.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
GLubyte GLubyte GLubyte a
Definition: glext.h:6279
GLfloat GLfloat p
Definition: glext.h:6305



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019