Main MRPT website > C++ reference for MRPT 1.9.9
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-2018, 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 "bayes-precomp.h" // Precompiled headers
11 
13 #include <mrpt/random.h>
14 #include <mrpt/math/ops_vectors.h>
15 #include <iostream>
16 
17 using namespace mrpt;
18 using namespace mrpt::bayes;
19 using namespace mrpt::random;
20 using namespace mrpt::math;
21 using namespace std;
22 
24  20;
25 
26 /*---------------------------------------------------------------
27  performResampling
28  ---------------------------------------------------------------*/
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++) log_ws[i] = getW(i);
43 
44  // Compute the surviving indexes:
45  computeResampling(
46  PF_options.resamplingMethod, log_ws, indxs, out_particle_count);
47 
48  // And perform the particle replacement:
49  performSubstitution(indxs);
50 
51  // Finally, equal weights:
52  for (size_t i = 0; i < out_particle_count; i++)
53  setW(i, 0 /* Logarithmic weight */);
54 
55  MRPT_END
56 }
57 
58 /*---------------------------------------------------------------
59  resample
60  ---------------------------------------------------------------*/
63  const vector<double>& in_logWeights, vector<size_t>& out_indexes,
64  size_t out_particle_count)
65 {
67 
68  // Compute the normalized linear weights:
69  // The array "linW" will be the input to the actual
70  // resampling algorithms.
71  size_t i, j, M = in_logWeights.size();
72  ASSERT_(M > 0);
73 
74  if (!out_particle_count) out_particle_count = M;
75 
76  vector<double> linW(M, 0);
79  double linW_SUM = 0;
80 
81  // This is to avoid float point range problems:
82  double max_log_w = math::maximum(in_logWeights);
83  for (i = 0, inIt = in_logWeights.begin(), outIt = linW.begin(); i < M;
84  i++, inIt++, outIt++)
85  linW_SUM += ((*outIt) = exp((*inIt) - max_log_w));
86 
87  // Normalize weights:
88  ASSERT_(linW_SUM > 0);
89  linW *= 1.0 / linW_SUM;
90 
91  switch (method)
92  {
94  {
95  // ==============================================
96  // Select with replacement
97  // ==============================================
98  vector<double> Q;
99  mrpt::math::cumsum_tmpl<vector<double>, vector<double>, double>(
100  linW, Q);
101  Q[M - 1] = 1.1;
102 
103  vector<double> T(M);
104  getRandomGenerator().drawUniformVector(T, 0.0, 0.999999);
105  T.push_back(1.0);
106 
107  // Sort:
108  // --------------------
109  sort(T.begin(), T.end());
110 
111  out_indexes.resize(out_particle_count);
112  i = j = 0;
113 
114  while (i < out_particle_count)
115  {
116  if (T[i] < Q[j])
117  {
118  out_indexes[i++] = (unsigned int)j;
119  }
120  else
121  {
122  j++;
123  if (j >= M) j = M - 1;
124  }
125  }
126  }
127  break; // end of "Select with replacement"
128 
130  {
131  // ==============================================
132  // prResidual
133  // ==============================================
134  // Repetition counts:
135  std::vector<uint32_t> N(M);
136  size_t R = 0; // Remainder or residual count
137  for (i = 0; i < M; i++)
138  {
139  N[i] = int(M * linW[i]);
140  R += N[i];
141  }
142  size_t N_rnd = out_particle_count >= R ? (out_particle_count - R)
143  : 0; // # of particles to be
144  // drawn randomly (the
145  // "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++) out_indexes[j++] = i;
151 
152  size_t M_fixed = j;
153 
154  // Prepare a multinomial resampling with the residual part,
155  // using the modified weights:
156  // ----------------------------------------------------------
157  if (N_rnd) // If there are "residual" part (should be virtually
158  // 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>(
169  linW_mod, Q);
170  Q[M - 1] = 1.1;
171 
172  vector<double> T(M);
173  getRandomGenerator().drawUniformVector(T, 0.0, 0.999999);
174  T.push_back(1.0);
175 
176  // Sort:
177  sort(T.begin(), T.end());
178 
179  i = 0;
180  j = 0;
181 
182  while (i < N_rnd)
183  {
184  if (T[i] < Q[j])
185  {
186  out_indexes[M_fixed + i++] = (unsigned int)j;
187  }
188  else
189  {
190  j++;
191  if (j >= M) j = M - 1;
192  }
193  }
194  } // end if N_rnd!=0
195  }
196  break;
198  {
199  // ==============================================
200  // prStratified
201  // ==============================================
202  vector<double> Q;
203  mrpt::math::cumsum_tmpl<vector<double>, vector<double>, double>(
204  linW, Q);
205  Q[M - 1] = 1.1;
206 
207  // Stratified-uniform random vector:
208  vector<double> T(M + 1);
209  const double _1_M = 1.0 / M;
210  const double _1_M_eps = _1_M - 0.000001;
211  double T_offset = 0;
212  for (i = 0; i < M; i++)
213  {
214  T[i] =
215  T_offset + getRandomGenerator().drawUniform(0.0, _1_M_eps);
216  T_offset += _1_M;
217  }
218  T[M] = 1;
219 
220  out_indexes.resize(out_particle_count);
221  i = j = 0;
222  while (i < out_particle_count)
223  {
224  if (T[i] < Q[j])
225  out_indexes[i++] = (unsigned int)j;
226  else
227  {
228  j++;
229  if (j >= M) j = M - 1;
230  }
231  }
232  }
233  break;
235  {
236  // ==============================================
237  // prSystematic
238  // ==============================================
239  vector<double> Q;
240  mrpt::math::cumsum_tmpl<vector<double>, vector<double>, double>(
241  linW, Q);
242  Q[M - 1] = 1.1;
243 
244  // Uniform random vector:
245  vector<double> T(M + 1);
246  double _1_M = 1.0 / M;
247  T[0] = getRandomGenerator().drawUniform(0.0, _1_M);
248  for (i = 1; i < M; i++) T[i] = T[i - 1] + _1_M;
249  T[M] = 1;
250 
251  out_indexes.resize(out_particle_count);
252  i = j = 0;
253  while (i < out_particle_count)
254  {
255  if (T[i] < Q[j])
256  out_indexes[i++] = (unsigned int)j;
257  else
258  {
259  j++;
260  if (j >= M) j = M - 1;
261  }
262  }
263  }
264  break;
265  default:
267  format(
268  "ERROR: Unknown resampling method selected: %i", method));
269  };
270 
271  MRPT_END
272 }
273 
274 /*---------------------------------------------------------------
275  prediction_and_update
276  ---------------------------------------------------------------*/
278  const mrpt::obs::CActionCollection* action,
279  const mrpt::obs::CSensoryFrame* observation,
281 {
282  switch (PF_options.PF_algorithm)
283  {
285  prediction_and_update_pfStandardProposal(
286  action, observation, PF_options);
287  break;
289  prediction_and_update_pfAuxiliaryPFStandard(
290  action, observation, PF_options);
291  break;
293  prediction_and_update_pfOptimalProposal(
294  action, observation, PF_options);
295  break;
297  prediction_and_update_pfAuxiliaryPFOptimal(
298  action, observation, PF_options);
299  break;
300  default:
301  {
302  THROW_EXCEPTION("Invalid particle filter algorithm selection!");
303  }
304  break;
305  }
306 }
307 
308 /*---------------------------------------------------------------
309  prediction_and_update_...
310  ---------------------------------------------------------------*/
312  const mrpt::obs::CActionCollection* action,
313  const mrpt::obs::CSensoryFrame* observation,
315 {
316  MRPT_UNUSED_PARAM(action);
317  MRPT_UNUSED_PARAM(observation);
318  MRPT_UNUSED_PARAM(PF_options);
320  "Algorithm 'pfStandardProposal' is not implemented in inherited "
321  "class!");
322 }
323 /*---------------------------------------------------------------
324  prediction_and_update_...
325  ---------------------------------------------------------------*/
327  const mrpt::obs::CActionCollection* action,
328  const mrpt::obs::CSensoryFrame* observation,
330 {
331  MRPT_UNUSED_PARAM(action);
332  MRPT_UNUSED_PARAM(observation);
333  MRPT_UNUSED_PARAM(PF_options);
335  "Algorithm 'pfAuxiliaryPFStandard' is not implemented in inherited "
336  "class!");
337 }
338 /*---------------------------------------------------------------
339  prediction_and_update_...
340  ---------------------------------------------------------------*/
342  const mrpt::obs::CActionCollection* action,
343  const mrpt::obs::CSensoryFrame* observation,
345 {
346  MRPT_UNUSED_PARAM(action);
347  MRPT_UNUSED_PARAM(observation);
348  MRPT_UNUSED_PARAM(PF_options);
350  "Algorithm 'pfOptimalProposal' is not implemented in inherited class!");
351 }
352 /*---------------------------------------------------------------
353  prediction_and_update_...
354  ---------------------------------------------------------------*/
356  const mrpt::obs::CActionCollection* action,
357  const mrpt::obs::CSensoryFrame* observation,
359 {
360  MRPT_UNUSED_PARAM(action);
361  MRPT_UNUSED_PARAM(observation);
362  MRPT_UNUSED_PARAM(PF_options);
364  "Algorithm 'pfAuxiliaryPFOptimal' is not implemented in inherited "
365  "class!");
366 }
367 
368 /*---------------------------------------------------------------
369  prepareFastDrawSample
370  ---------------------------------------------------------------*/
373  TParticleProbabilityEvaluator partEvaluator, const void* action,
374  const void* observation) const
375 {
376  MRPT_START
377 
378  if (PF_options.adaptiveSampleSize)
379  {
380  // --------------------------------------------------------
381  // CASE: Dynamic number of particles:
382  // -> Use m_fastDrawAuxiliary.CDF, PDF, CDF_indexes
383  // --------------------------------------------------------
386  "resamplingMethod must be 'prMultinomial' for a dynamic number "
387  "of particles!");
388 
389  size_t i, j = 666666, M = particlesCount();
390 
391  MRPT_START
392 
393  // Prepare buffers:
394  m_fastDrawAuxiliary.CDF.resize(
395  1 + PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS, 0);
396  m_fastDrawAuxiliary.CDF_indexes.resize(
397  PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS, 0);
398 
399  // Compute the vector of each particle's probability (usually
400  // it will be simply the weight, but there are other algorithms)
401  m_fastDrawAuxiliary.PDF.resize(M, 0);
402 
403  // This is done to avoid floating point overflow!! (JLBC - SEP 2007)
404  // -------------------------------------------------------------------
405  double SUM = 0;
406  // Save the log likelihoods:
407  for (i = 0; i < M; i++)
408  m_fastDrawAuxiliary.PDF[i] =
409  partEvaluator(PF_options, this, i, action, observation);
410  // "Normalize":
411  m_fastDrawAuxiliary.PDF += -math::maximum(m_fastDrawAuxiliary.PDF);
412  for (i = 0; i < M; i++)
413  SUM += m_fastDrawAuxiliary.PDF[i] = exp(m_fastDrawAuxiliary.PDF[i]);
414  ASSERT_(SUM >= 0);
416  m_fastDrawAuxiliary.PDF *= 1.0 / SUM;
417 
418  // Compute the CDF thresholds:
419  for (i = 0; i < PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS; i++)
420  m_fastDrawAuxiliary.CDF[i] =
421  ((double)i) / ((double)PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS);
422  m_fastDrawAuxiliary.CDF[PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS] = 1.0;
423 
424  // Compute the CDF and save threshold indexes:
425  double CDF = 0; // Cumulative density func.
426  for (i = 0, j = 0; i < M && j < PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS;
427  i++)
428  {
429  double CDF_next = CDF + m_fastDrawAuxiliary.PDF[i];
430  if (i == (M - 1)) CDF_next = 1.0; // rounds fix...
431  if (CDF_next > 1.0) CDF_next = 1.0;
432 
433  while (m_fastDrawAuxiliary.CDF[j] < CDF_next)
434  m_fastDrawAuxiliary.CDF_indexes[j++] = (unsigned int)i;
435 
436  CDF = CDF_next;
437  }
438 
439  ASSERT_(j == PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS);
440 
441 // Done!
442 #if !defined(_MSC_VER) || (_MSC_VER > 1400) // <=VC2005 doesn't work with this!
444  /* Debug: */ \
445  cout << "j=" << j << "\nm_fastDrawAuxiliary.CDF_indexes:" << m_fastDrawAuxiliary.CDF_indexes << endl; \
446  cout << "m_fastDrawAuxiliary.CDF:" << m_fastDrawAuxiliary.CDF << endl; \
447  );
448 #else
449  MRPT_END
450 #endif
451  }
452  else
453  {
454  // ------------------------------------------------------------------------
455  // CASE: Static number of particles:
456  // -> Use m_fastDrawAuxiliary.alreadyDrawnIndexes & alreadyDrawnNextOne
457  // ------------------------------------------------------------------------
458  // Generate the vector with the "probabilities" of each particle being
459  // selected:
460  size_t i, M = particlesCount();
461  vector<double> PDF(M, 0);
462  for (i = 0; i < M; i++)
463  PDF[i] = partEvaluator(
464  PF_options, this, i, action,
465  observation); // Default evaluator: takes current weight.
466 
467  vector<size_t> idxs;
468 
469  // Generate the particle samples:
470  computeResampling(PF_options.resamplingMethod, PDF, idxs);
471 
474  m_fastDrawAuxiliary.alreadyDrawnIndexes.resize(idxs.size());
475  for (it = idxs.begin(),
476  it2 = m_fastDrawAuxiliary.alreadyDrawnIndexes.begin();
477  it != idxs.end(); ++it, ++it2)
478  *it2 = (unsigned int)(*it);
479 
480  m_fastDrawAuxiliary.alreadyDrawnNextOne = 0;
481  }
482 
483  MRPT_END
484 }
485 
486 /*---------------------------------------------------------------
487  fastDrawSample
488  ---------------------------------------------------------------*/
490  const bayes::CParticleFilter::TParticleFilterOptions& PF_options) const
491 {
492  MRPT_START
493 
494  if (PF_options.adaptiveSampleSize)
495  {
496  // --------------------------------------------------------
497  // CASE: Dynamic number of particles:
498  // -> Use m_fastDrawAuxiliary.CDF, PDF, CDF_indexes
499  // --------------------------------------------------------
502  "resamplingMethod must be 'prMultinomial' for a dynamic number "
503  "of particles!");
504 
505  double draw = getRandomGenerator().drawUniform(0, 0.999999);
506  double CDF_next = -1.;
507  double CDF = -1.;
508 
509  MRPT_START
510 
511  // Use the look-up table to see the starting index we must start looking
512  // from:
513  size_t j = (size_t)floor(
514  draw * ((double)PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS - 0.05));
515  CDF = m_fastDrawAuxiliary.CDF[j];
516  size_t i = m_fastDrawAuxiliary.CDF_indexes[j];
517 
518  // Find the drawn particle!
519  while (draw > (CDF_next = CDF + m_fastDrawAuxiliary.PDF[i]))
520  {
521  CDF = CDF_next;
522  i++;
523  }
524 
525  return i;
526 
528  printf(
529  "\n[CParticleFilterCapable::fastDrawSample] DEBUG: draw=%f, "
530  "CDF=%f CDF_next=%f\n",
531  draw, CDF, CDF_next););
532  }
533  else
534  {
535  // --------------------------------------------------------
536  // CASE: Static number of particles:
537  // -> Use m_fastDrawAuxiliary.alreadyDrawnIndexes & alreadyDrawnNextOne
538  // --------------------------------------------------------
539  if (m_fastDrawAuxiliary.alreadyDrawnNextOne >=
540  m_fastDrawAuxiliary.alreadyDrawnIndexes.size())
542  "Have you called 'fastDrawSample' more times than the sample "
543  "size? Did you forget calling 'prepareFastCall' before?");
544 
545  return m_fastDrawAuxiliary
546  .alreadyDrawnIndexes[m_fastDrawAuxiliary.alreadyDrawnNextOne++];
547  }
548 
549  MRPT_END
550 }
551 
552 /*---------------------------------------------------------------
553  log2linearWeights
554  ---------------------------------------------------------------*/
556  const vector<double>& in_logWeights, vector<double>& out_linWeights)
557 {
558  MRPT_START
559 
560  size_t N = in_logWeights.size();
561 
562  out_linWeights.resize(N);
563 
564  if (!N) return;
565 
566  double sumW = 0;
567  size_t i;
568  for (i = 0; i < N; i++) sumW += out_linWeights[i] = exp(in_logWeights[i]);
569 
570  ASSERT_(sumW > 0);
571 
572  for (i = 0; i < N; i++) out_linWeights[i] /= sumW;
573 
574  MRPT_END
575 }
A namespace of pseudo-random numbers generators 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.
Scalar * iterator
Definition: eigen_plugins.h:26
#define MRPT_START
Definition: exceptions.h:262
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
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".
STL namespace.
#define MRPT_END_WITH_CLEAN_UP(stuff)
Definition: exceptions.h:268
Declares a class for storing a collection of robot actions.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
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...
Definition: exceptions.h:118
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:54
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
CONTAINER::Scalar maximum(const CONTAINER &v)
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.
const float R
The configuration of a particle filter.
#define MRPT_END
Definition: exceptions.h:266
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...
const Scalar * const_iterator
Definition: eigen_plugins.h:27
TParticleFilterAlgorithm PF_algorithm
The PF algorithm to use (default=pfStandardProposal) See TParticleFilterAlgorithm for the posibilitie...
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.
Definition: common.h:186



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019