MRPT  1.9.9
CParticleFilterCapable.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "bayes-precomp.h" // Precompiled headers
11 
13 #include <mrpt/math/ops_vectors.h>
14 #include <mrpt/random.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);
77  vector<double>::const_iterator inIt;
78  vector<double>::iterator outIt;
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>>(linW, Q);
100  Q[M - 1] = 1.1;
101 
102  vector<double> T(M);
103  getRandomGenerator().drawUniformVector(T, 0.0, 0.999999);
104  T.push_back(1.0);
105 
106  // Sort:
107  // --------------------
108  sort(T.begin(), T.end());
109 
110  out_indexes.resize(out_particle_count);
111  i = j = 0;
112 
113  while (i < out_particle_count)
114  {
115  if (T[i] < Q[j])
116  {
117  out_indexes[i++] = (unsigned int)j;
118  }
119  else
120  {
121  j++;
122  if (j >= M) j = M - 1;
123  }
124  }
125  }
126  break; // end of "Select with replacement"
127 
129  {
130  // ==============================================
131  // prResidual
132  // ==============================================
133  // Repetition counts:
134  std::vector<uint32_t> N(M);
135  size_t R = 0; // Remainder or residual count
136  for (i = 0; i < M; i++)
137  {
138  N[i] = int(M * linW[i]);
139  R += N[i];
140  }
141  size_t N_rnd = out_particle_count >= R ? (out_particle_count - R)
142  : 0; // # of particles to be
143  // drawn randomly (the
144  // "residual" part)
145 
146  // Fillout the deterministic part of the resampling:
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;
150 
151  size_t M_fixed = j;
152 
153  // Prepare a multinomial resampling with the residual part,
154  // using the modified weights:
155  // ----------------------------------------------------------
156  if (N_rnd) // If there are "residual" part (should be virtually
157  // always!)
158  {
159  // Compute modified weights:
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]);
164 
165  // perform resampling:
166  vector<double> Q;
167  mrpt::math::cumsum_tmpl<vector<double>, vector<double>>(
168  linW_mod, Q);
169  Q[M - 1] = 1.1;
170 
171  vector<double> T(M);
172  getRandomGenerator().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;
197  {
198  // ==============================================
199  // prStratified
200  // ==============================================
201  vector<double> Q;
202  mrpt::math::cumsum_tmpl<vector<double>, vector<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] =
213  T_offset + getRandomGenerator().drawUniform(0.0, _1_M_eps);
214  T_offset += _1_M;
215  }
216  T[M] = 1;
217 
218  out_indexes.resize(out_particle_count);
219  i = j = 0;
220  while (i < out_particle_count)
221  {
222  if (T[i] < Q[j])
223  out_indexes[i++] = (unsigned int)j;
224  else
225  {
226  j++;
227  if (j >= M) j = M - 1;
228  }
229  }
230  }
231  break;
233  {
234  // ==============================================
235  // prSystematic
236  // ==============================================
237  vector<double> Q;
238  mrpt::math::cumsum_tmpl<vector<double>, vector<double>>(linW, Q);
239  Q[M - 1] = 1.1;
240 
241  // Uniform random vector:
242  vector<double> T(M + 1);
243  double _1_M = 1.0 / M;
244  T[0] = getRandomGenerator().drawUniform(0.0, _1_M);
245  for (i = 1; i < M; i++) T[i] = T[i - 1] + _1_M;
246  T[M] = 1;
247 
248  out_indexes.resize(out_particle_count);
249  i = j = 0;
250  while (i < out_particle_count)
251  {
252  if (T[i] < Q[j])
253  out_indexes[i++] = (unsigned int)j;
254  else
255  {
256  j++;
257  if (j >= M) j = M - 1;
258  }
259  }
260  }
261  break;
262  default:
264  "ERROR: Unknown resampling method selected: %i", method));
265  };
266 
267  MRPT_END
268 }
269 
270 /*---------------------------------------------------------------
271  prediction_and_update
272  ---------------------------------------------------------------*/
274  const mrpt::obs::CActionCollection* action,
275  const mrpt::obs::CSensoryFrame* observation,
277 {
278  switch (PF_options.PF_algorithm)
279  {
281  prediction_and_update_pfStandardProposal(
282  action, observation, PF_options);
283  break;
285  prediction_and_update_pfAuxiliaryPFStandard(
286  action, observation, PF_options);
287  break;
289  prediction_and_update_pfOptimalProposal(
290  action, observation, PF_options);
291  break;
293  prediction_and_update_pfAuxiliaryPFOptimal(
294  action, observation, PF_options);
295  break;
296  default:
297  {
298  THROW_EXCEPTION("Invalid particle filter algorithm selection!");
299  }
300  break;
301  }
302 }
303 
304 /*---------------------------------------------------------------
305  prediction_and_update_...
306  ---------------------------------------------------------------*/
308  const mrpt::obs::CActionCollection* action,
309  const mrpt::obs::CSensoryFrame* observation,
311 {
312  MRPT_UNUSED_PARAM(action);
313  MRPT_UNUSED_PARAM(observation);
314  MRPT_UNUSED_PARAM(PF_options);
316  "Algorithm 'pfStandardProposal' is not implemented in inherited "
317  "class!");
318 }
319 /*---------------------------------------------------------------
320  prediction_and_update_...
321  ---------------------------------------------------------------*/
323  const mrpt::obs::CActionCollection* action,
324  const mrpt::obs::CSensoryFrame* observation,
326 {
327  MRPT_UNUSED_PARAM(action);
328  MRPT_UNUSED_PARAM(observation);
329  MRPT_UNUSED_PARAM(PF_options);
331  "Algorithm 'pfAuxiliaryPFStandard' is not implemented in inherited "
332  "class!");
333 }
334 /*---------------------------------------------------------------
335  prediction_and_update_...
336  ---------------------------------------------------------------*/
338  const mrpt::obs::CActionCollection* action,
339  const mrpt::obs::CSensoryFrame* observation,
341 {
342  MRPT_UNUSED_PARAM(action);
343  MRPT_UNUSED_PARAM(observation);
344  MRPT_UNUSED_PARAM(PF_options);
346  "Algorithm 'pfOptimalProposal' is not implemented in inherited class!");
347 }
348 /*---------------------------------------------------------------
349  prediction_and_update_...
350  ---------------------------------------------------------------*/
352  const mrpt::obs::CActionCollection* action,
353  const mrpt::obs::CSensoryFrame* observation,
355 {
356  MRPT_UNUSED_PARAM(action);
357  MRPT_UNUSED_PARAM(observation);
358  MRPT_UNUSED_PARAM(PF_options);
360  "Algorithm 'pfAuxiliaryPFOptimal' is not implemented in inherited "
361  "class!");
362 }
363 
364 /*---------------------------------------------------------------
365  prepareFastDrawSample
366  ---------------------------------------------------------------*/
369  TParticleProbabilityEvaluator partEvaluator, const void* action,
370  const void* observation) const
371 {
372  MRPT_START
373 
374  if (PF_options.adaptiveSampleSize)
375  {
376  // --------------------------------------------------------
377  // CASE: Dynamic number of particles:
378  // -> Use m_fastDrawAuxiliary.CDF, PDF, CDF_indexes
379  // --------------------------------------------------------
382  "resamplingMethod must be 'prMultinomial' for a dynamic number "
383  "of particles!");
384 
385  size_t i, j = 666666, M = particlesCount();
386 
387  MRPT_START
388 
389  // Prepare buffers:
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);
394 
395  // Compute the vector of each particle's probability (usually
396  // it will be simply the weight, but there are other algorithms)
397  m_fastDrawAuxiliary.PDF.resize(M, 0);
398 
399  // This is done to avoid floating point overflow!! (JLBC - SEP 2007)
400  // -------------------------------------------------------------------
401  double SUM = 0;
402  // Save the log likelihoods:
403  for (i = 0; i < M; i++)
404  m_fastDrawAuxiliary.PDF[i] =
405  partEvaluator(PF_options, this, i, action, observation);
406  // "Normalize":
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]);
410  ASSERT_(SUM >= 0);
412  m_fastDrawAuxiliary.PDF *= 1.0 / SUM;
413 
414  // Compute the CDF thresholds:
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;
419 
420  // Compute the CDF and save threshold indexes:
421  double CDF = 0; // Cumulative density func.
422  for (i = 0, j = 0; i < M && j < PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS;
423  i++)
424  {
425  double CDF_next = CDF + m_fastDrawAuxiliary.PDF[i];
426  if (i == (M - 1)) CDF_next = 1.0; // rounds fix...
427  if (CDF_next > 1.0) CDF_next = 1.0;
428 
429  while (m_fastDrawAuxiliary.CDF[j] < CDF_next)
430  m_fastDrawAuxiliary.CDF_indexes[j++] = (unsigned int)i;
431 
432  CDF = CDF_next;
433  }
434 
435  ASSERT_(j == PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS);
436 
437 // Done!
438 #if !defined(_MSC_VER) || (_MSC_VER > 1400) // <=VC2005 doesn't work with this!
439  MRPT_END_WITH_CLEAN_UP(/* Debug: */
440  cout << "j=" << j
441  << "\nm_fastDrawAuxiliary.CDF_indexes:"
442  << m_fastDrawAuxiliary.CDF_indexes << endl;
443  cout << "m_fastDrawAuxiliary.CDF:"
444  << m_fastDrawAuxiliary.CDF << endl;);
445 #else
446  MRPT_END
447 #endif
448  }
449  else
450  {
451  // ------------------------------------------------------------------------
452  // CASE: Static number of particles:
453  // -> Use m_fastDrawAuxiliary.alreadyDrawnIndexes & alreadyDrawnNextOne
454  // ------------------------------------------------------------------------
455  // Generate the vector with the "probabilities" of each particle being
456  // selected:
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,
462  observation); // Default evaluator: takes current weight.
463 
464  vector<size_t> idxs;
465 
466  // Generate the particle samples:
467  computeResampling(PF_options.resamplingMethod, PDF, idxs);
468 
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);
476 
477  m_fastDrawAuxiliary.alreadyDrawnNextOne = 0;
478  }
479 
480  MRPT_END
481 }
482 
483 /*---------------------------------------------------------------
484  fastDrawSample
485  ---------------------------------------------------------------*/
487  const bayes::CParticleFilter::TParticleFilterOptions& PF_options) const
488 {
489  MRPT_START
490 
491  if (PF_options.adaptiveSampleSize)
492  {
493  // --------------------------------------------------------
494  // CASE: Dynamic number of particles:
495  // -> Use m_fastDrawAuxiliary.CDF, PDF, CDF_indexes
496  // --------------------------------------------------------
499  "resamplingMethod must be 'prMultinomial' for a dynamic number "
500  "of particles!");
501 
502  double draw = getRandomGenerator().drawUniform(0, 0.999999);
503  double CDF_next = -1.;
504  double CDF = -1.;
505 
506  MRPT_START
507 
508  // Use the look-up table to see the starting index we must start looking
509  // from:
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];
514 
515  // Find the drawn particle!
516  while (draw > (CDF_next = CDF + m_fastDrawAuxiliary.PDF[i]))
517  {
518  CDF = CDF_next;
519  i++;
520  }
521 
522  return i;
523 
525  printf(
526  "\n[CParticleFilterCapable::fastDrawSample] DEBUG: draw=%f, "
527  "CDF=%f CDF_next=%f\n",
528  draw, CDF, CDF_next););
529  }
530  else
531  {
532  // --------------------------------------------------------
533  // CASE: Static number of particles:
534  // -> Use m_fastDrawAuxiliary.alreadyDrawnIndexes & alreadyDrawnNextOne
535  // --------------------------------------------------------
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?");
541 
542  return m_fastDrawAuxiliary
543  .alreadyDrawnIndexes[m_fastDrawAuxiliary.alreadyDrawnNextOne++];
544  }
545 
546  MRPT_END
547 }
548 
549 /*---------------------------------------------------------------
550  log2linearWeights
551  ---------------------------------------------------------------*/
553  const vector<double>& in_logWeights, vector<double>& out_linWeights)
554 {
555  MRPT_START
556 
557  size_t N = in_logWeights.size();
558 
559  out_linWeights.resize(N);
560 
561  if (!N) return;
562 
563  double sumW = 0;
564  size_t i;
565  for (i = 0; i < N; i++) sumW += out_linWeights[i] = exp(in_logWeights[i]);
566 
567  ASSERT_(sumW > 0);
568 
569  for (i = 0; i < N; i++) out_linWeights[i] /= sumW;
570 
571  MRPT_END
572 }
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.
#define MRPT_START
Definition: exceptions.h:241
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
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:247
Declares a class for storing a collection of robot actions.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
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:125
CONTAINER::Scalar maximum(const CONTAINER &v)
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:51
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
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
The configuration of a particle filter.
#define MRPT_END
Definition: exceptions.h:245
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 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: ce1a28c9f Fri Aug 23 08:02:09 2019 +0200 at vie ago 23 08:10:11 CEST 2019