MRPT  2.0.5
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
uint32_t drawUniform32bit()
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm, in the whole range of 32-bit integers.
mrpt::math::TPose3D asTPose() const
Definition: CPose3D.cpp:759
#define MRPT_START
Definition: exceptions.h:241
const_iterator find(const KEY &key) const
Definition: ts_hash_map.h:224
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
#define INVALID_LIKELIHOOD_VALUE
unsigned int pfAuxFilterOptimal_MaximumSearchSamples
In the algorithm "CParticleFilter::pfAuxiliaryPFOptimal" (and in "CParticleFilter::pfAuxiliaryPFStand...
void shuffle(RandomIt first, RandomIt last, URBG &&g)
Uniform shuffle a sequence.
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...
Option set for KLD algorithm.
Definition: TKLDParams.h:17
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...
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...
Declares a class for storing a collection of robot actions.
A set of common data shared by PF implementations for both SLAM and localization. ...
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
Represents a probabilistic 3D (6D) movement.
Represents a probabilistic 2D movement of the robot mobile base.
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)
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:38
CActionRobotMovement2D::Ptr getBestMovementEstimation() const
Returns the best pose increment estimator in the collection, based on the determinant of its pose cha...
#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
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
double KLD_minSamplesPerBin
(Default: KLD_minSamplesPerBin=0) The minimum number of samples will be the maximum of KLD_minSampleS...
Definition: TKLDParams.h:43
double averageLogLikelihood(const CVectorDouble &logLikelihoods)
A numerically-stable method to compute average likelihood values with strongly different ranges (unwe...
Definition: math.cpp:293
CONTAINER::Scalar maximum(const CONTAINER &v)
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:51
This virtual class defines the interface that any particles based PDF class must implement in order t...
bool pfAuxFilterStandard_FirstStageWeightsMonteCarlo
Only for PF_algorithm==pfAuxiliaryPFStandard: If false, the APF will predict the first stage weights ...
double BETA
The resampling of particles will be performed when ESS (in range [0,1]) < BETA (default is 0...
unsigned int KLD_maxSampleSize
Definition: TKLDParams.h:38
T::Ptr getActionByClass(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 ...
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...
CONTAINER::Scalar minimum(const CONTAINER &v)
constexpr std::size_t size() const
Definition: TPoseOrPoint.h:67
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)
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
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...
mrpt::containers::deepcopy_poly_ptr< mrpt::poses::CPosePDF::Ptr > poseChange
The 2D pose change probabilistic estimation.
double max_loglikelihood_dyn_range
Only for PF_algorithm=pfAuxiliaryPFOptimal: If a given particle has a max_likelihood (from the a-prio...
void logStr(const VerbosityLevel level, std::string_view msg_str) const
Main method to add the specified message string to the logger.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
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.
double powFactor
An optional step to "smooth" dramatic changes in the observation model to affect the variance of the ...
The configuration of a particle filter.
#define MRPT_END
Definition: exceptions.h:245
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
double mean(const CONTAINER &v)
Computes the mean value of a vector.
bool pfAuxFilterOptimal_MLE
(Default=false) In the algorithm "CParticleFilter::pfAuxiliaryPFOptimal", if set to true...
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...
particle_storage_mode
use for CProbabilityParticle
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1807
bool adaptiveSampleSize
A flag that indicates whether the CParticleFilterCapable object should perform adative sample size (d...
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:24



Page generated by Doxygen 1.8.14 for MRPT 2.0.5 Git: eda5ade6c Tue Aug 4 12:48:50 2020 +0200 at mar ago 4 13:00:11 CEST 2020