Main MRPT website > C++ reference for MRPT 1.9.9
PF_implementations.h
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 #ifndef PF_implementations_H
11 #define PF_implementations_H
12 
13 #include <mrpt/bayes/CParticleFilterCapable.h>
14 #include <mrpt/bayes/CParticleFilterData.h>
15 #include <mrpt/random.h>
16 #include <mrpt/obs/CActionCollection.h>
17 #include <mrpt/obs/CActionRobotMovement3D.h>
18 #include <mrpt/obs/CActionRobotMovement2D.h>
19 #include <mrpt/slam/TKLDParams.h>
20 
21 #include <mrpt/math/distributions.h> // chi2inv
22 #include <mrpt/math/data_utils.h> // averageLogLikelihood()
23 
24 #include <mrpt/slam/PF_implementations_data.h>
25 
26 #include <type_traits>
27 
28 /** \file PF_implementations.h
29  * This file contains the implementations of the template members declared in
30  * mrpt::slam::PF_implementation
31  */
32 
33 namespace mrpt
34 {
35 namespace slam
36 {
37 /** A generic implementation of the PF method "pfStandardProposal" (standard
38  * proposal distribution, that is, a simple SIS particle filter),
39  * common to both localization and mapping.
40  *
41  * - BINTYPE: TPoseBin or whatever to discretize the sample space for
42  * KLD-sampling.
43  */
44 class StandardProposal
45 {
46  public:
47  static constexpr bool DoesResampling = true;
48 
49  template <class PARTICLE_TYPE, class MYSELF, class BINTYPE>
50  static void PF_SLAM_implementation(
51  const mrpt::obs::CActionCollection* actions,
52  const mrpt::obs::CSensoryFrame* sf,
53  const mrpt::bayes::CParticleFilter::TParticleFilterOptions& PF_options,
54  const TKLDParams& KLD_options, MYSELF& me)
55  {
56  MRPT_START
57  typedef std::set<BINTYPE, typename BINTYPE::lt_operator>
58  TSetStateSpaceBins;
59 
60  // In this method we don't need the
61  // "PF_SLAM_implementation_gatherActionsCheckBothActObs" machinery,
62  // since prediction & update are two independent stages well separated
63  // for this algorithm.
64 
65  // --------------------------------------------------------------------------------------
66  // Prediction: Simply draw samples from the motion model
67  // --------------------------------------------------------------------------------------
68  if (actions)
69  {
70  // Find a robot movement estimation:
71  mrpt::poses::CPose3D motionModelMeanIncr;
72  {
73  mrpt::obs::CActionRobotMovement2D::Ptr robotMovement2D =
74  actions->getBestMovementEstimation();
75  // If there is no 2D action, look for a 3D action:
76  if (robotMovement2D)
77  {
78  ASSERT_(robotMovement2D->poseChange);
79  me.m_movementDrawer.setPosePDF(
80  robotMovement2D->poseChange.get_ptr());
81  motionModelMeanIncr = mrpt::poses::CPose3D(
82  robotMovement2D->poseChange->getMeanVal());
83  }
84  else
85  {
86  mrpt::obs::CActionRobotMovement3D::Ptr robotMovement3D =
87  actions->getActionByClass<
88  mrpt::obs::CActionRobotMovement3D>();
89  if (robotMovement3D)
90  {
91  me.m_movementDrawer.setPosePDF(
92  robotMovement3D->poseChange);
93  robotMovement3D->poseChange.getMean(
94  motionModelMeanIncr);
95  }
96  else
97  {
98  THROW_EXCEPTION(
99  "Action list does not contain any "
100  "CActionRobotMovement2D or CActionRobotMovement3D "
101  "object!");
102  }
103  }
104  }
105 
106  // Update particle poses:
107  if (!PF_options.adaptiveSampleSize)
108  {
109  const size_t M = me.m_poseParticles.m_particles.size();
110  // -------------------------------------------------------------
111  // FIXED SAMPLE SIZE
112  // -------------------------------------------------------------
113  mrpt::poses::CPose3D incrPose;
114  for (size_t i = 0; i < M; i++)
115  {
116  // Generate gaussian-distributed 2D-pose increments
117  // according to mean-cov:
118  me.m_movementDrawer.drawSample(incrPose);
119  bool pose_is_valid;
120  const mrpt::poses::CPose3D finalPose =
121  mrpt::poses::CPose3D(me.getLastPose(i, pose_is_valid)) +
122  incrPose;
123 
124  // Update the particle with the new pose: this part is
125  // caller-dependant and must be implemented there:
126  me.PF_SLAM_implementation_custom_update_particle_with_new_pose(
127  me.m_poseParticles.m_particles[i].d.get(),
128  mrpt::math::TPose3D(finalPose));
129  }
130  }
131  else
132  {
133  // -------------------------------------------------------------
134  // ADAPTIVE SAMPLE SIZE
135  // Implementation of Dieter Fox's KLD algorithm
136  // 31-Oct-2006 (JLBC): First version
137  // 19-Jan-2009 (JLBC): Rewriten within a generic template
138  // -------------------------------------------------------------
139  TSetStateSpaceBins stateSpaceBins;
140 
141  size_t Nx = KLD_options.KLD_minSampleSize;
142  const double delta_1 = 1.0 - KLD_options.KLD_delta;
143  const double epsilon_1 = 0.5 / KLD_options.KLD_epsilon;
144 
145  // Prepare data for executing "fastDrawSample"
146  me.m_poseParticles.prepareFastDrawSample(PF_options);
147 
148  // The new particle set:
149  std::vector<mrpt::math::TPose3D> newParticles;
150  std::vector<double> newParticlesWeight;
151  std::vector<size_t> newParticlesDerivedFromIdx;
152 
153  mrpt::poses::CPose3D increment_i;
154  size_t N = 1;
155 
156  do // THE MAIN DRAW SAMPLING LOOP
157  {
158  // Draw a robot movement increment:
159  me.m_movementDrawer.drawSample(increment_i);
160 
161  // generate the new particle:
162  const size_t drawn_idx =
163  me.m_poseParticles.fastDrawSample(PF_options);
164 
165  bool pose_is_valid;
166  const mrpt::poses::CPose3D newPose =
167  mrpt::poses::CPose3D(
168  me.getLastPose(drawn_idx, pose_is_valid)) +
169  increment_i;
170  const mrpt::math::TPose3D newPose_s = newPose;
171 
172  // Add to the new particles list:
173  newParticles.push_back(newPose_s);
174  newParticlesWeight.push_back(0);
175  newParticlesDerivedFromIdx.push_back(drawn_idx);
176 
177  // Now, look if the particle falls in a new bin or not:
178  // --------------------------------------------------------
179  BINTYPE p;
180  KLF_loadBinFromParticle<PARTICLE_TYPE, BINTYPE>(
181  p, KLD_options,
182  me.m_poseParticles.m_particles[drawn_idx].d.get(),
183  &newPose_s);
184 
185  if (stateSpaceBins.find(p) == stateSpaceBins.end())
186  {
187  // It falls into a new bin:
188  // Add to the stateSpaceBins:
189  stateSpaceBins.insert(p);
190 
191  // K = K + 1
192  size_t K = stateSpaceBins.size();
193  if (K > 1) //&& newParticles.size() >
194  // options.KLD_minSampleSize )
195  {
196  // Update the number of m_particles!!
197  Nx = round(
198  epsilon_1 * math::chi2inv(delta_1, K - 1));
199  // printf("k=%u \tn=%u \tNx:%u\n", k,
200  // newParticles.size(), Nx);
201  }
202  }
203  N = newParticles.size();
204  } while (
205  N < std::max(Nx, (size_t)KLD_options.KLD_minSampleSize) &&
206  N < KLD_options.KLD_maxSampleSize);
207 
208  // ---------------------------------------------------------------------------------
209  // Substitute old by new particle set:
210  // Old are in "m_particles"
211  // New are in "newParticles",
212  // "newParticlesWeight","newParticlesDerivedFromIdx"
213  // ---------------------------------------------------------------------------------
214  me.PF_SLAM_implementation_replaceByNewParticleSet(
215  me.m_poseParticles.m_particles, newParticles,
216  newParticlesWeight, newParticlesDerivedFromIdx);
217 
218  } // end adaptive sample size
219  }
220 
221  if (sf)
222  {
223  const size_t M = me.m_poseParticles.m_particles.size();
224  // UPDATE STAGE
225  // ----------------------------------------------------------------------
226  // Compute all the likelihood values & update particles weight:
227  for (size_t i = 0; i < M; i++)
228  {
229  bool pose_is_valid;
230  const mrpt::math::TPose3D partPose = me.getLastPose(
231  i, pose_is_valid); // Take the particle data:
232  mrpt::poses::CPose3D partPose2 = mrpt::poses::CPose3D(partPose);
233  const double obs_log_likelihood =
234  me.PF_SLAM_computeObservationLikelihoodForParticle(
235  PF_options, i, *sf, partPose2);
236  me.m_poseParticles.m_particles[i].log_w +=
237  obs_log_likelihood * PF_options.powFactor;
238  } // for each particle "i"
239 
240  // Normalization of weights is done outside of this method
241  // automatically.
242  }
243 
244  MRPT_END
245  } // end of PF_SLAM_implementation_pfStandardProposal
246 };
247 
248 /*---------------------------------------------------------------
249  PF_SLAM_particlesEvaluator_AuxPFOptimal
250  ---------------------------------------------------------------*/
251 class AuxPFOptimalEvaluator
252 {
253  public:
254  template <class BINTYPE, class MYSELF>
255  static double evaluate(
256  const mrpt::bayes::CParticleFilter::TParticleFilterOptions& PF_options,
257  unsigned long index, const mrpt::poses::CPose3D* action,
258  const mrpt::obs::CSensoryFrame* observation, const MYSELF& me)
259  {
260  MRPT_UNUSED_PARAM(action);
261  MRPT_START
262 
263  // Compute the quantity:
264  // w[i]*p(zt|z^{t-1},x^{[i],t-1})
265  // As the Monte-Carlo approximation of the integral over all posible
266  // $x_t$.
267  // --------------------------------------------
268  double indivLik, maxLik = -1e300;
269  mrpt::poses::CPose3D maxLikDraw;
270  size_t N = PF_options.pfAuxFilterOptimal_MaximumSearchSamples;
271  ASSERT_(N > 1)
272 
273  bool pose_is_valid;
274  const mrpt::poses::CPose3D oldPose =
275  mrpt::poses::CPose3D(me.getLastPose(index, pose_is_valid));
276  mrpt::math::CVectorDouble vectLiks(
277  N, 0); // The vector with the individual log-likelihoods.
278  mrpt::poses::CPose3D drawnSample;
279  for (size_t q = 0; q < N; q++)
280  {
281  me.m_movementDrawer.drawSample(drawnSample);
282  mrpt::poses::CPose3D x_predict = oldPose + drawnSample;
283 
284  // Estimate the mean...
285  indivLik = me.PF_SLAM_computeObservationLikelihoodForParticle(
286  PF_options, index, *observation, x_predict);
287 
288  MRPT_CHECK_NORMAL_NUMBER(indivLik);
289  vectLiks[q] = indivLik;
290  if (indivLik > maxLik)
291  { // Keep the maximum value:
292  maxLikDraw = drawnSample;
293  maxLik = indivLik;
294  }
295  }
296 
297  // This is done to avoid floating point overflow!!
298  // average_lik = \sum(e^liks) * e^maxLik / N
299  // log( average_lik ) = log( \sum(e^liks) ) + maxLik - log( N )
300  double avrgLogLik = math::averageLogLikelihood(vectLiks);
301 
302  // Save into the object:
303  me.m_pfAuxiliaryPFOptimal_estimatedProb[index] =
304  avrgLogLik; // log( accum / N );
305  me.m_pfAuxiliaryPFOptimal_maxLikelihood[index] = maxLik;
306 
307  if (PF_options.pfAuxFilterOptimal_MLE)
308  me.m_pfAuxiliaryPFOptimal_maxLikDrawnMovement[index] = maxLikDraw;
309 
310  // and compute the resulting probability of this particle:
311  // ------------------------------------------------------------
312  return me.m_poseParticles.m_particles[index].log_w +
313  me.m_pfAuxiliaryPFOptimal_estimatedProb[index];
314 
315  MRPT_END
316  } // end of PF_SLAM_particlesEvaluator_AuxPFOptimal
317 };
318 
319 /** Compute w[i]*p(z_t | mu_t^i), with mu_t^i being
320  * the mean of the new robot pose
321  *
322  * \param action MUST be a "const mrpt::poses::CPose3D*"
323  * \param observation MUST be a "const CSensoryFrame*"
324  */
325 class AuxPFStandardEvaluator
326 {
327  public:
328  template <class BINTYPE, class MYSELF>
329  static double evaluate(
330  const mrpt::bayes::CParticleFilter::TParticleFilterOptions& PF_options,
331  unsigned long index, const mrpt::poses::CPose3D* action,
332  const mrpt::obs::CSensoryFrame* observation, const MYSELF& me)
333  {
334  MRPT_START
335 
336  // Take the previous particle weight:
337  const double cur_logweight =
338  me.m_poseParticles.m_particles[index].log_w;
339  bool pose_is_valid;
340  const mrpt::poses::CPose3D oldPose =
341  mrpt::poses::CPose3D(me.getLastPose(index, pose_is_valid));
342 
343  if (!PF_options.pfAuxFilterStandard_FirstStageWeightsMonteCarlo)
344  {
345  // Just use the mean:
346  // , take the mean of the posterior density:
347  mrpt::poses::CPose3D x_predict;
348  x_predict.composeFrom(oldPose, *action);
349 
350  // and compute the obs. likelihood:
351  // --------------------------------------------
352  me.m_pfAuxiliaryPFStandard_estimatedProb[index] =
353  me.PF_SLAM_computeObservationLikelihoodForParticle(
354  PF_options, index, *observation, x_predict);
355 
356  // Combined log_likelihood: Previous weight * obs_likelihood:
357  return cur_logweight +
358  me.m_pfAuxiliaryPFStandard_estimatedProb[index];
359  }
360  else
361  {
362  // Do something similar to in Optimal sampling:
363  // Compute the quantity:
364  // w[i]*p(zt|z^{t-1},x^{[i],t-1})
365  // As the Monte-Carlo approximation of the integral over all posible
366  // $x_t$.
367  // --------------------------------------------
368  double indivLik, maxLik = -1e300;
369  mrpt::poses::CPose3D maxLikDraw;
370  size_t N = PF_options.pfAuxFilterOptimal_MaximumSearchSamples;
371  ASSERT_(N > 1)
372 
373  mrpt::math::CVectorDouble vectLiks(
374  N, 0); // The vector with the individual log-likelihoods.
375  mrpt::poses::CPose3D drawnSample;
376  for (size_t q = 0; q < N; q++)
377  {
378  me.m_movementDrawer.drawSample(drawnSample);
379  mrpt::poses::CPose3D x_predict = oldPose + drawnSample;
380 
381  // Estimate the mean...
382  indivLik = me.PF_SLAM_computeObservationLikelihoodForParticle(
383  PF_options, index, *observation, x_predict);
384 
385  MRPT_CHECK_NORMAL_NUMBER(indivLik);
386  vectLiks[q] = indivLik;
387  if (indivLik > maxLik)
388  { // Keep the maximum value:
389  maxLikDraw = drawnSample;
390  maxLik = indivLik;
391  }
392  }
393 
394  // This is done to avoid floating point overflow!!
395  // average_lik = \sum(e^liks) * e^maxLik / N
396  // log( average_lik ) = log( \sum(e^liks) ) + maxLik - log( N )
397  double avrgLogLik = math::averageLogLikelihood(vectLiks);
398 
399  // Save into the object:
400  me.m_pfAuxiliaryPFStandard_estimatedProb[index] =
401  avrgLogLik; // log( accum / N );
402 
403  me.m_pfAuxiliaryPFOptimal_maxLikelihood[index] = maxLik;
404  if (PF_options.pfAuxFilterOptimal_MLE)
405  me.m_pfAuxiliaryPFOptimal_maxLikDrawnMovement[index] =
406  maxLikDraw;
407 
408  // and compute the resulting probability of this particle:
409  // ------------------------------------------------------------
410  return cur_logweight +
411  me.m_pfAuxiliaryPFOptimal_estimatedProb[index];
412  }
413  MRPT_END
414  }
415 };
416 
417 // USE_OPTIMAL_SAMPLING:
418 // true -> PF_SLAM_implementation_pfAuxiliaryPFOptimal
419 // false -> PF_SLAM_implementation_pfAuxiliaryPFStandard
420 template <bool USE_OPTIMAL_SAMPLING>
421 class AuxiliaryPFStandardAndOptimal
422 {
423  public:
424  static constexpr bool DoesResampling = false;
425 
426  template <
427  class PARTICLE_TYPE, class MYSELF, class BINTYPE,
428  class EVALUATOR = std::conditional_t<
429  USE_OPTIMAL_SAMPLING, AuxPFOptimalEvaluator,
430  AuxPFStandardEvaluator>>
431  static void PF_SLAM_implementation(
432  const mrpt::obs::CActionCollection* actions,
433  const mrpt::obs::CSensoryFrame* sf,
434  const mrpt::bayes::CParticleFilter::TParticleFilterOptions& PF_options,
435  const TKLDParams& KLD_options, MYSELF& me)
436  {
437  MRPT_START
438  typedef std::set<BINTYPE, typename BINTYPE::lt_operator>
439  TSetStateSpaceBins;
440 
441  const size_t M = me.m_poseParticles.m_particles.size();
442 
443  // ----------------------------------------------------------------------
444  // We can execute optimal PF only when we have both, an action, and
445  // a valid observation from which to compute the likelihood:
446  // Accumulate odometry/actions until we have a valid observation, then
447  // process them simultaneously.
448  // ----------------------------------------------------------------------
449  if (!PF_SLAM_implementation_gatherActionsCheckBothActObs<
450  MYSELF, BINTYPE>(actions, sf, me))
451  return; // Nothing we can do here...
452  // OK, we have m_movementDrawer loaded and observations...let's roll!
453 
454  // -------------------------------------------------------------------------------
455  // 0) Common part: Prepare m_particles "draw" and compute
456  //"fastDrawSample"
457  // -------------------------------------------------------------------------------
458  // We need the (aproximate) maximum likelihood value for each
459  // previous particle [i]:
460  // max{ p( z^t | data^[i], x_(t-1)^[i], u_(t) ) }
461  //
462 
463  me.m_pfAuxiliaryPFOptimal_maxLikelihood.assign(
464  M, INVALID_LIKELIHOOD_VALUE);
465  me.m_pfAuxiliaryPFOptimal_maxLikDrawnMovement.resize(M);
466  me.m_pfAuxiliaryPFOptimal_estimatedProb.resize(M);
467  me.m_pfAuxiliaryPFStandard_estimatedProb.resize(M);
468 
469  // Pass the "mean" robot movement to the "weights" computing function:
470  mrpt::poses::CPose3D meanRobotMovement;
471  me.m_movementDrawer.getSamplingMean3D(meanRobotMovement);
472 
473  mrpt::bayes::CParticleFilterCapable::TParticleProbabilityEvaluator
474  func = [&](size_t i) {
475  return EVALUATOR::template evaluate<BINTYPE, MYSELF>(
476  PF_options, i, &meanRobotMovement, sf, me);
477  };
478 
479  me.m_poseParticles.prepareFastDrawSample(PF_options, func);
480 
481  // For USE_OPTIMAL_SAMPLING=1, m_pfAuxiliaryPFOptimal_maxLikelihood is
482  // now computed.
483 
484  if (USE_OPTIMAL_SAMPLING &&
485  me.isLoggingLevelVisible(mrpt::utils::LVL_DEBUG))
486  {
487  me.logStr(
488  mrpt::utils::LVL_DEBUG,
489  mrpt::format(
490  "[prepareFastDrawSample] max (log) = %10.06f\n",
491  math::maximum(me.m_pfAuxiliaryPFOptimal_estimatedProb)));
492  me.logStr(
493  mrpt::utils::LVL_DEBUG,
494  mrpt::format(
495  "[prepareFastDrawSample] max-mean (log) = %10.06f\n",
496  -math::mean(me.m_pfAuxiliaryPFOptimal_estimatedProb) +
497  math::maximum(
498  me.m_pfAuxiliaryPFOptimal_estimatedProb)));
499  me.logStr(
500  mrpt::utils::LVL_DEBUG,
501  mrpt::format(
502  "[prepareFastDrawSample] max-min (log) = %10.06f\n",
503  -math::minimum(me.m_pfAuxiliaryPFOptimal_estimatedProb) +
504  math::maximum(
505  me.m_pfAuxiliaryPFOptimal_estimatedProb)));
506  }
507 
508  // Now we have the vector "m_fastDrawProbability" filled out with:
509  // w[i]*p(zt|z^{t-1},x^{[i],t-1},X)
510  // where,
511  //
512  // =========== For USE_OPTIMAL_SAMPLING = true ====================
513  // X is the robot pose prior (as implemented in
514  // the aux. function "PF_SLAM_particlesEvaluator_AuxPFOptimal"),
515  // and also the "m_pfAuxiliaryPFOptimal_maxLikelihood" filled with the
516  // maximum lik. values.
517  //
518  // =========== For USE_OPTIMAL_SAMPLING = false ====================
519  // X is a single point close to the mean of the robot pose prior (as
520  // implemented in
521  // the aux. function "PF_SLAM_particlesEvaluator_AuxPFStandard").
522  //
523  std::vector<mrpt::math::TPose3D> newParticles;
524  std::vector<double> newParticlesWeight;
525  std::vector<size_t> newParticlesDerivedFromIdx;
526 
527  // We need the (aproximate) maximum likelihood value for each
528  // previous particle [i]:
529  //
530  // max{ p( z^t | data^[i], x_(t-1)^[i], u_(t) ) }
531  //
532  if (PF_options.pfAuxFilterOptimal_MLE)
533  me.m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed.assign(
534  M, false);
535 
536  const double maxMeanLik = math::maximum(
537  USE_OPTIMAL_SAMPLING ? me.m_pfAuxiliaryPFOptimal_estimatedProb
538  : me.m_pfAuxiliaryPFStandard_estimatedProb);
539 
540  if (!PF_options.adaptiveSampleSize)
541  {
542  // ----------------------------------------------------------------------
543  // 1) FIXED SAMPLE SIZE VERSION
544  // ----------------------------------------------------------------------
545  newParticles.resize(M);
546  newParticlesWeight.resize(M);
547  newParticlesDerivedFromIdx.resize(M);
548 
549  const bool doResample = me.m_poseParticles.ESS() < PF_options.BETA;
550 
551  for (size_t i = 0; i < M; i++)
552  {
553  size_t k;
554 
555  // Generate a new particle:
556  // (a) Draw a "t-1" m_particles' index:
557  // ----------------------------------------------------------------
558  if (doResample)
559  k = me.m_poseParticles.fastDrawSample(
560  PF_options); // Based on weights of last step only!
561  else
562  k = i;
563 
564  // Do one rejection sampling step:
565  // ---------------------------------------------
566  mrpt::poses::CPose3D newPose;
567  double newParticleLogWeight = 0.0;
568  PF_SLAM_aux_perform_one_rejection_sampling_step<
569  MYSELF, BINTYPE>(
570  doResample, maxMeanLik, k, sf, PF_options, newPose,
571  newParticleLogWeight, me);
572 
573  // Insert the new particle
574  newParticles[i] = newPose;
575  newParticlesDerivedFromIdx[i] = k;
576  newParticlesWeight[i] = newParticleLogWeight;
577 
578  } // for i
579  } // end fixed sample size
580  else
581  {
582  // -------------------------------------------------------------------------------------------------
583  // 2) ADAPTIVE SAMPLE SIZE VERSION
584  //
585  // Implementation of Dieter Fox's KLD algorithm
586  // JLBC (3/OCT/2006)
587  // -------------------------------------------------------------------------------------------------
588  // The new particle set:
589  newParticles.clear();
590  newParticlesWeight.resize(0);
591  newParticlesDerivedFromIdx.clear();
592 
593  // ------------------------------------------------------------------------------
594  // 2.1) PRELIMINARY STAGE: Build a list of
595  // pairs<TPathBin,vector_uint> with the
596  // indexes of m_particles that fall into each
597  // multi-dimensional-path bins
598  // //The bins will be saved into "stateSpaceBinsLastTimestep",
599  // and the list
600  // //of corresponding m_particles (in the last timestep), in
601  // "stateSpaceBinsLastTimestepParticles"
602  // - Added JLBC (01/DEC/2006)
603  // ------------------------------------------------------------------------------
604  TSetStateSpaceBins stateSpaceBinsLastTimestep;
605  std::vector<vector_uint> stateSpaceBinsLastTimestepParticles;
606  typename MYSELF::CParticleList::iterator partIt;
607  unsigned int partIndex;
608 
609  me.logStr(mrpt::utils::LVL_DEBUG, "[FIXED_SAMPLING] Computing...");
610  for (partIt = me.m_poseParticles.m_particles.begin(), partIndex = 0;
611  partIt != me.m_poseParticles.m_particles.end();
612  ++partIt, ++partIndex)
613  {
614  // Load the bin from the path data:
615  BINTYPE p;
616  KLF_loadBinFromParticle<PARTICLE_TYPE, BINTYPE>(
617  p, KLD_options, partIt->d.get());
618 
619  // Is it a new bin?
620  typename TSetStateSpaceBins::iterator posFound =
621  stateSpaceBinsLastTimestep.find(p);
622  if (posFound == stateSpaceBinsLastTimestep.end())
623  { // Yes, create a new pair <bin,index_list> in the list:
624  stateSpaceBinsLastTimestep.insert(p);
625  stateSpaceBinsLastTimestepParticles.push_back(
626  vector_uint(1, partIndex));
627  }
628  else
629  { // No, add the particle's index to the existing entry:
630  const size_t idx = std::distance(
631  stateSpaceBinsLastTimestep.begin(), posFound);
632  stateSpaceBinsLastTimestepParticles[idx].push_back(
633  partIndex);
634  }
635  }
636  me.logStr(
637  mrpt::utils::LVL_DEBUG,
638  mrpt::format(
639  "[FIXED_SAMPLING] done (%u bins in t-1)\n",
640  (unsigned int)stateSpaceBinsLastTimestep.size()));
641 
642  // ------------------------------------------------------------------------------
643  // 2.2) THE MAIN KLD-BASED DRAW SAMPLING LOOP
644  // ------------------------------------------------------------------------------
645  double delta_1 = 1.0 - KLD_options.KLD_delta;
646  double epsilon_1 = 0.5 / KLD_options.KLD_epsilon;
647  bool doResample = me.m_poseParticles.ESS() < PF_options.BETA;
648  // double maxLik =
649  // math::maximum(m_pfAuxiliaryPFOptimal_maxLikelihood);
650  // // For normalization purposes only
651 
652  // The desired dynamic number of m_particles (to be modified
653  // dynamically below):
654  const size_t minNumSamples_KLD = std::max(
655  (size_t)KLD_options.KLD_minSampleSize,
656  (size_t)round(
657  KLD_options.KLD_minSamplesPerBin *
658  stateSpaceBinsLastTimestep.size()));
659  size_t Nx = minNumSamples_KLD;
660 
661  const size_t Np1 = me.m_poseParticles.m_particles.size();
662  vector_size_t oldPartIdxsStillNotPropragated(
663  Np1); // Use a list since we'll use "erase" a lot here.
664  for (size_t k = 0; k < Np1; k++)
665  oldPartIdxsStillNotPropragated[k] = k; //.push_back(k);
666 
667  const size_t Np = stateSpaceBinsLastTimestepParticles.size();
668  vector_size_t permutationPathsAuxVector(Np);
669  for (size_t k = 0; k < Np; k++) permutationPathsAuxVector[k] = k;
670 
671  // Instead of picking randomly from "permutationPathsAuxVector", we
672  // can shuffle it now just once, then pick in sequence from the tail
673  // and resize the container:
674  std::random_shuffle(
675  permutationPathsAuxVector.begin(),
676  permutationPathsAuxVector.end(),
677  mrpt::random::random_generator_for_STL);
678 
679  size_t k = 0;
680  size_t N = 0;
681 
682  TSetStateSpaceBins stateSpaceBins;
683 
684  do // "N" is the index of the current "new particle":
685  {
686  // Generate a new particle:
687  //
688  // (a) Propagate the last set of m_particles, and only if the
689  // desired number of m_particles in this step is larger,
690  // perform a UNIFORM sampling from the last set. In this
691  // way the new weights can be computed in the same way for
692  // all m_particles.
693  // ---------------------------------------------------------------------------
694  if (doResample)
695  {
696  k = me.m_poseParticles.fastDrawSample(
697  PF_options); // Based on weights of last step only!
698  }
699  else
700  {
701  // Assure that at least one particle per "discrete-path" is
702  // taken (if the
703  // number of samples allows it)
704  if (permutationPathsAuxVector.size())
705  {
706  const size_t idxBinSpacePath =
707  *permutationPathsAuxVector.rbegin();
708  permutationPathsAuxVector.resize(
709  permutationPathsAuxVector.size() - 1);
710 
711  const size_t idx =
712  mrpt::random::getRandomGenerator()
713  .drawUniform32bit() %
714  stateSpaceBinsLastTimestepParticles[idxBinSpacePath]
715  .size();
716  k = stateSpaceBinsLastTimestepParticles[idxBinSpacePath]
717  [idx];
718  ASSERT_(k < me.m_poseParticles.m_particles.size());
719 
720  // Also erase it from the other permutation vector list:
721  oldPartIdxsStillNotPropragated.erase(std::find(
722  oldPartIdxsStillNotPropragated.begin(),
723  oldPartIdxsStillNotPropragated.end(), k));
724  }
725  else
726  {
727  // Select a particle from the previous set with a
728  // UNIFORM distribution, in such a way we will assign
729  // each particle the updated weight accounting for its
730  // last weight. The first "old_N" m_particles will be
731  // drawn using a uniform random selection without
732  // repetitions:
733  //
734  // Select a index from "oldPartIdxsStillNotPropragated"
735  // and remove it from the list:
736  if (oldPartIdxsStillNotPropragated.size())
737  {
738  const size_t idx =
739  mrpt::random::getRandomGenerator()
740  .drawUniform32bit() %
741  oldPartIdxsStillNotPropragated.size();
742  vector_size_t::iterator it =
743  oldPartIdxsStillNotPropragated.begin() +
744  idx; // advance(it,idx);
745  k = *it;
746  oldPartIdxsStillNotPropragated.erase(it);
747  }
748  else
749  {
750  // N>N_old -> Uniformly draw index:
751  k = mrpt::random::getRandomGenerator()
752  .drawUniform32bit() %
753  me.m_poseParticles.m_particles.size();
754  }
755  }
756  }
757 
758  // Do one rejection sampling step:
759  // ---------------------------------------------
760  mrpt::poses::CPose3D newPose;
761  double newParticleLogWeight;
762  PF_SLAM_aux_perform_one_rejection_sampling_step<
763  MYSELF, BINTYPE>(
764  doResample, maxMeanLik, k, sf, PF_options, newPose,
765  newParticleLogWeight, me);
766 
767  // Insert the new particle
768  newParticles.push_back(newPose);
769  newParticlesDerivedFromIdx.push_back(k);
770  newParticlesWeight.push_back(newParticleLogWeight);
771 
772  // ----------------------------------------------------------------
773  // Now, the KLD-sampling dynamic sample size stuff:
774  // look if the particle's PATH falls into a new bin or not:
775  // ----------------------------------------------------------------
776  BINTYPE p;
777  const mrpt::math::TPose3D newPose_s = newPose;
778  KLF_loadBinFromParticle<PARTICLE_TYPE, BINTYPE>(
779  p, KLD_options, me.m_poseParticles.m_particles[k].d.get(),
780  &newPose_s);
781 
782  // -----------------------------------------------------------------------------
783  // Look for the bin "p" into "stateSpaceBins": If it is not yet
784  // into the set,
785  // then we may increase the desired particle number:
786  // -----------------------------------------------------------------------------
787 
788  // Found?
789  if (stateSpaceBins.find(p) == stateSpaceBins.end())
790  {
791  // It falls into a new bin: add to the stateSpaceBins:
792  stateSpaceBins.insert(p);
793 
794  // K = K + 1
795  int K = stateSpaceBins.size();
796  if (K > 1)
797  {
798  // Update the number of m_particles!!
799  Nx =
800  (size_t)(epsilon_1 * math::chi2inv(delta_1, K - 1));
801  // printf("k=%u \tn=%u \tNx:%u\n", k,
802  // newParticles.size(), Nx);
803  }
804  }
805 
806  N = newParticles.size();
807 
808  } while ((N < KLD_options.KLD_maxSampleSize &&
809  N < std::max(Nx, minNumSamples_KLD)) ||
810  (permutationPathsAuxVector.size() && !doResample));
811 
812  me.logStr(
813  mrpt::utils::LVL_DEBUG,
814  mrpt::format(
815  "[ADAPTIVE SAMPLE SIZE] #Bins: %u \t #Particles: %u \t "
816  "Nx=%u\n",
817  static_cast<unsigned>(stateSpaceBins.size()),
818  static_cast<unsigned>(N), (unsigned)Nx));
819  } // end adaptive sample size
820 
821  // ---------------------------------------------------------------------------------
822  // Substitute old by new particle set:
823  // Old are in "m_particles"
824  // New are in "newParticles",
825  // "newParticlesWeight","newParticlesDerivedFromIdx"
826  // ---------------------------------------------------------------------------------
827  me.PF_SLAM_implementation_replaceByNewParticleSet(
828  me.m_poseParticles.m_particles, newParticles, newParticlesWeight,
829  newParticlesDerivedFromIdx);
830 
831  // In this PF_algorithm, we must do weight normalization by ourselves:
832  me.m_poseParticles.normalizeWeights();
833 
834  MRPT_END
835  } // end of PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal
836 
837  /** Auxiliary method called by PF implementations: return true if we have
838  * both action & observation, otherwise, return false AND accumulate the
839  * odometry so when we have an observation we didn't lose a thing. On
840  * return=true, the "m_movementDrawer" member is loaded and ready to draw
841  * samples of the increment of pose since last step.
842  * This method is smart enough to accumulate CActionRobotMovement2D or
843  * CActionRobotMovement3D, whatever comes in.
844  * \ingroup mrpt_slam_grp
845  */
846  template <class MYSELF, class BINTYPE>
847  static bool PF_SLAM_implementation_gatherActionsCheckBothActObs(
848  const mrpt::obs::CActionCollection* actions,
849  const mrpt::obs::CSensoryFrame* sf, MYSELF& me)
850  {
851  if (actions != nullptr) // A valid action?
852  {
853  mrpt::obs::CActionRobotMovement2D::Ptr robotMovement2D =
854  actions->getBestMovementEstimation();
855  if (robotMovement2D)
856  {
857  if (me.m_accumRobotMovement3DIsValid)
858  THROW_EXCEPTION("Mixing 2D and 3D actions is not allowed.");
859 
860  ASSERT_(robotMovement2D->poseChange);
861  if (!me.m_accumRobotMovement2DIsValid)
862  { // First time:
863  robotMovement2D->poseChange->getMean(
864  me.m_accumRobotMovement2D.rawOdometryIncrementReading);
865  me.m_accumRobotMovement2D.motionModelConfiguration =
866  robotMovement2D->motionModelConfiguration;
867  }
868  else
869  me.m_accumRobotMovement2D.rawOdometryIncrementReading +=
870  robotMovement2D->poseChange->getMeanVal();
871 
872  me.m_accumRobotMovement2DIsValid = true;
873  }
874  else // If there is no 2D action, look for a 3D action:
875  {
876  mrpt::obs::CActionRobotMovement3D::Ptr robotMovement3D =
877  actions
878  ->getActionByClass<mrpt::obs::CActionRobotMovement3D>();
879  if (robotMovement3D)
880  {
881  if (me.m_accumRobotMovement2DIsValid)
882  THROW_EXCEPTION(
883  "Mixing 2D and 3D actions is not allowed.")
884 
885  if (!me.m_accumRobotMovement3DIsValid)
886  me.m_accumRobotMovement3D = robotMovement3D->poseChange;
887  else
888  me.m_accumRobotMovement3D +=
889  robotMovement3D->poseChange;
890  // This "+=" takes care of all the Jacobians, etc... You
891  // MUST love C++!!! ;-)
892 
893  me.m_accumRobotMovement3DIsValid = true;
894  }
895  else
896  return false; // We have no actions...
897  }
898  }
899 
900  const bool SFhasValidObservations =
901  (sf == nullptr)
902  ? false
903  : me.PF_SLAM_implementation_doWeHaveValidObservations(
904  me.m_poseParticles.m_particles, sf);
905 
906  // All the things we need?
907  if (!((me.m_accumRobotMovement2DIsValid ||
908  me.m_accumRobotMovement3DIsValid) &&
909  SFhasValidObservations))
910  return false;
911 
912  // Since we're gonna return true, load the pose-drawer:
913  // Take the accum. actions as input:
914  if (me.m_accumRobotMovement3DIsValid)
915  {
916  me.m_movementDrawer.setPosePDF(
917  me.m_accumRobotMovement3D); // <--- Set mov. drawer
918  me.m_accumRobotMovement3DIsValid =
919  false; // Reset odometry for next iteration
920  }
921  else
922  {
923  mrpt::obs::CActionRobotMovement2D theResultingRobotMov;
924  theResultingRobotMov.computeFromOdometry(
925  me.m_accumRobotMovement2D.rawOdometryIncrementReading,
926  me.m_accumRobotMovement2D.motionModelConfiguration);
927 
928  ASSERT_(theResultingRobotMov.poseChange);
929  me.m_movementDrawer.setPosePDF(
930  theResultingRobotMov.poseChange
931  .get_ptr()); // <--- Set mov. drawer
932  me.m_accumRobotMovement2DIsValid =
933  false; // Reset odometry for next iteration
934  }
935  return true;
936  } // end of PF_SLAM_implementation_gatherActionsCheckBothActObs
937 
938  /* ------------------------------------------------------------------------
939  PF_SLAM_aux_perform_one_rejection_sampling_step
940  ------------------------------------------------------------------------
941  */
942  template <class MYSELF, class BINTYPE>
943  static void PF_SLAM_aux_perform_one_rejection_sampling_step(
944  const bool doResample, const double maxMeanLik,
945  size_t k, // The particle from the old set "m_particles[]"
946  const mrpt::obs::CSensoryFrame* sf,
947  const mrpt::bayes::CParticleFilter::TParticleFilterOptions& PF_options,
948  mrpt::poses::CPose3D& out_newPose, double& out_newParticleLogWeight,
949  MYSELF& me)
950  {
951  // ADD-ON: If the 'm_pfAuxiliaryPFOptimal_estimatedProb[k]' is
952  // **extremelly** low relative to the other m_particles,
953  // resample only this particle with a copy of another one, uniformly:
954  while (((USE_OPTIMAL_SAMPLING
955  ? me.m_pfAuxiliaryPFOptimal_estimatedProb[k]
956  : me.m_pfAuxiliaryPFStandard_estimatedProb[k]) -
957  maxMeanLik) < -PF_options.max_loglikelihood_dyn_range)
958  {
959  // Select another 'k' uniformly:
960  k = mrpt::random::getRandomGenerator().drawUniform32bit() %
961  me.m_poseParticles.m_particles.size();
962  me.logStr(
963  mrpt::utils::LVL_DEBUG,
964  "[PF_SLAM_aux_perform_one_rejection_sampling_step] Warning: "
965  "Discarding very unlikely particle.");
966  }
967 
968  bool pose_is_valid;
969  const mrpt::poses::CPose3D oldPose =
970  mrpt::poses::CPose3D(me.getLastPose(
971  k, pose_is_valid)); // current pose of the k'th particle
972  // ASSERT_(pose_is_valid); Use the default (0,0,0) if path is empty.
973 
974  // (b) Rejection-sampling: Draw a new robot pose from x[k],
975  // and accept it with probability p(zk|x) / maxLikelihood:
976  // ----------------------------------------------------------------
977  double poseLogLik;
978  if (me.PF_SLAM_implementation_skipRobotMovement())
979  {
980  // The first robot pose in the SLAM execution: All m_particles start
981  // at the same point (this is the lowest bound of subsequent
982  // uncertainty):
983  out_newPose = oldPose;
984  poseLogLik = 0;
985  }
986  else
987  {
988  mrpt::poses::CPose3D movementDraw;
989  if (!USE_OPTIMAL_SAMPLING)
990  { // APF:
991  me.m_movementDrawer.drawSample(movementDraw);
992  out_newPose.composeFrom(
993  oldPose,
994  movementDraw); // newPose = oldPose + movementDraw;
995  // Compute likelihood:
996  poseLogLik = me.PF_SLAM_computeObservationLikelihoodForParticle(
997  PF_options, k, *sf, out_newPose);
998  }
999  else
1000  { // Optimal APF with rejection sampling:
1001  // Rejection-sampling:
1002  double acceptanceProb;
1003  int timeout = 0;
1004  const int maxTries = 10000;
1005  double bestTryByNow_loglik =
1006  -std::numeric_limits<double>::max();
1007  mrpt::math::TPose3D bestTryByNow_pose;
1008  do
1009  {
1010  // Draw new robot pose:
1011  if (PF_options.pfAuxFilterOptimal_MLE &&
1012  !me.m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed
1013  [k])
1014  { // No! first take advantage of a good drawn value, but
1015  // only
1016  // once!!
1017  me.m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed
1018  [k] = true;
1019  movementDraw = mrpt::poses::CPose3D(
1020  me.m_pfAuxiliaryPFOptimal_maxLikDrawnMovement[k]);
1021  }
1022  else
1023  {
1024  // Draw new robot pose:
1025  me.m_movementDrawer.drawSample(movementDraw);
1026  }
1027 
1028  out_newPose.composeFrom(
1029  oldPose,
1030  movementDraw); // out_newPose = oldPose + movementDraw;
1031 
1032  // Compute acceptance probability:
1033  poseLogLik =
1034  me.PF_SLAM_computeObservationLikelihoodForParticle(
1035  PF_options, k, *sf, out_newPose);
1036 
1037  if (poseLogLik > bestTryByNow_loglik)
1038  {
1039  bestTryByNow_loglik = poseLogLik;
1040  bestTryByNow_pose = out_newPose;
1041  }
1042 
1043  double ratioLikLik = std::exp(
1044  poseLogLik -
1045  me.m_pfAuxiliaryPFOptimal_maxLikelihood[k]);
1046  acceptanceProb = std::min(1.0, ratioLikLik);
1047 
1048  if (ratioLikLik > 1)
1049  {
1050  me.m_pfAuxiliaryPFOptimal_maxLikelihood[k] =
1051  poseLogLik; // :'-( !!!
1052  // acceptanceProb = 0; // Keep searching or keep
1053  // this one?
1054  }
1055  } while (++timeout < maxTries &&
1056  acceptanceProb <
1057  mrpt::random::getRandomGenerator().drawUniform(
1058  0.0, 0.999));
1059 
1060  if (timeout >= maxTries)
1061  {
1062  out_newPose = mrpt::poses::CPose3D(bestTryByNow_pose);
1063  poseLogLik = bestTryByNow_loglik;
1064  me.logStr(
1065  mrpt::utils::LVL_WARN,
1066  "[PF_implementation] Warning: timeout in rejection "
1067  "sampling.");
1068  }
1069  }
1070 
1071  // And its weight:
1072  if (USE_OPTIMAL_SAMPLING)
1073  { // Optimal PF
1074  if (doResample)
1075  out_newParticleLogWeight =
1076  0; // By definition of our optimal
1077  // PF, all samples have identical
1078  // weights.
1079  else
1080  {
1081  const double weightFact =
1082  me.m_pfAuxiliaryPFOptimal_estimatedProb[k] *
1083  PF_options.powFactor;
1084  out_newParticleLogWeight =
1085  me.m_poseParticles.m_particles[k].log_w + weightFact;
1086  }
1087  }
1088  else
1089  { // APF:
1090  const double weightFact =
1091  (poseLogLik - me.m_pfAuxiliaryPFStandard_estimatedProb[k]) *
1092  PF_options.powFactor;
1093  if (doResample)
1094  out_newParticleLogWeight = weightFact;
1095  else
1096  out_newParticleLogWeight =
1097  weightFact + me.m_poseParticles.m_particles[k].log_w;
1098  }
1099  }
1100  // Done.
1101  } // end PF_SLAM_aux_perform_one_rejection_sampling_step
1102 };
1103 
1104 /** A generic implementation of the PF method
1105  * "prediction_and_update_pfAuxiliaryPFStandard" (Auxiliary particle filter with
1106  * the standard proposal),
1107  * common to both localization and mapping.
1108  *
1109  * - BINTYPE: TPoseBin or whatever to discretize the sample space for
1110  * KLD-sampling.
1111  *
1112  * This method is described in the paper:
1113  * Pitt, M.K.; Shephard, N. (1999). "Filtering Via Simulation: Auxiliary
1114  * Particle Filters".
1115  * Journal of the American Statistical Association 94 (446): 590-591.
1116  * doi:10.2307/2670179.
1117  *
1118  */
1119 using AuxiliaryPFStandard = AuxiliaryPFStandardAndOptimal<false>;
1120 
1121 /** A generic implementation of the PF method
1122  * "prediction_and_update_pfAuxiliaryPFOptimal" (optimal sampling with rejection
1123  * sampling approximation),
1124  * common to both localization and mapping.
1125  *
1126  * - BINTYPE: TPoseBin or whatever to discretize the sample space for
1127  * KLD-sampling.
1128  *
1129  * This method implements optimal sampling with a rejection sampling-based
1130  * approximation of the true posterior.
1131  * For details, see the papers:
1132  *
1133  * J.-L. Blanco, J. Gonzalez, and J.-A. Fernandez-Madrigal,
1134  * "An Optimal Filtering Algorithm for Non-Parametric Observation Models in
1135  * Robot Localization," in Proc. IEEE International Conference on Robotics
1136  * and Automation (ICRA'08), 2008, pp. 461466.
1137  */
1138 using AuxiliaryPFOptimal = AuxiliaryPFStandardAndOptimal<true>;
1139 
1140 } // namespace slam
1141 } // namespace mrpt
1142 
1143 #endif
CActionRobotMovement2D.h
mrpt::random::CRandomGenerator::drawUniform32bit
uint32_t drawUniform32bit()
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm, in the whole range of 32-bit integers.
Definition: RandomGenerator.cpp:42
mrpt::obs::CActionRobotMovement3D::poseChange
mrpt::poses::CPose3DPDFGaussian poseChange
The 3D pose change probabilistic estimation.
Definition: CActionRobotMovement3D.h:49
min
#define min(a, b)
Definition: rplidar_driver.cpp:42
mrpt::vector_uint
std::vector< uint32_t > vector_uint
Definition: types_simple.h:29
mrpt::poses::CPose3DPDFGaussian::getMean
void getMean(CPose3D &mean_pose) const override
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF).
Definition: CPose3DPDFGaussian.h:91
mrpt::mrpt::format
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
mrpt::slam::AuxiliaryPFStandardAndOptimal::PF_SLAM_aux_perform_one_rejection_sampling_step
static void PF_SLAM_aux_perform_one_rejection_sampling_step(const bool doResample, const double maxMeanLik, size_t k, const mrpt::obs::CSensoryFrame *sf, const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options, mrpt::poses::CPose3D &out_newPose, double &out_newParticleLogWeight, MYSELF &me)
Definition: PF_implementations.h:943
mrpt::slam::StandardProposal::DoesResampling
static constexpr bool DoesResampling
Definition: PF_implementations.h:47
INVALID_LIKELIHOOD_VALUE
#define INVALID_LIKELIHOOD_VALUE
Definition: CParticleFilterCapable.h:21
mrpt::bayes::CParticleFilter::TParticleFilterOptions::pfAuxFilterOptimal_MaximumSearchSamples
unsigned int pfAuxFilterOptimal_MaximumSearchSamples
In the algorithm "CParticleFilter::pfAuxiliaryPFOptimal" (and in "CParticleFilter::pfAuxiliaryPFStand...
Definition: CParticleFilter.h:132
MRPT_CHECK_NORMAL_NUMBER
#define MRPT_CHECK_NORMAL_NUMBER(val)
Definition: CColouredPointsMap.h:313
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: CColouredPointsMap.h:112
q
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3721
iterator
Scalar * iterator
Definition: eigen_plugins.h:26
data_utils.h
mrpt::math::dynamic_vector
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:42
mrpt::slam::TKLDParams
Option set for KLD algorithm.
Definition: TKLDParams.h:20
TKLDParams.h
CActionRobotMovement3D.h
mrpt::obs::CActionRobotMovement2D::computeFromOdometry
void computeFromOdometry(const mrpt::poses::CPose2D &odometryIncrement, const TMotionModelOptions &options)
Computes the PDF of the pose increment from an odometry reading and according to the given motion mod...
Definition: CActionRobotMovement2D.cpp:408
PF_implementations_data.h
mrpt::slam::AuxPFStandardEvaluator
Compute w[i]*p(z_t | mu_t^i), with mu_t^i being the mean of the new robot pose.
Definition: PF_implementations.h:325
mrpt::utils::find
const_iterator find(const KEY &key) const
Definition: ts_hash_map.h:220
mrpt::slam::StandardProposal
A generic implementation of the PF method "pfStandardProposal" (standard proposal distribution...
Definition: PF_implementations.h:44
mrpt::obs::CActionCollection
Declares a class for storing a collection of robot actions.
Definition: CActionCollection.h:29
mrpt::slam::AuxiliaryPFStandardAndOptimal::PF_SLAM_implementation
static void PF_SLAM_implementation(const mrpt::obs::CActionCollection *actions, const mrpt::obs::CSensoryFrame *sf, const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options, const TKLDParams &KLD_options, MYSELF &me)
Definition: PF_implementations.h:431
mrpt::math::minimum
CONTAINER::Scalar minimum(const CONTAINER &v)
Definition: ops_containers.h:144
mrpt::obs::CActionRobotMovement3D
Represents a probabilistic 3D (6D) movement.
Definition: CActionRobotMovement3D.h:29
mrpt::obs::CActionRobotMovement2D
Represents a probabilistic 2D movement of the robot mobile base.
Definition: CActionRobotMovement2D.h:33
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::slam::TKLDParams::KLD_minSampleSize
unsigned int KLD_minSampleSize
Parameters for the KLD adaptive sample size algorithm (see Dieter Fox&#39;s papers), which is used only i...
Definition: TKLDParams.h:41
CParticleFilterData.h
mrpt::slam::StandardProposal::PF_SLAM_implementation
static void PF_SLAM_implementation(const mrpt::obs::CActionCollection *actions, const mrpt::obs::CSensoryFrame *sf, const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options, const TKLDParams &KLD_options, MYSELF &me)
Definition: PF_implementations.h:50
MRPT_END
#define MRPT_END
Definition: CColouredPointsMap.h:430
mrpt::obs::CActionRobotMovement2D::Ptr
std::shared_ptr< CActionRobotMovement2D > Ptr
Definition: CActionRobotMovement2D.h:35
mrpt::obs::CActionCollection::getBestMovementEstimation
CActionRobotMovement2D::Ptr getBestMovementEstimation() const
Returns the best pose increment estimator in the collection, based on the determinant of its pose cha...
Definition: CActionCollection.cpp:118
MRPT_UNUSED_PARAM
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
Definition: CColouredPointsMap.h:366
mrpt::poses::CPose3D::composeFrom
void composeFrom(const CPose3D &A, const CPose3D &B)
Makes "this = A (+) B"; this method is slightly more efficient than "this= A + B;" since it avoids th...
Definition: CPose3D.cpp:639
mrpt::slam::TKLDParams::KLD_minSamplesPerBin
double KLD_minSamplesPerBin
(Default: KLD_minSamplesPerBin=0) The minimum number of samples will be the maximum of KLD_minSampleS...
Definition: TKLDParams.h:46
mrpt::math::averageLogLikelihood
double averageLogLikelihood(const CVectorDouble &logLikelihoods)
A numerically-stable method to compute average likelihood values with strongly different ranges (unwe...
Definition: math.cpp:1777
index
GLuint index
Definition: glext.h:4054
mrpt::obs::CActionRobotMovement2D::poseChange
mrpt::utils::poly_ptr_ptr< mrpt::poses::CPosePDF::Ptr > poseChange
The 2D pose change probabilistic estimation.
Definition: CActionRobotMovement2D.h:51
mrpt::slam::AuxiliaryPFStandardAndOptimal::PF_SLAM_implementation_gatherActionsCheckBothActObs
static bool PF_SLAM_implementation_gatherActionsCheckBothActObs(const mrpt::obs::CActionCollection *actions, const mrpt::obs::CSensoryFrame *sf, MYSELF &me)
Auxiliary method called by PF implementations: return true if we have both action & observation...
Definition: PF_implementations.h:847
mrpt::obs::CSensoryFrame
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:54
mrpt::obs::CActionRobotMovement3D::Ptr
std::shared_ptr< CActionRobotMovement3D > Ptr
Definition: CActionRobotMovement3D.h:31
mrpt::bayes::CParticleFilter::TParticleFilterOptions::pfAuxFilterStandard_FirstStageWeightsMonteCarlo
bool pfAuxFilterStandard_FirstStageWeightsMonteCarlo
Only for PF_algorithm==pfAuxiliaryPFStandard: If false, the APF will predict the first stage weights ...
Definition: CParticleFilter.h:162
mrpt::math::maximum
CONTAINER::Scalar maximum(const CONTAINER &v)
Definition: ops_containers.h:139
mrpt::bayes::CParticleFilter::TParticleFilterOptions::BETA
double BETA
The resampling of particles will be performed when ESS (in range [0,1]) < BETA (default is 0...
Definition: CParticleFilter.h:121
mrpt::random::random_generator_for_STL
ptrdiff_t random_generator_for_STL(ptrdiff_t i)
A random number generator for usage in STL algorithms expecting a function like this (eg...
Definition: RandomGenerators.h:333
mrpt::slam::TKLDParams::KLD_maxSampleSize
unsigned int KLD_maxSampleSize
Definition: TKLDParams.h:41
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
mrpt::slam::AuxPFOptimalEvaluator::evaluate
static double evaluate(const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options, unsigned long index, const mrpt::poses::CPose3D *action, const mrpt::obs::CSensoryFrame *observation, const MYSELF &me)
Definition: PF_implementations.h:255
CParticleFilterCapable.h
mrpt::math::chi2inv
double chi2inv(double P, unsigned int dim=1)
The "quantile" of the Chi-Square distribution, for dimension "dim" and probability 0<P<1 (the inverse...
Definition: math.cpp:998
mrpt::utils::poly_ptr_ptr::get_ptr
const T & get_ptr() const
Definition: poly_ptr_ptr.h:92
mrpt::bayes::CParticleFilter::TParticleFilterOptions::max_loglikelihood_dyn_range
double max_loglikelihood_dyn_range
Only for PF_algorithm=pfAuxiliaryPFOptimal: If a given particle has a max_likelihood (from the a-prio...
Definition: CParticleFilter.h:151
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
mrpt::bayes::CParticleFilter::TParticleFilterOptions::powFactor
double powFactor
An optional step to "smooth" dramatic changes in the observation model to affect the variance of the ...
Definition: CParticleFilter.h:136
mrpt::bayes::CParticleFilter::TParticleFilterOptions
The configuration of a particle filter.
Definition: CParticleFilter.h:102
mrpt::math::TPose3D
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: lightweight_geom_data.h:616
mrpt::math::mean
double mean(const CONTAINER &v)
Computes the mean value of a vector.
Definition: ops_containers.h:234
ASSERT_
#define ASSERT_(f)
Definition: CColouredPointsMap.h:310
mrpt::utils::round
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:25
mrpt::slam::AuxPFStandardEvaluator::evaluate
static double evaluate(const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options, unsigned long index, const mrpt::poses::CPose3D *action, const mrpt::obs::CSensoryFrame *observation, const MYSELF &me)
Definition: PF_implementations.h:329
mrpt::vector_size_t
std::vector< size_t > vector_size_t
Definition: types_simple.h:26
mrpt::bayes::CParticleFilter::TParticleFilterOptions::pfAuxFilterOptimal_MLE
bool pfAuxFilterOptimal_MLE
(Default=false) In the algorithm "CParticleFilter::pfAuxiliaryPFOptimal", if set to true...
Definition: CParticleFilter.h:168
mrpt::obs::CActionCollection::getActionByClass
T::Ptr getActionByClass(const size_t &ith=0) const
Access to the i&#39;th action of a given class, or a nullptr smart pointer if there is no action of that ...
Definition: CActionCollection.h:132
CActionCollection.h
mrpt::random::getRandomGenerator
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:25
p
GLfloat GLfloat p
Definition: glext.h:6305
mrpt::math::distance
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1885
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