10 #ifndef math_modelsearch_impl_h 11 #define math_modelsearch_impl_h 13 #ifndef math_modelsearch_h 25 template <
typename TModelFit>
27 const TModelFit& p_state,
size_t p_kernelSize,
28 const typename TModelFit::Real& p_fitnessThreshold,
29 typename TModelFit::Model& p_bestModel,
vector_size_t& p_inliers)
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);
49 if (i > 100)
return false;
54 for (
size_t i = 0; i < nSamples; i++)
56 if (p_state.testSample(i, currentModel) < p_fitnessThreshold)
62 const size_t ninliers = inliers.size();
63 bool update_estim_num_iters =
67 if (ninliers > bestScore ||
68 (bestScore == std::string::npos && ninliers != 0))
71 p_bestModel = currentModel;
73 update_estim_num_iters =
true;
76 if (update_estim_num_iters)
80 double f = ninliers /
static_cast<double>(nSamples);
81 double p = 1 - pow(f, static_cast<double>(p_kernelSize));
82 const double eps = std::numeric_limits<double>::epsilon();
85 softIterLimit = log(1 -
p) / log(
p);
96 template <
typename TModelFit>
98 const TModelFit& p_state,
size_t p_kernelSize,
99 const typename TModelFit::Real& p_fitnessThreshold,
size_t p_populationSize,
100 size_t p_maxIteration,
typename TModelFit::Model& p_bestModel,
105 std::vector<Species> storage;
106 std::vector<Species*> population;
107 std::vector<Species*> sortedPopulation;
109 size_t sampleCount = p_state.getSampleCount();
110 int elderCnt = (int)p_populationSize / 3;
111 int siblingCnt = (p_populationSize - elderCnt) / 2;
112 int speciesAlive = 0;
114 storage.resize(p_populationSize);
115 population.reserve(p_populationSize);
116 sortedPopulation.reserve(p_populationSize);
118 it != storage.end(); it++)
121 population.push_back(&*it);
122 sortedPopulation.push_back(&*it);
126 while (iter < p_maxIteration)
135 for (; i < elderCnt; i++)
137 population.push_back(sortedPopulation[i]);
141 int se = (int)speciesAlive;
142 for (; i < elderCnt + siblingCnt; i++)
144 Species* sibling = sortedPopulation[--se];
145 population.push_back(sibling);
154 (p1 > se / 2) ? (r2 % p1) : p1 + 1 + (r2 % (se - p1 - 1));
155 ASSERT_(p1 != p2 && p1 < se && p2 < se);
157 Species*
a = sortedPopulation[p1];
158 Species*
b = sortedPopulation[p2];
161 std::set<size_t> sampleSet;
162 sampleSet.insert(
a->sample.begin(),
a->sample.end());
163 sampleSet.insert(
b->sample.begin(),
b->sample.end());
166 sampleSet.insert(rand() % sampleCount);
171 for (; i < (int)p_populationSize; i++)
173 Species*
s = sortedPopulation[i];
174 population.push_back(
s);
182 it != population.end(); it++)
185 if (p_state.fitModel(
s.sample,
s.model))
188 for (
size_t i = 0; i < p_state.getSampleCount(); i++)
190 typename TModelFit::Real f = p_state.testSample(i,
s.model);
191 if (f < p_fitnessThreshold)
194 s.inliers.push_back(i);
199 s.fitness /=
s.inliers.size();
201 s.fitness *= (sampleCount -
s.inliers.size());
206 std::numeric_limits<typename TModelFit::Real>::max();
217 sortedPopulation.begin(), sortedPopulation.end(), Species::compare);
222 p_bestModel = sortedPopulation[0]->model;
223 p_inliers = sortedPopulation[0]->inliers;
225 return !p_inliers.empty();
231 #endif // math_modelsearch_h
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.
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.
std::vector< size_t > vector_size_t
GLubyte GLubyte GLubyte a