10#ifndef math_modelsearch_impl_h
11#define math_modelsearch_impl_h
13#ifndef math_modelsearch_h
24template<
typename TModelFit>
27 const typename TModelFit::Real& p_fitnessThreshold,
28 typename TModelFit::Model& p_bestModel,
31 size_t bestScore = std::string::npos;
33 size_t softIterLimit = 1;
34 size_t hardIterLimit = 100;
36 size_t nSamples = p_state.getSampleCount();
39 while ( iter < softIterLimit && iter < hardIterLimit )
41 bool degenerate =
true;
42 typename TModelFit::Model currentModel;
47 degenerate = !p_state.fitModel( ind, currentModel );
55 for(
size_t i = 0; i < nSamples; i++ )
57 if( p_state.testSample( i, currentModel ) < p_fitnessThreshold )
58 inliers.push_back( i );
63 const size_t ninliers = inliers.size();
64 bool update_estim_num_iters = (iter==0);
66 if ( ninliers > bestScore || (bestScore==std::string::npos && ninliers!=0))
69 p_bestModel = currentModel;
71 update_estim_num_iters =
true;
74 if (update_estim_num_iters)
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);
81 p = std::min( 1-eps, p);
82 softIterLimit = log(1-p) / log(p);
93template<
typename TModelFit>
96 const typename TModelFit::Real& p_fitnessThreshold,
97 size_t p_populationSize,
98 size_t p_maxIteration,
99 typename TModelFit::Model& p_bestModel,
104 std::vector<Species> storage;
105 std::vector<Species*> population;
106 std::vector<Species*> sortedPopulation;
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;
113 storage.resize( p_populationSize );
114 population.reserve( p_populationSize );
115 sortedPopulation.reserve( p_populationSize );
119 population.push_back( &*it );
120 sortedPopulation.push_back( &*it );
124 while ( iter < p_maxIteration )
133 for(; i < elderCnt; i++ )
135 population.push_back( sortedPopulation[i] );
139 int se = (int)speciesAlive;
140 for( ; i < elderCnt + siblingCnt ; i++ )
142 Species* sibling = sortedPopulation[--se];
143 population.push_back( sibling );
150 int p2 = (p1 > se / 2) ? (r2 % p1) : p1 + 1 + (r2 % (se-p1-1));
151 ASSERT_( p1 != p2 && p1 < se && p2 < se );
153 Species* a = sortedPopulation[p1];
154 Species* b = sortedPopulation[p2];
157 std::set<size_t> sampleSet;
158 sampleSet.insert( a->sample.begin(), a->sample.end() );
159 sampleSet.insert( b->sample.begin(), b->sample.end() );
161 sampleSet.insert( rand() % sampleCount );
166 for( ; i < (int)p_populationSize; i++ )
168 Species* s = sortedPopulation[i];
169 population.push_back( s );
179 if( p_state.fitModel( s.sample, s.model ) )
182 for(
size_t i = 0; i < p_state.getSampleCount(); i++ )
184 typename TModelFit::Real f = p_state.testSample( i, s.model );
185 if( f < p_fitnessThreshold )
188 s.inliers.push_back( i );
191 ASSERT_( s.inliers.size() > 0 );
193 s.fitness /= s.inliers.size();
195 s.fitness *= (sampleCount - s.inliers.size());
199 s.fitness = std::numeric_limits<typename TModelFit::Real>::max();
209 std::sort( sortedPopulation.begin(), sortedPopulation.end(), Species::compare );
214 p_bestModel = sortedPopulation[0]->model;
215 p_inliers = sortedPopulation[0]->inliers;
217 return !p_inliers.empty();
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
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.