Main MRPT website > C++ reference
MRPT logo
model_search_impl.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 
29 #ifndef math_modelsearch_impl_h
30 #define math_modelsearch_impl_h
31 
32 #ifndef math_modelsearch_h
33 # include "model_search.h"
34 #endif
35 
36 #include <limits>
37 
38 namespace mrpt {
39  namespace math {
40 
41 //----------------------------------------------------------------------
42 //! Run the ransac algorithm searching for a single model
43 template<typename TModelFit>
44 bool ModelSearch::ransacSingleModel( const TModelFit& p_state,
45  size_t p_kernelSize,
46  const typename TModelFit::Real& p_fitnessThreshold,
47  typename TModelFit::Model& p_bestModel,
48  vector_size_t& p_inliers )
49 {
50  size_t bestScore = std::string::npos; // npos will mean "none"
51  size_t iter = 0;
52  size_t softIterLimit = 1; // will be updated by the size of inliers
53  size_t hardIterLimit = 100; // a fixed iteration step
54  p_inliers.clear();
55  size_t nSamples = p_state.getSampleCount();
56  vector_size_t ind( p_kernelSize );
57 
58  while ( iter < softIterLimit && iter < hardIterLimit )
59  {
60  bool degenerate = true;
61  typename TModelFit::Model currentModel;
62  size_t i = 0;
63  while ( degenerate )
64  {
65  pickRandomIndex( nSamples, p_kernelSize, ind );
66  degenerate = !p_state.fitModel( ind, currentModel );
67  i++;
68  if( i > 100 )
69  return false;
70  }
71 
72  vector_size_t inliers;
73 
74  for( size_t i = 0; i < nSamples; i++ )
75  {
76  if( p_state.testSample( i, currentModel ) < p_fitnessThreshold )
77  inliers.push_back( i );
78  }
79  ASSERT_( inliers.size() > 0 );
80 
81  // Find the number of inliers to this model.
82  const size_t ninliers = inliers.size();
83  bool update_estim_num_iters = (iter==0); // Always update on the first iteration, regardless of the result (even for ninliers=0)
84 
85  if ( ninliers > bestScore || (bestScore==std::string::npos && ninliers!=0))
86  {
87  bestScore = ninliers;
88  p_bestModel = currentModel;
89  p_inliers = inliers;
90  update_estim_num_iters = true;
91  }
92 
93  if (update_estim_num_iters)
94  {
95  // Update the estimation of maxIter to pick dataset with no outliers at propability p
96  double f = ninliers / static_cast<double>( nSamples );
97  double p = 1 - pow( f, static_cast<double>( p_kernelSize ) );
98  const double eps = std::numeric_limits<double>::epsilon();
99  p = std::max( eps, p); // Avoid division by -Inf
100  p = std::min( 1-eps, p); // Avoid division by 0.
101  softIterLimit = log(1-p) / log(p);
102  }
103 
104  iter++;
105  }
106 
107  return true;
108 }
109 
110 //----------------------------------------------------------------------
111 //! Run a generic programming version of ransac searching for a single model
112 template<typename TModelFit>
113 bool ModelSearch::geneticSingleModel( const TModelFit& p_state,
114  size_t p_kernelSize,
115  const typename TModelFit::Real& p_fitnessThreshold,
116  size_t p_populationSize,
117  size_t p_maxIteration,
118  typename TModelFit::Model& p_bestModel,
119  vector_size_t& p_inliers )
120 {
121  // a single specie of the population
122  typedef TSpecies<TModelFit> Species;
123  std::vector<Species> storage;
124  std::vector<Species*> population;
125  std::vector<Species*> sortedPopulation;
126 
127  size_t sampleCount = p_state.getSampleCount();
128  int elderCnt = (int)p_populationSize/3;
129  int siblingCnt = (p_populationSize - elderCnt) / 2;
130  int speciesAlive = 0;
131 
132  storage.resize( p_populationSize );
133  population.reserve( p_populationSize );
134  sortedPopulation.reserve( p_populationSize );
135  for( typename std::vector<Species>::iterator it = storage.begin(); it != storage.end(); it++ )
136  {
137  pickRandomIndex( sampleCount, p_kernelSize, it->sample );
138  population.push_back( &*it );
139  sortedPopulation.push_back( &*it );
140  }
141 
142  size_t iter = 0;
143  while ( iter < p_maxIteration )
144  {
145  if( iter > 0 )
146  {
147  //generate new population: old, siblings, new
148  population.clear();
149  int i = 0;
150 
151  //copy the best elders
152  for(; i < elderCnt; i++ )
153  {
154  population.push_back( sortedPopulation[i] );
155  }
156 
157  // mate elders to make siblings
158  int se = (int)speciesAlive; //dead species cannot mate
159  for( ; i < elderCnt + siblingCnt ; i++ )
160  {
161  Species* sibling = sortedPopulation[--se];
162  population.push_back( sibling );
163 
164  //pick two parents, from the species not yet refactored
165  //better elders has more chance to mate as they are removed later from the list
166  int r1 = rand();
167  int r2 = rand();
168  int p1 = r1 % se;
169  int p2 = (p1 > se / 2) ? (r2 % p1) : p1 + 1 + (r2 % (se-p1-1));
170  ASSERT_( p1 != p2 && p1 < se && p2 < se );
171  ASSERT_( se >= elderCnt );
172  Species* a = sortedPopulation[p1];
173  Species* b = sortedPopulation[p2];
174 
175  // merge the sample candidates
176  std::set<size_t> sampleSet;
177  sampleSet.insert( a->sample.begin(), a->sample.end() );
178  sampleSet.insert( b->sample.begin(), b->sample.end() );
179  //mutate - add a random sample that will be selected with some (non-zero) probability
180  sampleSet.insert( rand() % sampleCount );
181  pickRandomIndex( sampleSet, p_kernelSize, sibling->sample );
182  }
183 
184  // generate some new random species
185  for( ; i < (int)p_populationSize; i++ )
186  {
187  Species* s = sortedPopulation[i];
188  population.push_back( s );
189  pickRandomIndex( sampleCount, p_kernelSize, s->sample );
190  }
191  }
192 
193  //evaluate species
194  speciesAlive = 0;
195  for( typename std::vector<Species*>::iterator it = population.begin(); it != population.end(); it++ )
196  {
197  Species& s = **it;
198  if( p_state.fitModel( s.sample, s.model ) )
199  {
200  s.fitness = 0;
201  for( size_t i = 0; i < p_state.getSampleCount(); i++ )
202  {
203  typename TModelFit::Real f = p_state.testSample( i, s.model );
204  if( f < p_fitnessThreshold )
205  {
206  s.fitness += f;
207  s.inliers.push_back( i );
208  }
209  }
210  ASSERT_( s.inliers.size() > 0 );
211 
212  s.fitness /= s.inliers.size();
213  // scale by the number of outliers
214  s.fitness *= (sampleCount - s.inliers.size());
215  speciesAlive++;
216  }
217  else
218  s.fitness = std::numeric_limits<typename TModelFit::Real>::max();
219  }
220 
221  if( !speciesAlive )
222  {
223  // the world is dead, no model was found
224  return false;
225  }
226 
227  //sort by fitness (ascending)
228  std::sort( sortedPopulation.begin(), sortedPopulation.end(), Species::compare );
229 
230  iter++;
231  }
232 
233  p_bestModel = sortedPopulation[0]->model;
234  p_inliers = sortedPopulation[0]->inliers;
235 
236  return !p_inliers.empty();
237 }
238 
239 } // namespace math
240 } // namespace mrpt
241 
242 #endif // math_modelsearch_h


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013