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



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 15 15:49:54 UTC 2020