24 const unsigned CParticleFilterCapable::PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS = 20;
29 void CParticleFilterCapable::performResampling(
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++)
53 performSubstitution( indxs );
56 for (
size_t i=0;i<out_particle_count;i++) setW(i, 0 );
64 void CParticleFilterCapable::computeResampling(
66 const vector<double> &in_logWeights,
67 vector<size_t> &out_indexes,
68 size_t out_particle_count )
75 size_t i,j,M=in_logWeights.size();
78 if (!out_particle_count)
79 out_particle_count = M;
81 vector<double> linW( M,0 );
88 for (i=0,inIt=in_logWeights.begin(),outIt=linW.begin();i<M;i++,inIt++,outIt++)
89 linW_SUM += ( (*outIt) = exp( (*inIt) - max_log_w ) );
93 linW *= 1.0 / linW_SUM;
97 case CParticleFilter::prMultinomial:
103 mrpt::math::cumsum_tmpl<vector<double>,vector<double>,
double>(linW, Q);
112 sort( T.begin(), T.end() );
114 out_indexes.resize(out_particle_count);
117 while (i < out_particle_count)
121 out_indexes[i++] = (
unsigned int) j;
132 case CParticleFilter::prResidual:
142 N[i] = int( M*linW[i] );
145 size_t N_rnd = out_particle_count>=
R ? (out_particle_count-
R) : 0;
148 out_indexes.resize(out_particle_count);
149 for (i=0, j=0 ;i<out_particle_count;i++)
150 for (
size_t k=0;k<N[i];k++)
151 out_indexes[j++] = i;
161 vector<double> linW_mod(M);
162 const double M_R_1 = 1.0/N_rnd;
164 linW_mod[i] = M_R_1 * (M*linW[i]-N[i]);
168 mrpt::math::cumsum_tmpl<vector<double>,vector<double>,
double>(linW_mod, Q);
176 sort( T.begin(), T.end() );
185 out_indexes[M_fixed + i++] = (
unsigned int) j;
196 case CParticleFilter::prStratified:
202 mrpt::math::cumsum_tmpl<vector<double>,vector<double>,
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;
217 out_indexes.resize(out_particle_count);
219 while (i < out_particle_count)
222 out_indexes[i++] = (
unsigned int)j;
231 case CParticleFilter::prSystematic:
237 mrpt::math::cumsum_tmpl<vector<double>,vector<double>,
double>(linW, Q);
241 vector<double> T(M+1);
242 double _1_M = 1.0 / M;
244 for (i=1;i<M;i++) T[i] = T[i-1] + _1_M;
247 out_indexes.resize(out_particle_count);
249 while (i < out_particle_count)
252 out_indexes[i++] = (
unsigned int)j;
272 void CParticleFilterCapable::prediction_and_update(
279 case CParticleFilter::pfStandardProposal:
280 prediction_and_update_pfStandardProposal( action, observation, PF_options );
282 case CParticleFilter::pfAuxiliaryPFStandard:
283 prediction_and_update_pfAuxiliaryPFStandard( action, observation, PF_options );
285 case CParticleFilter::pfOptimalProposal:
286 prediction_and_update_pfOptimalProposal( action, observation, PF_options );
288 case CParticleFilter::pfAuxiliaryPFOptimal:
289 prediction_and_update_pfAuxiliaryPFOptimal( action, observation, PF_options );
303 void CParticleFilterCapable::prediction_and_update_pfStandardProposal(
309 THROW_EXCEPTION(
"Algorithm 'pfStandardProposal' is not implemented in inherited class!");
314 void CParticleFilterCapable::prediction_and_update_pfAuxiliaryPFStandard(
320 THROW_EXCEPTION(
"Algorithm 'pfAuxiliaryPFStandard' is not implemented in inherited class!");
325 void CParticleFilterCapable::prediction_and_update_pfOptimalProposal(
331 THROW_EXCEPTION(
"Algorithm 'pfOptimalProposal' is not implemented in inherited class!");
336 void CParticleFilterCapable::prediction_and_update_pfAuxiliaryPFOptimal(
342 THROW_EXCEPTION(
"Algorithm 'pfAuxiliaryPFOptimal' is not implemented in inherited class!");
348 void CParticleFilterCapable::prepareFastDrawSample(
350 TParticleProbabilityEvaluator partEvaluator,
352 const void * observation )
const 363 THROW_EXCEPTION(
"resamplingMethod must be 'prMultinomial' for a dynamic number of particles!");
365 size_t i,j=666666,M = particlesCount();
370 m_fastDrawAuxiliary.CDF.resize( 1+PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS, 0);
371 m_fastDrawAuxiliary.CDF_indexes.resize( PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS, 0);
375 m_fastDrawAuxiliary.PDF.resize( M, 0);
381 for (i=0;i<M;i++) m_fastDrawAuxiliary.PDF[i] = partEvaluator(PF_options,
this,i,action,observation);
383 m_fastDrawAuxiliary.PDF += -
math::maximum( m_fastDrawAuxiliary.PDF );
384 for (i=0;i<M;i++) SUM += m_fastDrawAuxiliary.PDF[i] = exp( m_fastDrawAuxiliary.PDF[i] );
387 m_fastDrawAuxiliary.PDF *= 1.0/SUM;
390 for (i=0;i<PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS;i++)
391 m_fastDrawAuxiliary.CDF[i] = ((
double)i) / ((double)PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS);
392 m_fastDrawAuxiliary.CDF[PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS] = 1.0;
396 for (i=0,j=0;i<M && j<PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS;i++)
398 double CDF_next = CDF + m_fastDrawAuxiliary.PDF[i];
399 if (i==(M-1)) CDF_next = 1.0;
400 if (CDF_next>1.0) CDF_next = 1.0;
402 while ( m_fastDrawAuxiliary.CDF[j] < CDF_next )
403 m_fastDrawAuxiliary.CDF_indexes[j++] = (
unsigned int)i;
408 ASSERT_( j == PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS );
411 #if !defined(_MSC_VER) || (_MSC_VER>1400) // <=VC2005 doesn't work with this! 414 cout <<
"j=" << j <<
"\nm_fastDrawAuxiliary.CDF_indexes:" << m_fastDrawAuxiliary.CDF_indexes << endl; \
415 cout <<
"m_fastDrawAuxiliary.CDF:" << m_fastDrawAuxiliary.CDF << endl; \
429 size_t i,M = particlesCount();
430 vector<double> PDF(M,0);
432 PDF[i] = partEvaluator(PF_options,
this,i,action,observation);
441 m_fastDrawAuxiliary.alreadyDrawnIndexes.resize( idxs.size() );
442 for ( it=idxs.begin(),it2=m_fastDrawAuxiliary.alreadyDrawnIndexes.begin();it!=idxs.end(); ++it, ++it2)
443 *it2 = (
unsigned int)(*it);
445 m_fastDrawAuxiliary.alreadyDrawnNextOne = 0;
465 THROW_EXCEPTION(
"resamplingMethod must be 'prMultinomial' for a dynamic number of particles!");
474 size_t j = (size_t)floor( draw * ((
double)PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS-0.05) );
475 CDF = m_fastDrawAuxiliary.CDF[j];
476 size_t i = m_fastDrawAuxiliary.CDF_indexes[j];
479 while ( draw > (CDF_next = CDF+m_fastDrawAuxiliary.PDF[i]) )
488 printf(
"\n[CParticleFilterCapable::fastDrawSample] DEBUG: draw=%f, CDF=%f CDF_next=%f\n",draw,CDF,CDF_next); \
497 if ( m_fastDrawAuxiliary.alreadyDrawnNextOne>=m_fastDrawAuxiliary.alreadyDrawnIndexes.size() )
498 THROW_EXCEPTION(
"Have you called 'fastDrawSample' more times than the sample size? Did you forget calling 'prepareFastCall' before?");
500 return m_fastDrawAuxiliary.alreadyDrawnIndexes[m_fastDrawAuxiliary.alreadyDrawnNextOne++];
509 void CParticleFilterCapable::log2linearWeights(
510 const vector<double> &in_logWeights,
511 vector<double> &out_linWeights )
515 size_t N = in_logWeights.size();
517 out_linWeights.resize( N );
524 sumW += out_linWeights[i] = exp( in_logWeights[i] );
529 out_linWeights[i] /= sumW;
A namespace of pseudo-random numbers genrators of diferent distributions.
double drawUniform(const double Min, const double Max)
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm, scaled to the selected range.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
std::vector< uint32_t > vector_uint
#define MRPT_END_WITH_CLEAN_UP(stuff)
#define MRPT_CHECK_NORMAL_NUMBER(val)
The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorith...
#define THROW_EXCEPTION(msg)
BASE_IMPEXP CRandomGenerator randomGenerator
A static instance of a CRandomGenerator class, for use in single-thread applications.
const Scalar * const_iterator
Declares a class for storing a collection of robot actions.
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_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
CONTAINER::Scalar maximum(const CONTAINER &v)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
The configuration of a particle filter.
void drawUniformVector(VEC &v, const double unif_min=0, const double unif_max=1)
Fills the given vector with independent, uniformly distributed samples.
TParticleFilterAlgorithm PF_algorithm
The PF algorithm to use (default=pfStandardProposal) See TParticleFilterAlgorithm for the posibilitie...
bool adaptiveSampleSize
A flag that indicates whether the CParticleFilterCapable object should perform adative sample size (d...