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-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 
12 #include <mrpt/bayes/CParticleFilterCapable.h>
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 const unsigned CParticleFilterCapable::PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS =
24  20;
25 
26 /*---------------------------------------------------------------
27  performResampling
28  ---------------------------------------------------------------*/
29 void CParticleFilterCapable::performResampling(
30  const bayes::CParticleFilter::TParticleFilterOptions& PF_options,
31  size_t out_particle_count)
32 {
33  MRPT_START
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  ---------------------------------------------------------------*/
61 void CParticleFilterCapable::computeResampling(
62  CParticleFilter::TParticleResamplingAlgorithm method,
63  const vector<double>& in_logWeights, vector<size_t>& out_indexes,
64  size_t out_particle_count)
65 {
66  MRPT_START
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  {
93  case CParticleFilter::prMultinomial:
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 
129  case CParticleFilter::prResidual:
130  {
131  // ==============================================
132  // prResidual
133  // ==============================================
134  // Repetition counts:
135  vector_uint 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;
197  case CParticleFilter::prStratified:
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;
234  case CParticleFilter::prSystematic:
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:
266  THROW_EXCEPTION(format(
267  "ERROR: Unknown resampling method selected: %i", method));
268  };
269 
270  MRPT_END
271 }
272 
273 /*---------------------------------------------------------------
274  prepareFastDrawSample
275  ---------------------------------------------------------------*/
276 void CParticleFilterCapable::prepareFastDrawSample(
277  const bayes::CParticleFilter::TParticleFilterOptions& PF_options,
278  TParticleProbabilityEvaluator partEvaluator) const
279 {
280  MRPT_START
281 
282  if (PF_options.adaptiveSampleSize)
283  {
284  // --------------------------------------------------------
285  // CASE: Dynamic number of particles:
286  // -> Use m_fastDrawAuxiliary.CDF, PDF, CDF_indexes
287  // --------------------------------------------------------
288  if (PF_options.resamplingMethod != CParticleFilter::prMultinomial)
289  THROW_EXCEPTION(
290  "resamplingMethod must be 'prMultinomial' for a dynamic number "
291  "of particles!");
292 
293  size_t i, j = 666666, M = particlesCount();
294 
295  MRPT_START
296 
297  // Prepare buffers:
298  m_fastDrawAuxiliary.CDF.resize(
299  1 + PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS, 0);
300  m_fastDrawAuxiliary.CDF_indexes.resize(
301  PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS, 0);
302 
303  // Compute the vector of each particle's probability (usually
304  // it will be simply the weight, but there are other algorithms)
305  m_fastDrawAuxiliary.PDF.resize(M, 0);
306 
307  // This is done to avoid floating point overflow!! (JLBC - SEP 2007)
308  // -------------------------------------------------------------------
309  double SUM = 0;
310  // Save the log likelihoods:
311  for (i = 0; i < M; i++) m_fastDrawAuxiliary.PDF[i] = partEvaluator(i);
312  // "Normalize":
313  m_fastDrawAuxiliary.PDF += -math::maximum(m_fastDrawAuxiliary.PDF);
314  for (i = 0; i < M; i++)
315  SUM += m_fastDrawAuxiliary.PDF[i] = exp(m_fastDrawAuxiliary.PDF[i]);
316  ASSERT_(SUM >= 0);
317  MRPT_CHECK_NORMAL_NUMBER(SUM);
318  m_fastDrawAuxiliary.PDF *= 1.0 / SUM;
319 
320  // Compute the CDF thresholds:
321  for (i = 0; i < PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS; i++)
322  m_fastDrawAuxiliary.CDF[i] =
323  ((double)i) / ((double)PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS);
324  m_fastDrawAuxiliary.CDF[PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS] = 1.0;
325 
326  // Compute the CDF and save threshold indexes:
327  double CDF = 0; // Cumulative density func.
328  for (i = 0, j = 0; i < M && j < PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS;
329  i++)
330  {
331  double CDF_next = CDF + m_fastDrawAuxiliary.PDF[i];
332  if (i == (M - 1)) CDF_next = 1.0; // rounds fix...
333  if (CDF_next > 1.0) CDF_next = 1.0;
334 
335  while (m_fastDrawAuxiliary.CDF[j] < CDF_next)
336  m_fastDrawAuxiliary.CDF_indexes[j++] = (unsigned int)i;
337 
338  CDF = CDF_next;
339  }
340 
341  ASSERT_(j == PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS);
342 
343 // Done!
344 #if !defined(_MSC_VER) || (_MSC_VER > 1400) // <=VC2005 doesn't work with this!
345  MRPT_END_WITH_CLEAN_UP(/* Debug: */
346  cout << "j=" << j
347  << "\nm_fastDrawAuxiliary.CDF_indexes:"
348  << m_fastDrawAuxiliary.CDF_indexes << endl;
349  cout << "m_fastDrawAuxiliary.CDF:"
350  << m_fastDrawAuxiliary.CDF << endl;);
351 #else
352  MRPT_END
353 #endif
354  }
355  else
356  {
357  // ------------------------------------------------------------------------
358  // CASE: Static number of particles:
359  // -> Use m_fastDrawAuxiliary.alreadyDrawnIndexes & alreadyDrawnNextOne
360  // ------------------------------------------------------------------------
361  // Generate the vector with the "probabilities" of each particle being
362  // selected:
363  size_t i, M = particlesCount();
364  vector<double> PDF(M, 0);
365  for (i = 0; i < M; i++) PDF[i] = partEvaluator(i);
366 
367  vector<size_t> idxs;
368 
369  // Generate the particle samples:
370  computeResampling(PF_options.resamplingMethod, PDF, idxs);
371 
372  vector<size_t>::iterator it;
373  vector_uint::iterator it2;
374  m_fastDrawAuxiliary.alreadyDrawnIndexes.resize(idxs.size());
375  for (it = idxs.begin(),
376  it2 = m_fastDrawAuxiliary.alreadyDrawnIndexes.begin();
377  it != idxs.end(); ++it, ++it2)
378  *it2 = (unsigned int)(*it);
379 
380  m_fastDrawAuxiliary.alreadyDrawnNextOne = 0;
381  }
382 
383  MRPT_END
384 }
385 
386 /*---------------------------------------------------------------
387  fastDrawSample
388  ---------------------------------------------------------------*/
389 size_t CParticleFilterCapable::fastDrawSample(
390  const bayes::CParticleFilter::TParticleFilterOptions& PF_options) const
391 {
392  MRPT_START
393 
394  if (PF_options.adaptiveSampleSize)
395  {
396  // --------------------------------------------------------
397  // CASE: Dynamic number of particles:
398  // -> Use m_fastDrawAuxiliary.CDF, PDF, CDF_indexes
399  // --------------------------------------------------------
400  if (PF_options.resamplingMethod != CParticleFilter::prMultinomial)
401  THROW_EXCEPTION(
402  "resamplingMethod must be 'prMultinomial' for a dynamic number "
403  "of particles!");
404 
405  double draw = getRandomGenerator().drawUniform(0, 0.999999);
406  double CDF_next = -1.;
407  double CDF = -1.;
408 
409  MRPT_START
410 
411  // Use the look-up table to see the starting index we must start looking
412  // from:
413  size_t j = (size_t)floor(
414  draw * ((double)PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS - 0.05));
415  CDF = m_fastDrawAuxiliary.CDF[j];
416  size_t i = m_fastDrawAuxiliary.CDF_indexes[j];
417 
418  // Find the drawn particle!
419  while (draw > (CDF_next = CDF + m_fastDrawAuxiliary.PDF[i]))
420  {
421  CDF = CDF_next;
422  i++;
423  }
424 
425  return i;
426 
427  MRPT_END_WITH_CLEAN_UP(
428  printf(
429  "\n[CParticleFilterCapable::fastDrawSample] DEBUG: draw=%f, "
430  "CDF=%f CDF_next=%f\n",
431  draw, CDF, CDF_next););
432  }
433  else
434  {
435  // --------------------------------------------------------
436  // CASE: Static number of particles:
437  // -> Use m_fastDrawAuxiliary.alreadyDrawnIndexes & alreadyDrawnNextOne
438  // --------------------------------------------------------
439  if (m_fastDrawAuxiliary.alreadyDrawnNextOne >=
440  m_fastDrawAuxiliary.alreadyDrawnIndexes.size())
441  THROW_EXCEPTION(
442  "Have you called 'fastDrawSample' more times than the sample "
443  "size? Did you forget calling 'prepareFastCall' before?");
444 
445  return m_fastDrawAuxiliary
446  .alreadyDrawnIndexes[m_fastDrawAuxiliary.alreadyDrawnNextOne++];
447  }
448 
449  MRPT_END
450 }
451 
452 /*---------------------------------------------------------------
453  log2linearWeights
454  ---------------------------------------------------------------*/
455 void CParticleFilterCapable::log2linearWeights(
456  const vector<double>& in_logWeights, vector<double>& out_linWeights)
457 {
458  MRPT_START
459 
460  size_t N = in_logWeights.size();
461 
462  out_linWeights.resize(N);
463 
464  if (!N) return;
465 
466  double sumW = 0;
467  size_t i;
468  for (i = 0; i < N; i++) sumW += out_linWeights[i] = exp(in_logWeights[i]);
469 
470  ASSERT_(sumW > 0)
471 
472  for (i = 0; i < N; i++) out_linWeights[i] /= sumW;
473 
474  MRPT_END
475 }
mrpt::random
A namespace of pseudo-random numbers genrators of diferent distributions.
Definition: RandomGenerators.h:25
mrpt::random::CRandomGenerator::drawUniform
double drawUniform(const double Min, const double Max)
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm, scaled to the selected range.
Definition: RandomGenerators.h:103
mrpt::utils
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: CParticleFilter.h:223
mrpt::vector_uint
std::vector< uint32_t > vector_uint
Definition: types_simple.h:29
MRPT_END_WITH_CLEAN_UP
#define MRPT_END_WITH_CLEAN_UP(stuff)
Definition: CColouredPointsMap.h:432
MRPT_CHECK_NORMAL_NUMBER
#define MRPT_CHECK_NORMAL_NUMBER(val)
Definition: CColouredPointsMap.h:313
mrpt::bayes
The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorith...
Definition: CParticleFilter.h:28
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: CColouredPointsMap.h:112
ops_vectors.h
iterator
Scalar * iterator
Definition: eigen_plugins.h:26
std
STL namespace.
const_iterator
const Scalar * const_iterator
Definition: eigen_plugins.h:27
mrpt::bayes::CParticleFilterCapable::TParticleProbabilityEvaluator
std::function< double(size_t index)> TParticleProbabilityEvaluator
A callback function type for evaluating the probability of m_particles of being selected, used in "fastDrawSample".
Definition: CParticleFilterCapable.h:60
mrpt::math
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
mrpt::bayes::CParticleFilter::TParticleResamplingAlgorithm
TParticleResamplingAlgorithm
Defines the different resampling algorithms.
Definition: CParticleFilter.h:92
mrpt::bayes::CParticleFilter::TParticleFilterOptions::resamplingMethod
TParticleResamplingAlgorithm resamplingMethod
The resampling algorithm to use (default=prMultinomial).
Definition: CParticleFilter.h:141
MRPT_END
#define MRPT_END
Definition: CColouredPointsMap.h:430
mrpt::format
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:19
mrpt::math::maximum
CONTAINER::Scalar maximum(const CONTAINER &v)
Definition: ops_containers.h:139
MRPT_START
#define MRPT_START
Definition: CColouredPointsMap.h:426
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:17
CParticleFilterCapable.h
R
const float R
Definition: CKinematicChain.cpp:149
mrpt::bayes::CParticleFilter::TParticleFilterOptions
The configuration of a particle filter.
Definition: CParticleFilter.h:102
ASSERT_
#define ASSERT_(f)
Definition: CColouredPointsMap.h:310
mrpt::random::getRandomGenerator
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:25
mrpt::random::CRandomGenerator::drawUniformVector
void drawUniformVector(VEC &v, const double unif_min=0, const double unif_max=1)
Fills the given vector with independent, uniformly distributed samples.
Definition: RandomGenerators.h:129
mrpt::bayes::CParticleFilter::TParticleFilterOptions::adaptiveSampleSize
bool adaptiveSampleSize
A flag that indicates whether the CParticleFilterCapable object should perform adative sample size (d...
Definition: CParticleFilter.h:118


Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019