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) |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (c) 2005-2013, Individual contributors, see AUTHORS file |
7  | Copyright (c) 2005-2013, MAPIR group, University of Malaga |
8  | Copyright (c) 2012-2013, University of Almeria |
9  | All rights reserved. |
10  | |
11  | Redistribution and use in source and binary forms, with or without |
12  | modification, are permitted provided that the following conditions are |
13  | met: |
14  | * Redistributions of source code must retain the above copyright |
15  | notice, this list of conditions and the following disclaimer. |
16  | * Redistributions in binary form must reproduce the above copyright |
17  | notice, this list of conditions and the following disclaimer in the |
18  | documentation and/or other materials provided with the distribution. |
19  | * Neither the name of the copyright holders nor the |
20  | names of its contributors may be used to endorse or promote products |
21  | derived from this software without specific prior written permission.|
22  | |
23  | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
24  | 'AS IS' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED |
25  | TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR|
26  | PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS BE LIABLE |
27  | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL|
28  | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR|
29  | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) |
30  | HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, |
31  | STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
32  | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
33  | POSSIBILITY OF SUCH DAMAGE. |
34  +---------------------------------------------------------------------------+ */
35 
36 #ifndef math_modelsearch_impl_h
37 #define math_modelsearch_impl_h
38 
39 #ifndef math_modelsearch_h
40 # include "model_search.h"
41 #endif
42 
43 #include <limits>
44 
45 namespace mrpt {
46  namespace math {
47 
48 //----------------------------------------------------------------------
49 //! Run the ransac algorithm searching for a single model
50 template<typename TModelFit>
51 bool ModelSearch::ransacSingleModel( const TModelFit& p_state,
52  size_t p_kernelSize,
53  const typename TModelFit::Real& p_fitnessThreshold,
54  typename TModelFit::Model& p_bestModel,
55  vector_size_t& p_inliers )
56 {
57  size_t bestScore = std::string::npos; // npos will mean "none"
58  size_t iter = 0;
59  size_t softIterLimit = 1; // will be updated by the size of inliers
60  size_t hardIterLimit = 100; // a fixed iteration step
61  p_inliers.clear();
62  size_t nSamples = p_state.getSampleCount();
63  vector_size_t ind( p_kernelSize );
64 
65  while ( iter < softIterLimit && iter < hardIterLimit )
66  {
67  bool degenerate = true;
68  typename TModelFit::Model currentModel;
69  size_t i = 0;
70  while ( degenerate )
71  {
72  pickRandomIndex( nSamples, p_kernelSize, ind );
73  degenerate = !p_state.fitModel( ind, currentModel );
74  i++;
75  if( i > 100 )
76  return false;
77  }
78 
79  vector_size_t inliers;
80 
81  for( size_t i = 0; i < nSamples; i++ )
82  {
83  if( p_state.testSample( i, currentModel ) < p_fitnessThreshold )
84  inliers.push_back( i );
85  }
86  ASSERT_( inliers.size() > 0 );
87 
88  // Find the number of inliers to this model.
89  const size_t ninliers = inliers.size();
90  bool update_estim_num_iters = (iter==0); // Always update on the first iteration, regardless of the result (even for ninliers=0)
91 
92  if ( ninliers > bestScore || (bestScore==std::string::npos && ninliers!=0))
93  {
94  bestScore = ninliers;
95  p_bestModel = currentModel;
96  p_inliers = inliers;
97  update_estim_num_iters = true;
98  }
99 
100  if (update_estim_num_iters)
101  {
102  // Update the estimation of maxIter to pick dataset with no outliers at propability p
103  double f = ninliers / static_cast<double>( nSamples );
104  double p = 1 - pow( f, static_cast<double>( p_kernelSize ) );
105  const double eps = std::numeric_limits<double>::epsilon();
106  p = std::max( eps, p); // Avoid division by -Inf
107  p = std::min( 1-eps, p); // Avoid division by 0.
108  softIterLimit = log(1-p) / log(p);
109  }
110 
111  iter++;
112  }
113 
114  return true;
115 }
116 
117 //----------------------------------------------------------------------
118 //! Run a generic programming version of ransac searching for a single model
119 template<typename TModelFit>
120 bool ModelSearch::geneticSingleModel( const TModelFit& p_state,
121  size_t p_kernelSize,
122  const typename TModelFit::Real& p_fitnessThreshold,
123  size_t p_populationSize,
124  size_t p_maxIteration,
125  typename TModelFit::Model& p_bestModel,
126  vector_size_t& p_inliers )
127 {
128  // a single specie of the population
129  typedef TSpecies<TModelFit> Species;
130  std::vector<Species> storage;
131  std::vector<Species*> population;
132  std::vector<Species*> sortedPopulation;
133 
134  size_t sampleCount = p_state.getSampleCount();
135  int elderCnt = (int)p_populationSize/3;
136  int siblingCnt = (p_populationSize - elderCnt) / 2;
137  int speciesAlive = 0;
138 
139  storage.resize( p_populationSize );
140  population.reserve( p_populationSize );
141  sortedPopulation.reserve( p_populationSize );
142  for( typename std::vector<Species>::iterator it = storage.begin(); it != storage.end(); it++ )
143  {
144  pickRandomIndex( sampleCount, p_kernelSize, it->sample );
145  population.push_back( &*it );
146  sortedPopulation.push_back( &*it );
147  }
148 
149  size_t iter = 0;
150  while ( iter < p_maxIteration )
151  {
152  if( iter > 0 )
153  {
154  //generate new population: old, siblings, new
155  population.clear();
156  int i = 0;
157 
158  //copy the best elders
159  for(; i < elderCnt; i++ )
160  {
161  population.push_back( sortedPopulation[i] );
162  }
163 
164  // mate elders to make siblings
165  int se = (int)speciesAlive; //dead species cannot mate
166  for( ; i < elderCnt + siblingCnt ; i++ )
167  {
168  Species* sibling = sortedPopulation[--se];
169  population.push_back( sibling );
170 
171  //pick two parents, from the species not yet refactored
172  //better elders has more chance to mate as they are removed later from the list
173  int r1 = rand();
174  int r2 = rand();
175  int p1 = r1 % se;
176  int p2 = (p1 > se / 2) ? (r2 % p1) : p1 + 1 + (r2 % (se-p1-1));
177  ASSERT_( p1 != p2 && p1 < se && p2 < se );
178  ASSERT_( se >= elderCnt );
179  Species* a = sortedPopulation[p1];
180  Species* b = sortedPopulation[p2];
181 
182  // merge the sample candidates
183  std::set<size_t> sampleSet;
184  sampleSet.insert( a->sample.begin(), a->sample.end() );
185  sampleSet.insert( b->sample.begin(), b->sample.end() );
186  //mutate - add a random sample that will be selected with some (non-zero) probability
187  sampleSet.insert( rand() % sampleCount );
188  pickRandomIndex( sampleSet, p_kernelSize, sibling->sample );
189  }
190 
191  // generate some new random species
192  for( ; i < (int)p_populationSize; i++ )
193  {
194  Species* s = sortedPopulation[i];
195  population.push_back( s );
196  pickRandomIndex( sampleCount, p_kernelSize, s->sample );
197  }
198  }
199 
200  //evaluate species
201  speciesAlive = 0;
202  for( typename std::vector<Species*>::iterator it = population.begin(); it != population.end(); it++ )
203  {
204  Species& s = **it;
205  if( p_state.fitModel( s.sample, s.model ) )
206  {
207  s.fitness = 0;
208  for( size_t i = 0; i < p_state.getSampleCount(); i++ )
209  {
210  typename TModelFit::Real f = p_state.testSample( i, s.model );
211  if( f < p_fitnessThreshold )
212  {
213  s.fitness += f;
214  s.inliers.push_back( i );
215  }
216  }
217  ASSERT_( s.inliers.size() > 0 );
218 
219  s.fitness /= s.inliers.size();
220  // scale by the number of outliers
221  s.fitness *= (sampleCount - s.inliers.size());
222  speciesAlive++;
223  }
224  else
225  s.fitness = std::numeric_limits<typename TModelFit::Real>::max();
226  }
227 
228  if( !speciesAlive )
229  {
230  // the world is dead, no model was found
231  return false;
232  }
233 
234  //sort by fitness (ascending)
235  std::sort( sortedPopulation.begin(), sortedPopulation.end(), Species::compare );
236 
237  iter++;
238  }
239 
240  p_bestModel = sortedPopulation[0]->model;
241  p_inliers = sortedPopulation[0]->inliers;
242 
243  return !p_inliers.empty();
244 }
245 
246 } // namespace math
247 } // namespace mrpt
248 
249 #endif // math_modelsearch_h
Scalar * iterator
Definition: eigen_plugins.h:49
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.
std::vector< size_t > vector_size_t
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)



Page generated by Doxygen 1.8.14 for MRPT 1.0.2 SVN: at lun oct 28 00:52:41 CET 2019 Hosted on:
SourceForge.net Logo