Main MRPT website > C++ reference for MRPT 1.4.0
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-2016, 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
19namespace mrpt {
20 namespace math {
21
22//----------------------------------------------------------------------
23//! Run the ransac algorithm searching for a single model
24template<typename TModelFit>
25bool 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
93template<typename TModelFit>
94bool 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
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.
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.
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
#define ASSERT_(f)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.



Page generated by Doxygen 1.9.8 for MRPT 1.4.0 SVN: at Thu Dec 14 16:54:58 UTC 2023