Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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  namespace math {
21 
22 //----------------------------------------------------------------------
23 //! Run the ransac algorithm searching for a single model
24 template<typename TModelFit>
25 bool ModelSearch::ransacSingleModel( const TModelFit& p_state,
26  size_t p_kernelSize,
27  const typename TModelFit::Real& p_fitnessThreshold,
28  typename TModelFit::Model& p_bestModel,
29  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  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 )
50  return false;
51  }
52 
53  vector_size_t inliers;
54 
55  for( size_t i = 0; i < nSamples; i++ )
56  {
57  if( p_state.testSample( i, currentModel ) < p_fitnessThreshold )
58  inliers.push_back( i );
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 = (iter==0); // Always update on the first iteration, regardless of the result (even for ninliers=0)
65 
66  if ( ninliers > bestScore || (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 at propability p
77  double f = ninliers / static_cast<double>( nSamples );
78  double p = 1 - pow( f, static_cast<double>( p_kernelSize ) );
79  const double eps = std::numeric_limits<double>::epsilon();
80  p = std::max( eps, p); // Avoid division by -Inf
81  p = std::min( 1-eps, p); // Avoid division by 0.
82  softIterLimit = log(1-p) / log(p);
83  }
84 
85  iter++;
86  }
87 
88  return true;
89 }
90 
91 //----------------------------------------------------------------------
92 //! Run a generic programming version of ransac searching for a single model
93 template<typename TModelFit>
94 bool ModelSearch::geneticSingleModel( const TModelFit& p_state,
95  size_t p_kernelSize,
96  const typename TModelFit::Real& p_fitnessThreshold,
97  size_t p_populationSize,
98  size_t p_maxIteration,
99  typename TModelFit::Model& p_bestModel,
100  vector_size_t& p_inliers )
101 {
102  // a single specie of the population
103  typedef TSpecies<TModelFit> Species;
104  std::vector<Species> storage;
105  std::vector<Species*> population;
106  std::vector<Species*> sortedPopulation;
107 
108  size_t sampleCount = p_state.getSampleCount();
109  int elderCnt = (int)p_populationSize/3;
110  int siblingCnt = (p_populationSize - elderCnt) / 2;
111  int speciesAlive = 0;
112 
113  storage.resize( p_populationSize );
114  population.reserve( p_populationSize );
115  sortedPopulation.reserve( p_populationSize );
116  for( typename std::vector<Species>::iterator it = storage.begin(); 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 later from the list
147  int r1 = rand();
148  int r2 = rand();
149  int p1 = r1 % se;
150  int p2 = (p1 > se / 2) ? (r2 % p1) : p1 + 1 + (r2 % (se-p1-1));
151  ASSERT_( p1 != p2 && p1 < se && p2 < se );
152  ASSERT_( se >= elderCnt );
153  Species* a = sortedPopulation[p1];
154  Species* b = sortedPopulation[p2];
155 
156  // merge the sample candidates
157  std::set<size_t> sampleSet;
158  sampleSet.insert( a->sample.begin(), a->sample.end() );
159  sampleSet.insert( b->sample.begin(), b->sample.end() );
160  //mutate - add a random sample that will be selected with some (non-zero) probability
161  sampleSet.insert( rand() % sampleCount );
162  pickRandomIndex( sampleSet, p_kernelSize, sibling->sample );
163  }
164 
165  // generate some new random species
166  for( ; i < (int)p_populationSize; i++ )
167  {
168  Species* s = sortedPopulation[i];
169  population.push_back( s );
170  pickRandomIndex( sampleCount, p_kernelSize, s->sample );
171  }
172  }
173 
174  //evaluate species
175  speciesAlive = 0;
176  for( typename std::vector<Species*>::iterator it = population.begin(); it != population.end(); it++ )
177  {
178  Species& s = **it;
179  if( p_state.fitModel( s.sample, s.model ) )
180  {
181  s.fitness = 0;
182  for( size_t i = 0; i < p_state.getSampleCount(); i++ )
183  {
184  typename TModelFit::Real f = p_state.testSample( i, s.model );
185  if( f < p_fitnessThreshold )
186  {
187  s.fitness += f;
188  s.inliers.push_back( i );
189  }
190  }
191  ASSERT_( s.inliers.size() > 0 );
192 
193  s.fitness /= s.inliers.size();
194  // scale by the number of outliers
195  s.fitness *= (sampleCount - s.inliers.size());
196  speciesAlive++;
197  }
198  else
199  s.fitness = std::numeric_limits<typename TModelFit::Real>::max();
200  }
201 
202  if( !speciesAlive )
203  {
204  // the world is dead, no model was found
205  return false;
206  }
207 
208  //sort by fitness (ascending)
209  std::sort( sortedPopulation.begin(), sortedPopulation.end(), Species::compare );
210 
211  iter++;
212  }
213 
214  p_bestModel = sortedPopulation[0]->model;
215  p_inliers = sortedPopulation[0]->inliers;
216 
217  return !p_inliers.empty();
218 }
219 
220 } // namespace math
221 } // namespace mrpt
222 
223 #endif // math_modelsearch_h
#define min(a, b)
Scalar * iterator
Definition: eigen_plugins.h:23
GLdouble s
Definition: glext.h:3602
bool ransacSingleModel(const TModelFit &p_state, size_t p_kernelSize, const typename TModelFit::Real &p_fitnessThreshold, typename TModelFit::Model &p_bestModel, vector_size_t &p_inliers)
Run the ransac algorithm searching for a single model.
GLubyte GLubyte b
Definition: glext.h:5575
const double eps
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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, vector_size_t &p_inliers)
Run a generic programming version of ransac searching for a single model.
void pickRandomIndex(size_t p_size, size_t p_pick, vector_size_t &p_ind)
Select random (unique) indices from the 0..p_size sequence.
#define ASSERT_(f)
std::vector< size_t > vector_size_t
Definition: types_simple.h:25
GLubyte GLubyte GLubyte a
Definition: glext.h:5575
GLfloat GLfloat p
Definition: glext.h:5587



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019