Main MRPT website > C++ reference for MRPT 1.5.6
CParticleFilterCapable.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, 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 #include "base-precomp.h" // Precompiled headers
11 
13 #include <mrpt/random.h>
14 #include <mrpt/math/ops_vectors.h>
15 
16 using namespace mrpt;
17 using namespace mrpt::utils;
18 using namespace mrpt::bayes;
19 using namespace mrpt::random;
20 using namespace mrpt::math;
21 using namespace std;
22 
23 
24 const unsigned CParticleFilterCapable::PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS = 20;
25 
26 /*---------------------------------------------------------------
27  performResampling
28  ---------------------------------------------------------------*/
29 void CParticleFilterCapable::performResampling(
31  size_t out_particle_count )
32 {
34 
35  // Make a vector with the particles' log. weights:
36  const size_t in_particle_count = particlesCount();
37  ASSERT_(in_particle_count>0)
38 
39  vector<size_t> indxs;
40  vector<double> log_ws;
41  log_ws.assign(in_particle_count, .0);
42  for (size_t i=0;i<in_particle_count;i++)
43  log_ws[i] = getW(i);
44 
45  // Compute the surviving indexes:
46  computeResampling(
47  PF_options.resamplingMethod,
48  log_ws,
49  indxs,
50  out_particle_count );
51 
52  // And perform the particle replacement:
53  performSubstitution( indxs );
54 
55  // Finally, equal weights:
56  for (size_t i=0;i<out_particle_count;i++) setW(i, 0 /* Logarithmic weight */ );
57 
58  MRPT_END
59 }
60 
61 /*---------------------------------------------------------------
62  resample
63  ---------------------------------------------------------------*/
64 void CParticleFilterCapable::computeResampling(
66  const vector<double> &in_logWeights,
67  vector<size_t> &out_indexes,
68  size_t out_particle_count )
69 {
71 
72  // Compute the normalized linear weights:
73  // The array "linW" will be the input to the actual
74  // resampling algorithms.
75  size_t i,j,M=in_logWeights.size();
76  ASSERT_(M>0)
77 
78  if (!out_particle_count)
79  out_particle_count = M;
80 
81  vector<double> linW( M,0 );
84  double linW_SUM=0;
85 
86  // This is to avoid float point range problems:
87  double max_log_w = math::maximum( in_logWeights );
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 ) );
90 
91  // Normalize weights:
92  ASSERT_(linW_SUM>0);
93  linW *= 1.0 / linW_SUM;
94 
95  switch ( method )
96  {
97  case CParticleFilter::prMultinomial:
98  {
99  // ==============================================
100  // Select with replacement
101  // ==============================================
102  vector<double> Q;
103  mrpt::math::cumsum_tmpl<vector<double>,vector<double>,double>(linW, Q);
104  Q[M-1] = 1.1;
105 
106  vector<double> T(M);
107  randomGenerator.drawUniformVector(T,0.0, 0.999999);
108  T.push_back(1.0);
109 
110  // Sort:
111  // --------------------
112  sort( T.begin(), T.end() );
113 
114  out_indexes.resize(out_particle_count);
115  i=j=0;
116 
117  while (i < out_particle_count)
118  {
119  if (T[i]<Q[j])
120  {
121  out_indexes[i++] = (unsigned int) j;
122  }
123  else
124  {
125  j++;
126  if (j>=M) j=M-1;
127  }
128  }
129  }
130  break; // end of "Select with replacement"
131 
132  case CParticleFilter::prResidual:
133  {
134  // ==============================================
135  // prResidual
136  // ==============================================
137  // Repetition counts:
138  vector_uint N(M);
139  size_t R=0; // Remainder or residual count
140  for (i=0;i<M;i++)
141  {
142  N[i] = int( M*linW[i] );
143  R+= N[i];
144  }
145  size_t N_rnd = out_particle_count>=R ? (out_particle_count-R) : 0; // # of particles to be drawn randomly (the "residual" part)
146 
147  // Fillout the deterministic part of the resampling:
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;
152 
153  size_t M_fixed = j;
154 
155  // Prepare a multinomial resampling with the residual part,
156  // using the modified weights:
157  // ----------------------------------------------------------
158  if (N_rnd) // If there are "residual" part (should be virtually always!)
159  {
160  // Compute modified weights:
161  vector<double> linW_mod(M);
162  const double M_R_1 = 1.0/N_rnd;
163  for (i=0;i<M;i++)
164  linW_mod[i] = M_R_1 * (M*linW[i]-N[i]);
165 
166  // perform resampling:
167  vector<double> Q;
168  mrpt::math::cumsum_tmpl<vector<double>,vector<double>,double>(linW_mod, Q);
169  Q[M-1] = 1.1;
170 
171  vector<double> T(M);
172  randomGenerator.drawUniformVector(T, 0.0 , 0.999999);
173  T.push_back(1.0);
174 
175  // Sort:
176  sort( T.begin(), T.end() );
177 
178  i=0;
179  j=0;
180 
181  while (i < N_rnd)
182  {
183  if (T[i]<Q[j])
184  {
185  out_indexes[M_fixed + i++] = (unsigned int) j;
186  }
187  else
188  {
189  j++;
190  if (j>=M) j=M-1;
191  }
192  }
193  } // end if N_rnd!=0
194  }
195  break;
196  case CParticleFilter::prStratified:
197  {
198  // ==============================================
199  // prStratified
200  // ==============================================
201  vector<double> Q;
202  mrpt::math::cumsum_tmpl<vector<double>,vector<double>,double>(linW, Q);
203  Q[M-1] = 1.1;
204 
205  // Stratified-uniform random vector:
206  vector<double> T(M+1);
207  const double _1_M = 1.0 / M;
208  const double _1_M_eps = _1_M - 0.000001;
209  double T_offset = 0;
210  for (i=0;i<M;i++)
211  {
212  T[i] = T_offset + randomGenerator.drawUniform(0.0,_1_M_eps);
213  T_offset+= _1_M;
214  }
215  T[M] = 1;
216 
217  out_indexes.resize(out_particle_count);
218  i=j=0;
219  while (i < out_particle_count)
220  {
221  if (T[i]<Q[j])
222  out_indexes[i++] = (unsigned int)j;
223  else
224  {
225  j++;
226  if (j>=M) j=M-1;
227  }
228  }
229  }
230  break;
231  case CParticleFilter::prSystematic:
232  {
233  // ==============================================
234  // prSystematic
235  // ==============================================
236  vector<double> Q;
237  mrpt::math::cumsum_tmpl<vector<double>,vector<double>,double>(linW, Q);
238  Q[M-1] = 1.1;
239 
240  // Uniform random vector:
241  vector<double> T(M+1);
242  double _1_M = 1.0 / M;
243  T[0] = randomGenerator.drawUniform(0.0,_1_M);
244  for (i=1;i<M;i++) T[i] = T[i-1] + _1_M;
245  T[M] = 1;
246 
247  out_indexes.resize(out_particle_count);
248  i=j=0;
249  while (i < out_particle_count)
250  {
251  if (T[i]<Q[j])
252  out_indexes[i++] = (unsigned int)j;
253  else
254  {
255  j++;
256  if (j>=M) j=M-1;
257  }
258  }
259 
260  }
261  break;
262  default:
263  THROW_EXCEPTION( format("ERROR: Unknown resampling method selected: %i",method) );
264  };
265 
266  MRPT_END
267 }
268 
269 /*---------------------------------------------------------------
270  prediction_and_update
271  ---------------------------------------------------------------*/
272 void CParticleFilterCapable::prediction_and_update(
273  const mrpt::obs::CActionCollection * action,
274  const mrpt::obs::CSensoryFrame * observation,
276 {
277  switch ( PF_options.PF_algorithm )
278  {
279  case CParticleFilter::pfStandardProposal:
280  prediction_and_update_pfStandardProposal( action, observation, PF_options );
281  break;
282  case CParticleFilter::pfAuxiliaryPFStandard:
283  prediction_and_update_pfAuxiliaryPFStandard( action, observation, PF_options );
284  break;
285  case CParticleFilter::pfOptimalProposal:
286  prediction_and_update_pfOptimalProposal( action, observation, PF_options );
287  break;
288  case CParticleFilter::pfAuxiliaryPFOptimal:
289  prediction_and_update_pfAuxiliaryPFOptimal( action, observation, PF_options );
290  break;
291  default:
292  {
293  THROW_EXCEPTION("Invalid particle filter algorithm selection!");
294  } break;
295 
296  }
297 
298 }
299 
300 /*---------------------------------------------------------------
301  prediction_and_update_...
302  ---------------------------------------------------------------*/
303 void CParticleFilterCapable::prediction_and_update_pfStandardProposal(
304  const mrpt::obs::CActionCollection * action,
305  const mrpt::obs::CSensoryFrame * observation,
307 {
308  MRPT_UNUSED_PARAM(action); MRPT_UNUSED_PARAM(observation); MRPT_UNUSED_PARAM(PF_options);
309  THROW_EXCEPTION("Algorithm 'pfStandardProposal' is not implemented in inherited class!");
310 }
311 /*---------------------------------------------------------------
312  prediction_and_update_...
313  ---------------------------------------------------------------*/
314 void CParticleFilterCapable::prediction_and_update_pfAuxiliaryPFStandard(
315  const mrpt::obs::CActionCollection * action,
316  const mrpt::obs::CSensoryFrame * observation,
318 {
319  MRPT_UNUSED_PARAM(action); MRPT_UNUSED_PARAM(observation); MRPT_UNUSED_PARAM(PF_options);
320  THROW_EXCEPTION("Algorithm 'pfAuxiliaryPFStandard' is not implemented in inherited class!");
321 }
322 /*---------------------------------------------------------------
323  prediction_and_update_...
324  ---------------------------------------------------------------*/
325 void CParticleFilterCapable::prediction_and_update_pfOptimalProposal(
326  const mrpt::obs::CActionCollection * action,
327  const mrpt::obs::CSensoryFrame * observation,
329 {
330  MRPT_UNUSED_PARAM(action); MRPT_UNUSED_PARAM(observation); MRPT_UNUSED_PARAM(PF_options);
331  THROW_EXCEPTION("Algorithm 'pfOptimalProposal' is not implemented in inherited class!");
332 }
333 /*---------------------------------------------------------------
334  prediction_and_update_...
335  ---------------------------------------------------------------*/
336 void CParticleFilterCapable::prediction_and_update_pfAuxiliaryPFOptimal(
337  const mrpt::obs::CActionCollection * action,
338  const mrpt::obs::CSensoryFrame * observation,
340 {
341  MRPT_UNUSED_PARAM(action); MRPT_UNUSED_PARAM(observation); MRPT_UNUSED_PARAM(PF_options);
342  THROW_EXCEPTION("Algorithm 'pfAuxiliaryPFOptimal' is not implemented in inherited class!");
343 }
344 
345 /*---------------------------------------------------------------
346  prepareFastDrawSample
347  ---------------------------------------------------------------*/
348 void CParticleFilterCapable::prepareFastDrawSample(
350  TParticleProbabilityEvaluator partEvaluator,
351  const void * action,
352  const void * observation ) const
353 {
354  MRPT_START
355 
356  if (PF_options.adaptiveSampleSize)
357  {
358  // --------------------------------------------------------
359  // CASE: Dynamic number of particles:
360  // -> Use m_fastDrawAuxiliary.CDF, PDF, CDF_indexes
361  // --------------------------------------------------------
362  if (PF_options.resamplingMethod!=CParticleFilter::prMultinomial)
363  THROW_EXCEPTION("resamplingMethod must be 'prMultinomial' for a dynamic number of particles!");
364 
365  size_t i,j=666666,M = particlesCount();
366 
367  MRPT_START
368 
369  // Prepare buffers:
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);
372 
373  // Compute the vector of each particle's probability (usually
374  // it will be simply the weight, but there are other algorithms)
375  m_fastDrawAuxiliary.PDF.resize( M, 0);
376 
377  // This is done to avoid floating point overflow!! (JLBC - SEP 2007)
378  // -------------------------------------------------------------------
379  double SUM = 0;
380  // Save the log likelihoods:
381  for (i=0;i<M;i++) m_fastDrawAuxiliary.PDF[i] = partEvaluator(PF_options, this,i,action,observation);
382  // "Normalize":
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] );
385  ASSERT_(SUM>=0);
387  m_fastDrawAuxiliary.PDF *= 1.0/SUM;
388 
389  // Compute the CDF thresholds:
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;
393 
394  // Compute the CDF and save threshold indexes:
395  double CDF = 0; // Cumulative density func.
396  for (i=0,j=0;i<M && j<PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS;i++)
397  {
398  double CDF_next = CDF + m_fastDrawAuxiliary.PDF[i];
399  if (i==(M-1)) CDF_next = 1.0; // rounds fix...
400  if (CDF_next>1.0) CDF_next = 1.0;
401 
402  while ( m_fastDrawAuxiliary.CDF[j] < CDF_next )
403  m_fastDrawAuxiliary.CDF_indexes[j++] = (unsigned int)i;
404 
405  CDF = CDF_next;
406  }
407 
408  ASSERT_( j == PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS );
409 
410  // Done!
411 #if !defined(_MSC_VER) || (_MSC_VER>1400) // <=VC2005 doesn't work with this!
413  /* Debug: */ \
414  cout << "j=" << j << "\nm_fastDrawAuxiliary.CDF_indexes:" << m_fastDrawAuxiliary.CDF_indexes << endl; \
415  cout << "m_fastDrawAuxiliary.CDF:" << m_fastDrawAuxiliary.CDF << endl; \
416  );
417 #else
418  MRPT_END
419 #endif
420 
421  }
422  else
423  {
424  // ------------------------------------------------------------------------
425  // CASE: Static number of particles:
426  // -> Use m_fastDrawAuxiliary.alreadyDrawnIndexes & alreadyDrawnNextOne
427  // ------------------------------------------------------------------------
428  // Generate the vector with the "probabilities" of each particle being selected:
429  size_t i,M = particlesCount();
430  vector<double> PDF(M,0);
431  for (i=0;i<M;i++)
432  PDF[i] = partEvaluator(PF_options,this,i,action,observation); // Default evaluator: takes current weight.
433 
434  vector<size_t> idxs;
435 
436  // Generate the particle samples:
437  computeResampling( PF_options.resamplingMethod, PDF, idxs );
438 
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);
444 
445  m_fastDrawAuxiliary.alreadyDrawnNextOne = 0;
446  }
447 
448  MRPT_END
449 }
450 
451 /*---------------------------------------------------------------
452  fastDrawSample
453  ---------------------------------------------------------------*/
454 size_t CParticleFilterCapable::fastDrawSample( const bayes::CParticleFilter::TParticleFilterOptions &PF_options ) const
455 {
456  MRPT_START
457 
458  if (PF_options.adaptiveSampleSize)
459  {
460  // --------------------------------------------------------
461  // CASE: Dynamic number of particles:
462  // -> Use m_fastDrawAuxiliary.CDF, PDF, CDF_indexes
463  // --------------------------------------------------------
464  if (PF_options.resamplingMethod!=CParticleFilter::prMultinomial)
465  THROW_EXCEPTION("resamplingMethod must be 'prMultinomial' for a dynamic number of particles!");
466 
467  double draw = randomGenerator.drawUniform(0,0.999999);
468  double CDF_next=-1.;
469  double CDF = -1.;
470 
471  MRPT_START
472 
473  // Use the look-up table to see the starting index we must start looking from:
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];
477 
478  // Find the drawn particle!
479  while ( draw > (CDF_next = CDF+m_fastDrawAuxiliary.PDF[i]) )
480  {
481  CDF = CDF_next;
482  i++;
483  }
484 
485  return i;
486 
488  printf("\n[CParticleFilterCapable::fastDrawSample] DEBUG: draw=%f, CDF=%f CDF_next=%f\n",draw,CDF,CDF_next); \
489  );
490  }
491  else
492  {
493  // --------------------------------------------------------
494  // CASE: Static number of particles:
495  // -> Use m_fastDrawAuxiliary.alreadyDrawnIndexes & alreadyDrawnNextOne
496  // --------------------------------------------------------
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?");
499 
500  return m_fastDrawAuxiliary.alreadyDrawnIndexes[m_fastDrawAuxiliary.alreadyDrawnNextOne++];
501  }
502 
503  MRPT_END
504 }
505 
506 /*---------------------------------------------------------------
507  log2linearWeights
508  ---------------------------------------------------------------*/
509 void CParticleFilterCapable::log2linearWeights(
510  const vector<double> &in_logWeights,
511  vector<double> &out_linWeights )
512 {
513  MRPT_START
514 
515  size_t N = in_logWeights.size();
516 
517  out_linWeights.resize( N );
518 
519  if (!N) return;
520 
521  double sumW = 0;
522  size_t i;
523  for (i=0;i<N;i++)
524  sumW += out_linWeights[i] = exp( in_logWeights[i] );
525 
526  ASSERT_(sumW>0)
527 
528  for (i=0;i<N;i++)
529  out_linWeights[i] /= sumW;
530 
531  MRPT_END
532 }
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.
Definition: zip.h:16
std::vector< uint32_t > vector_uint
Definition: types_simple.h:28
#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.
Scalar * iterator
Definition: eigen_plugins.h:23
STL namespace.
const Scalar * const_iterator
Definition: eigen_plugins.h:24
Declares a class for storing a collection of robot actions.
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
TParticleResamplingAlgorithm
Defines the different resampling algorithms.
TParticleResamplingAlgorithm resamplingMethod
The resampling algorithm to use (default=prMultinomial).
#define MRPT_END
#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.
Definition: format.cpp:21
CONTAINER::Scalar maximum(const CONTAINER &v)
#define MRPT_START
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
const float R
The configuration of a particle filter.
#define ASSERT_(f)
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...



Page generated by Doxygen 1.8.14 for MRPT 1.5.6 Git: 4c65e8431 Tue Apr 24 08:18:17 2018 +0200 at lun oct 28 01:35:26 CET 2019