31 size_t out_particle_count)
36 const size_t in_particle_count = particlesCount();
40 vector<double> log_ws;
41 log_ws.assign(in_particle_count, .0);
42 for (
size_t i = 0; i < in_particle_count; i++) log_ws[i] = getW(i);
49 performSubstitution(indxs);
52 for (
size_t i = 0; i < out_particle_count; i++)
63 const vector<double>& in_logWeights, vector<size_t>& out_indexes,
64 size_t out_particle_count)
71 size_t i, j, M = in_logWeights.size();
74 if (!out_particle_count) out_particle_count = M;
76 vector<double> linW(M, 0);
77 vector<double>::const_iterator inIt;
78 vector<double>::iterator outIt;
83 for (i = 0, inIt = in_logWeights.begin(), outIt = linW.begin(); i < M;
85 linW_SUM += ((*outIt) = exp((*inIt) - max_log_w));
89 linW *= 1.0 / linW_SUM;
99 mrpt::math::cumsum_tmpl<vector<double>, vector<double>>(linW, Q);
108 sort(T.begin(), T.end());
110 out_indexes.resize(out_particle_count);
113 while (i < out_particle_count)
117 out_indexes[i++] = (
unsigned int)j;
122 if (j >= M) j = M - 1;
134 std::vector<uint32_t> N(M);
136 for (i = 0; i < M; i++)
138 N[i] = int(M * linW[i]);
141 size_t N_rnd = out_particle_count >=
R ? (out_particle_count -
R)
147 out_indexes.resize(out_particle_count);
148 for (i = 0, j = 0; i < out_particle_count; i++)
149 for (
size_t k = 0; k < N[i]; k++) out_indexes[j++] = i;
160 vector<double> linW_mod(M);
161 const double M_R_1 = 1.0 / N_rnd;
162 for (i = 0; i < M; i++)
163 linW_mod[i] = M_R_1 * (M * linW[i] - N[i]);
167 mrpt::math::cumsum_tmpl<vector<double>, vector<double>>(
176 sort(T.begin(), T.end());
185 out_indexes[M_fixed + i++] = (
unsigned int)j;
190 if (j >= M) j = M - 1;
202 mrpt::math::cumsum_tmpl<vector<double>, vector<double>>(linW, Q);
206 vector<double> T(M + 1);
207 const double _1_M = 1.0 / M;
208 const double _1_M_eps = _1_M - 0.000001;
210 for (i = 0; i < M; i++)
218 out_indexes.resize(out_particle_count);
220 while (i < out_particle_count)
223 out_indexes[i++] = (
unsigned int)j;
227 if (j >= M) j = M - 1;
238 mrpt::math::cumsum_tmpl<vector<double>, vector<double>>(linW, Q);
242 vector<double> T(M + 1);
243 double _1_M = 1.0 / M;
245 for (i = 1; i < M; i++) T[i] = T[i - 1] + _1_M;
248 out_indexes.resize(out_particle_count);
250 while (i < out_particle_count)
253 out_indexes[i++] = (
unsigned int)j;
257 if (j >= M) j = M - 1;
264 "ERROR: Unknown resampling method selected: %i", method));
281 prediction_and_update_pfStandardProposal(
282 action, observation, PF_options);
285 prediction_and_update_pfAuxiliaryPFStandard(
286 action, observation, PF_options);
289 prediction_and_update_pfOptimalProposal(
290 action, observation, PF_options);
293 prediction_and_update_pfAuxiliaryPFOptimal(
294 action, observation, PF_options);
316 "Algorithm 'pfStandardProposal' is not implemented in inherited " 331 "Algorithm 'pfAuxiliaryPFStandard' is not implemented in inherited " 346 "Algorithm 'pfOptimalProposal' is not implemented in inherited class!");
360 "Algorithm 'pfAuxiliaryPFOptimal' is not implemented in inherited " 370 const void* observation)
const 382 "resamplingMethod must be 'prMultinomial' for a dynamic number " 385 size_t i, j = 666666, M = particlesCount();
390 m_fastDrawAuxiliary.CDF.resize(
391 1 + PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS, 0);
392 m_fastDrawAuxiliary.CDF_indexes.resize(
393 PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS, 0);
397 m_fastDrawAuxiliary.PDF.resize(M, 0);
403 for (i = 0; i < M; i++)
404 m_fastDrawAuxiliary.PDF[i] =
405 partEvaluator(PF_options,
this, i, action, observation);
407 m_fastDrawAuxiliary.PDF += -
math::maximum(m_fastDrawAuxiliary.PDF);
408 for (i = 0; i < M; i++)
409 SUM += m_fastDrawAuxiliary.PDF[i] = exp(m_fastDrawAuxiliary.PDF[i]);
412 m_fastDrawAuxiliary.PDF *= 1.0 / SUM;
415 for (i = 0; i < PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS; i++)
416 m_fastDrawAuxiliary.CDF[i] =
417 ((
double)i) / ((double)PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS);
418 m_fastDrawAuxiliary.CDF[PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS] = 1.0;
422 for (i = 0, j = 0; i < M && j < PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS;
425 double CDF_next = CDF + m_fastDrawAuxiliary.PDF[i];
426 if (i == (M - 1)) CDF_next = 1.0;
427 if (CDF_next > 1.0) CDF_next = 1.0;
429 while (m_fastDrawAuxiliary.CDF[j] < CDF_next)
430 m_fastDrawAuxiliary.CDF_indexes[j++] = (
unsigned int)i;
435 ASSERT_(j == PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS);
438 #if !defined(_MSC_VER) || (_MSC_VER > 1400) // <=VC2005 doesn't work with this! 441 <<
"\nm_fastDrawAuxiliary.CDF_indexes:" 442 << m_fastDrawAuxiliary.CDF_indexes << endl;
443 cout <<
"m_fastDrawAuxiliary.CDF:" 444 << m_fastDrawAuxiliary.CDF << endl;);
457 size_t i, M = particlesCount();
458 vector<double> PDF(M, 0);
459 for (i = 0; i < M; i++)
460 PDF[i] = partEvaluator(
461 PF_options,
this, i, action,
469 vector<size_t>::iterator it;
470 std::vector<uint32_t>::iterator it2;
471 m_fastDrawAuxiliary.alreadyDrawnIndexes.resize(idxs.size());
472 for (it = idxs.begin(),
473 it2 = m_fastDrawAuxiliary.alreadyDrawnIndexes.begin();
474 it != idxs.end(); ++it, ++it2)
475 *it2 = (
unsigned int)(*it);
477 m_fastDrawAuxiliary.alreadyDrawnNextOne = 0;
499 "resamplingMethod must be 'prMultinomial' for a dynamic number " 503 double CDF_next = -1.;
510 auto j = (size_t)floor(
511 draw * ((
double)PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS - 0.05));
512 CDF = m_fastDrawAuxiliary.CDF[j];
513 size_t i = m_fastDrawAuxiliary.CDF_indexes[j];
516 while (draw > (CDF_next = CDF + m_fastDrawAuxiliary.PDF[i]))
526 "\n[CParticleFilterCapable::fastDrawSample] DEBUG: draw=%f, " 527 "CDF=%f CDF_next=%f\n",
528 draw, CDF, CDF_next););
536 if (m_fastDrawAuxiliary.alreadyDrawnNextOne >=
537 m_fastDrawAuxiliary.alreadyDrawnIndexes.size())
539 "Have you called 'fastDrawSample' more times than the sample " 540 "size? Did you forget calling 'prepareFastCall' before?");
542 return m_fastDrawAuxiliary
543 .alreadyDrawnIndexes[m_fastDrawAuxiliary.alreadyDrawnNextOne++];
553 const vector<double>& in_logWeights, vector<double>& out_linWeights)
557 size_t N = in_logWeights.size();
559 out_linWeights.resize(N);
565 for (i = 0; i < N; i++) sumW += out_linWeights[i] = exp(in_logWeights[i]);
569 for (i = 0; i < N; i++) out_linWeights[i] /= sumW;
A namespace of pseudo-random numbers generators of diferent distributions.
#define THROW_EXCEPTION(msg)
std::string std::string format(std::string_view fmt, ARGS &&... args)
static void log2linearWeights(const std::vector< double > &in_logWeights, std::vector< double > &out_linWeights)
A static method to compute the linear, normalized (the sum the unity) weights from log-weights...
The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorith...
virtual void prediction_and_update_pfAuxiliaryPFStandard(const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options)
Performs the particle filter prediction/update stages for the algorithm "pfAuxiliaryPFStandard" (if n...
void prepareFastDrawSample(const bayes::CParticleFilter::TParticleFilterOptions &PF_options, TParticleProbabilityEvaluator partEvaluator=defaultEvaluator, const void *action=nullptr, const void *observation=nullptr) const
Prepares data structures for calling fastDrawSample method next.
double(*)(const bayes::CParticleFilter::TParticleFilterOptions &PF_options, const CParticleFilterCapable *obj, size_t index, const void *action, const void *observation) TParticleProbabilityEvaluator
A callback function type for evaluating the probability of m_particles of being selected, used in "fastDrawSample".
#define MRPT_END_WITH_CLEAN_UP(stuff)
Declares a class for storing a collection of robot actions.
#define ASSERT_(f)
Defines an assertion mechanism.
return_t drawUniform(const double Min, const double Max)
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm, scaled to the selected range.
This base provides a set of functions for maths stuff.
TParticleResamplingAlgorithm
Defines the different resampling algorithms.
TParticleResamplingAlgorithm resamplingMethod
The resampling algorithm to use (default=prMultinomial).
#define MRPT_CHECK_NORMAL_NUMBER(v)
Throws an exception if the number is NaN, IND, or +/-INF, or return the same number otherwise...
CONTAINER::Scalar maximum(const CONTAINER &v)
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
virtual void prediction_and_update_pfStandardProposal(const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options)
Performs the particle filter prediction/update stages for the algorithm "pfStandardProposal" (if not ...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
virtual void prediction_and_update_pfAuxiliaryPFOptimal(const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options)
Performs the particle filter prediction/update stages for the algorithm "pfAuxiliaryPFOptimal" (if no...
void prediction_and_update(const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options)
Performs the prediction stage of the Particle Filter.
The configuration of a particle filter.
void performResampling(const bayes::CParticleFilter::TParticleFilterOptions &PF_options, size_t out_particle_count=0)
Performs a resample of the m_particles, using the method selected in the constructor.
static const unsigned PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS
static void computeResampling(CParticleFilter::TParticleResamplingAlgorithm method, const std::vector< double > &in_logWeights, std::vector< size_t > &out_indexes, size_t out_particle_count=0)
A static method to perform the computation of the samples resulting from resampling a given set of pa...
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
void drawUniformVector(VEC &v, const double unif_min=0, const double unif_max=1)
Fills the given vector with independent, uniformly distributed samples.
virtual void prediction_and_update_pfOptimalProposal(const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options)
Performs the particle filter prediction/update stages for the algorithm "pfOptimalProposal" (if not i...
TParticleFilterAlgorithm PF_algorithm
The PF algorithm to use (default=pfStandardProposal) See TParticleFilterAlgorithm for the possibiliti...
size_t fastDrawSample(const bayes::CParticleFilter::TParticleFilterOptions &PF_options) const
Draws a random sample from the particle filter, in such a way that each particle has a probability pr...
bool adaptiveSampleSize
A flag that indicates whether the CParticleFilterCapable object should perform adative sample size (d...
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.