MRPT  1.9.9
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-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #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>
427  const void* action, const void* observation)
428 {
429  MRPT_UNUSED_PARAM(action);
430  MRPT_START
431 
432  // const PF_implementation<PARTICLE_TYPE,MYSELF> *myObj =
433  // reinterpret_cast<const PF_implementation<PARTICLE_TYPE,MYSELF>*>( obj );
434  const auto* me = static_cast<const MYSELF*>(obj);
435 
436  // Compute the quantity:
437  // w[i]*p(zt|z^{t-1},x^{[i],t-1})
438  // As the Monte-Carlo approximation of the integral over all posible $x_t$.
439  // --------------------------------------------
440  double indivLik, maxLik = -1e300;
441  mrpt::poses::CPose3D maxLikDraw;
442  size_t N = PF_options.pfAuxFilterOptimal_MaximumSearchSamples;
443  ASSERT_(N > 1);
444 
445  bool pose_is_valid;
446  const mrpt::poses::CPose3D oldPose =
447  mrpt::poses::CPose3D(me->getLastPose(index, pose_is_valid));
448  mrpt::math::CVectorDouble vectLiks(
449  N, 0); // The vector with the individual log-likelihoods.
450  mrpt::poses::CPose3D drawnSample;
451  for (size_t q = 0; q < N; q++)
452  {
453  me->m_movementDrawer.drawSample(drawnSample);
454  mrpt::poses::CPose3D x_predict = oldPose + drawnSample;
455 
456  // Estimate the mean...
457  indivLik = me->PF_SLAM_computeObservationLikelihoodForParticle(
458  PF_options, index,
459  *static_cast<const mrpt::obs::CSensoryFrame*>(observation),
460  x_predict);
461 
462  MRPT_CHECK_NORMAL_NUMBER(indivLik);
463  vectLiks[q] = indivLik;
464  if (indivLik > maxLik)
465  { // Keep the maximum value:
466  maxLikDraw = drawnSample;
467  maxLik = indivLik;
468  }
469  }
470 
471  // This is done to avoid floating point overflow!!
472  // average_lik = \sum(e^liks) * e^maxLik / N
473  // log( average_lik ) = log( \sum(e^liks) ) + maxLik - log( N )
474  double avrgLogLik = math::averageLogLikelihood(vectLiks);
475 
476  // Save into the object:
477  me->m_pfAuxiliaryPFOptimal_estimatedProb[index] =
478  avrgLogLik; // log( accum / N );
479  me->m_pfAuxiliaryPFOptimal_maxLikelihood[index] = maxLik;
480 
481  if (PF_options.pfAuxFilterOptimal_MLE)
482  me->m_pfAuxiliaryPFOptimal_maxLikDrawnMovement[index] =
483  maxLikDraw.asTPose();
484 
485  // and compute the resulting probability of this particle:
486  // ------------------------------------------------------------
487  return me->m_particles[index].log_w +
488  me->m_pfAuxiliaryPFOptimal_estimatedProb[index];
489 
490  MRPT_END
491 } // end of PF_SLAM_particlesEvaluator_AuxPFOptimal
492 
493 /** Compute w[i]*p(z_t | mu_t^i), with mu_t^i being
494  * the mean of the new robot pose
495  *
496  * \param action MUST be a "const mrpt::poses::CPose3D*"
497  * \param observation MUST be a "const CSensoryFrame*"
498  */
499 template <
500  class PARTICLE_TYPE, class MYSELF,
502 template <class BINTYPE>
507  const void* action, const void* observation)
508 {
509  MRPT_START
510 
511  // const PF_implementation<PARTICLE_TYPE,MYSELF> *myObj =
512  // reinterpret_cast<const PF_implementation<PARTICLE_TYPE,MYSELF>*>( obj );
513  const auto* myObj = static_cast<const MYSELF*>(obj);
514 
515  // Take the previous particle weight:
516  const double cur_logweight = myObj->m_particles[index].log_w;
517  bool pose_is_valid;
518  const mrpt::poses::CPose3D oldPose =
519  mrpt::poses::CPose3D(myObj->getLastPose(index, pose_is_valid));
520 
522  {
523  // Just use the mean:
524  // , take the mean of the posterior density:
525  mrpt::poses::CPose3D x_predict;
526  x_predict.composeFrom(
527  oldPose, *static_cast<const mrpt::poses::CPose3D*>(action));
528 
529  // and compute the obs. likelihood:
530  // --------------------------------------------
531  myObj->m_pfAuxiliaryPFStandard_estimatedProb[index] =
532  myObj->PF_SLAM_computeObservationLikelihoodForParticle(
533  PF_options, index,
534  *static_cast<const mrpt::obs::CSensoryFrame*>(observation),
535  x_predict);
536 
537  // Combined log_likelihood: Previous weight * obs_likelihood:
538  return cur_logweight +
539  myObj->m_pfAuxiliaryPFStandard_estimatedProb[index];
540  }
541  else
542  {
543  // Do something similar to in Optimal sampling:
544  // Compute the quantity:
545  // w[i]*p(zt|z^{t-1},x^{[i],t-1})
546  // As the Monte-Carlo approximation of the integral over all posible
547  // $x_t$.
548  // --------------------------------------------
549  double indivLik, maxLik = -1e300;
550  mrpt::poses::CPose3D maxLikDraw;
551  size_t N = PF_options.pfAuxFilterOptimal_MaximumSearchSamples;
552  ASSERT_(N > 1);
553 
554  mrpt::math::CVectorDouble vectLiks(
555  N, 0); // The vector with the individual log-likelihoods.
556  mrpt::poses::CPose3D drawnSample;
557  for (size_t q = 0; q < N; q++)
558  {
559  myObj->m_movementDrawer.drawSample(drawnSample);
560  mrpt::poses::CPose3D x_predict = oldPose + drawnSample;
561 
562  // Estimate the mean...
563  indivLik = myObj->PF_SLAM_computeObservationLikelihoodForParticle(
564  PF_options, index,
565  *static_cast<const mrpt::obs::CSensoryFrame*>(observation),
566  x_predict);
567 
568  MRPT_CHECK_NORMAL_NUMBER(indivLik);
569  vectLiks[q] = indivLik;
570  if (indivLik > maxLik)
571  { // Keep the maximum value:
572  maxLikDraw = drawnSample;
573  maxLik = indivLik;
574  }
575  }
576 
577  // This is done to avoid floating point overflow!!
578  // average_lik = \sum(e^liks) * e^maxLik / N
579  // log( average_lik ) = log( \sum(e^liks) ) + maxLik - log( N )
580  double avrgLogLik = math::averageLogLikelihood(vectLiks);
581 
582  // Save into the object:
583  myObj->m_pfAuxiliaryPFStandard_estimatedProb[index] =
584  avrgLogLik; // log( accum / N );
585 
586  myObj->m_pfAuxiliaryPFOptimal_maxLikelihood[index] = maxLik;
587  if (PF_options.pfAuxFilterOptimal_MLE)
588  myObj->m_pfAuxiliaryPFOptimal_maxLikDrawnMovement[index] =
589  maxLikDraw.asTPose();
590 
591  // and compute the resulting probability of this particle:
592  // ------------------------------------------------------------
593  return cur_logweight +
594  myObj->m_pfAuxiliaryPFOptimal_estimatedProb[index];
595  }
596  MRPT_END
597 }
598 
599 // USE_OPTIMAL_SAMPLING:
600 // true -> PF_SLAM_implementation_pfAuxiliaryPFOptimal
601 // false -> PF_SLAM_implementation_pfAuxiliaryPFStandard
602 template <
603  class PARTICLE_TYPE, class MYSELF,
605 template <class BINTYPE>
608  const mrpt::obs::CActionCollection* actions,
609  const mrpt::obs::CSensoryFrame* sf,
611  const TKLDParams& KLD_options, const bool USE_OPTIMAL_SAMPLING)
612 {
613  MRPT_START
614  using TSetStateSpaceBins = std::set<BINTYPE, typename BINTYPE::lt_operator>;
615 
616  auto* me = static_cast<MYSELF*>(this);
617 
618  const size_t M = me->m_particles.size();
619 
620  // ----------------------------------------------------------------------
621  // We can execute optimal PF only when we have both, an action, and
622  // a valid observation from which to compute the likelihood:
623  // Accumulate odometry/actions until we have a valid observation, then
624  // process them simultaneously.
625  // ----------------------------------------------------------------------
626  if (!PF_SLAM_implementation_gatherActionsCheckBothActObs<BINTYPE>(
627  actions, sf))
628  return; // Nothing we can do here...
629  // OK, we have m_movementDrawer loaded and observations...let's roll!
630 
631  // -------------------------------------------------------------------------------
632  // 0) Common part: Prepare m_particles "draw" and compute
633  //"fastDrawSample"
634  // -------------------------------------------------------------------------------
635  // We need the (aproximate) maximum likelihood value for each
636  // previous particle [i]:
637  // max{ p( z^t | data^[i], x_(t-1)^[i], u_(t) ) }
638  //
639 
640  m_pfAuxiliaryPFOptimal_maxLikelihood.assign(M, INVALID_LIKELIHOOD_VALUE);
641  m_pfAuxiliaryPFOptimal_maxLikDrawnMovement.resize(M);
642  m_pfAuxiliaryPFOptimal_estimatedProb.resize(M);
643  m_pfAuxiliaryPFStandard_estimatedProb.resize(M);
644 
645  // Pass the "mean" robot movement to the "weights" computing function:
646  mrpt::poses::CPose3D meanRobotMovement;
647  m_movementDrawer.getSamplingMean3D(meanRobotMovement);
648 
649  // Prepare data for executing "fastDrawSample"
651  auto funcOpt =
652  &TMyClass::template PF_SLAM_particlesEvaluator_AuxPFOptimal<BINTYPE>;
653  auto funcStd =
654  &TMyClass::template PF_SLAM_particlesEvaluator_AuxPFStandard<BINTYPE>;
655 
656  me->prepareFastDrawSample(
657  PF_options, USE_OPTIMAL_SAMPLING ? funcOpt : funcStd,
658  &meanRobotMovement, sf);
659 
660  // For USE_OPTIMAL_SAMPLING=1, m_pfAuxiliaryPFOptimal_maxLikelihood is now
661  // computed.
662 
663  if (USE_OPTIMAL_SAMPLING &&
664  me->isLoggingLevelVisible(mrpt::system::LVL_DEBUG))
665  {
666  me->logStr(
668  mrpt::format(
669  "[prepareFastDrawSample] max (log) = %10.06f\n",
670  math::maximum(m_pfAuxiliaryPFOptimal_estimatedProb)));
671  me->logStr(
673  mrpt::format(
674  "[prepareFastDrawSample] max-mean (log) = %10.06f\n",
675  -math::mean(m_pfAuxiliaryPFOptimal_estimatedProb) +
676  math::maximum(m_pfAuxiliaryPFOptimal_estimatedProb)));
677  me->logStr(
679  mrpt::format(
680  "[prepareFastDrawSample] max-min (log) = %10.06f\n",
681  -math::minimum(m_pfAuxiliaryPFOptimal_estimatedProb) +
682  math::maximum(m_pfAuxiliaryPFOptimal_estimatedProb)));
683  }
684 
685  // Now we have the vector "m_fastDrawProbability" filled out with:
686  // w[i]*p(zt|z^{t-1},x^{[i],t-1},X)
687  // where,
688  //
689  // =========== For USE_OPTIMAL_SAMPLING = true ====================
690  // X is the robot pose prior (as implemented in
691  // the aux. function "PF_SLAM_particlesEvaluator_AuxPFOptimal"),
692  // and also the "m_pfAuxiliaryPFOptimal_maxLikelihood" filled with the
693  // maximum lik. values.
694  //
695  // =========== For USE_OPTIMAL_SAMPLING = false ====================
696  // X is a single point close to the mean of the robot pose prior (as
697  // implemented in
698  // the aux. function "PF_SLAM_particlesEvaluator_AuxPFStandard").
699  //
700  std::vector<mrpt::math::TPose3D> newParticles;
701  std::vector<double> newParticlesWeight;
702  std::vector<size_t> newParticlesDerivedFromIdx;
703 
704  // We need the (aproximate) maximum likelihood value for each
705  // previous particle [i]:
706  //
707  // max{ p( z^t | data^[i], x_(t-1)^[i], u_(t) ) }
708  //
709  if (PF_options.pfAuxFilterOptimal_MLE)
710  m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed.assign(M, false);
711 
712  const double maxMeanLik = math::maximum(
713  USE_OPTIMAL_SAMPLING ? m_pfAuxiliaryPFOptimal_estimatedProb
714  : m_pfAuxiliaryPFStandard_estimatedProb);
715 
716  if (!PF_options.adaptiveSampleSize)
717  {
718  // ----------------------------------------------------------------------
719  // 1) FIXED SAMPLE SIZE VERSION
720  // ----------------------------------------------------------------------
721  newParticles.resize(M);
722  newParticlesWeight.resize(M);
723  newParticlesDerivedFromIdx.resize(M);
724 
725  const bool doResample = me->ESS() < PF_options.BETA;
726 
727  for (size_t i = 0; i < M; i++)
728  {
729  size_t k;
730 
731  // Generate a new particle:
732  // (a) Draw a "t-1" m_particles' index:
733  // ----------------------------------------------------------------
734  if (doResample)
735  k = me->fastDrawSample(
736  PF_options); // Based on weights of last step only!
737  else
738  k = i;
739 
740  // Do one rejection sampling step:
741  // ---------------------------------------------
742  mrpt::poses::CPose3D newPose;
743  double newParticleLogWeight = 0;
744  PF_SLAM_aux_perform_one_rejection_sampling_step<BINTYPE>(
745  USE_OPTIMAL_SAMPLING, doResample, maxMeanLik, k, sf, PF_options,
746  newPose, newParticleLogWeight);
747 
748  // Insert the new particle
749  newParticles[i] = newPose.asTPose();
750  newParticlesDerivedFromIdx[i] = k;
751  newParticlesWeight[i] = newParticleLogWeight;
752 
753  } // for i
754  } // end fixed sample size
755  else
756  {
757  // -------------------------------------------------------------------------------------------------
758  // 2) ADAPTIVE SAMPLE SIZE VERSION
759  //
760  // Implementation of Dieter Fox's KLD algorithm
761  // JLBC (3/OCT/2006)
762  // -------------------------------------------------------------------------------------------------
763  // The new particle set:
764  newParticles.clear();
765  newParticlesWeight.resize(0);
766  newParticlesDerivedFromIdx.clear();
767 
768  // ------------------------------------------------------------------------------
769  // 2.1) PRELIMINARY STAGE: Build a list of
770  // pairs<TPathBin,std::vector<uint32_t>>
771  // with the
772  // indexes of m_particles that fall into each
773  // multi-dimensional-path bins
774  // //The bins will be saved into "stateSpaceBinsLastTimestep", and
775  // the list
776  // //of corresponding m_particles (in the last timestep), in
777  // "stateSpaceBinsLastTimestepParticles"
778  // - Added JLBC (01/DEC/2006)
779  // ------------------------------------------------------------------------------
780  TSetStateSpaceBins stateSpaceBinsLastTimestep;
781  std::vector<std::vector<uint32_t>> stateSpaceBinsLastTimestepParticles;
782  typename MYSELF::CParticleList::iterator partIt;
783  unsigned int partIndex;
784 
785  me->logStr(mrpt::system::LVL_DEBUG, "[FIXED_SAMPLING] Computing...");
786  for (partIt = me->m_particles.begin(), partIndex = 0;
787  partIt != me->m_particles.end(); ++partIt, ++partIndex)
788  {
789  // Load the bin from the path data:
790  const PARTICLE_TYPE* part;
791  if constexpr (
793  part = partIt->d.get();
794  else
795  part = &partIt->d;
796 
797  BINTYPE p;
798  KLF_loadBinFromParticle<PARTICLE_TYPE, BINTYPE>(
799  p, KLD_options, part);
800 
801  // Is it a new bin?
802  auto posFound = stateSpaceBinsLastTimestep.find(p);
803  if (posFound == stateSpaceBinsLastTimestep.end())
804  { // Yes, create a new pair <bin,index_list> in the list:
805  stateSpaceBinsLastTimestep.insert(p);
806  stateSpaceBinsLastTimestepParticles.emplace_back(1, partIndex);
807  }
808  else
809  { // No, add the particle's index to the existing entry:
810  const size_t idx =
811  std::distance(stateSpaceBinsLastTimestep.begin(), posFound);
812  stateSpaceBinsLastTimestepParticles[idx].push_back(partIndex);
813  }
814  }
815  me->logStr(
817  mrpt::format(
818  "[FIXED_SAMPLING] done (%u bins in t-1)\n",
819  (unsigned int)stateSpaceBinsLastTimestep.size()));
820 
821  // ------------------------------------------------------------------------------
822  // 2.2) THE MAIN KLD-BASED DRAW SAMPLING LOOP
823  // ------------------------------------------------------------------------------
824  double delta_1 = 1.0 - KLD_options.KLD_delta;
825  double epsilon_1 = 0.5 / KLD_options.KLD_epsilon;
826  bool doResample = me->ESS() < PF_options.BETA;
827  // double maxLik =
828  // math::maximum(m_pfAuxiliaryPFOptimal_maxLikelihood);
829  // // For normalization purposes only
830 
831  // The desired dynamic number of m_particles (to be modified dynamically
832  // below):
833  const size_t minNumSamples_KLD = std::max(
834  (size_t)KLD_options.KLD_minSampleSize,
835  (size_t)round(
836  KLD_options.KLD_minSamplesPerBin *
837  stateSpaceBinsLastTimestep.size()));
838  size_t Nx = minNumSamples_KLD;
839 
840  const size_t Np1 = me->m_particles.size();
841  std::vector<size_t> oldPartIdxsStillNotPropragated(
842  Np1); // Use a list since we'll use "erase" a lot here.
843  for (size_t k = 0; k < Np1; k++)
844  oldPartIdxsStillNotPropragated[k] = k; //.push_back(k);
845 
846  const size_t Np = stateSpaceBinsLastTimestepParticles.size();
847  std::vector<size_t> permutationPathsAuxVector(Np);
848  for (size_t k = 0; k < Np; k++) permutationPathsAuxVector[k] = k;
849 
850  // Instead of picking randomly from "permutationPathsAuxVector", we can
851  // shuffle it now just once,
852  // then pick in sequence from the tail and resize the container:
854  permutationPathsAuxVector.begin(), permutationPathsAuxVector.end());
855 
856  size_t k = 0;
857  size_t N = 0;
858 
859  TSetStateSpaceBins stateSpaceBins;
860 
861  do // "N" is the index of the current "new particle":
862  {
863  // Generate a new particle:
864  //
865  // (a) Propagate the last set of m_particles, and only if the
866  // desired number of m_particles in this step is larger,
867  // perform a UNIFORM sampling from the last set. In this way
868  // the new weights can be computed in the same way for all
869  // m_particles.
870  // ---------------------------------------------------------------------------
871  if (doResample)
872  {
873  k = me->fastDrawSample(
874  PF_options); // Based on weights of last step only!
875  }
876  else
877  {
878  // Assure that at least one particle per "discrete-path" is
879  // taken (if the
880  // number of samples allows it)
881  if (permutationPathsAuxVector.size())
882  {
883  const size_t idxBinSpacePath =
884  *permutationPathsAuxVector.rbegin();
885  permutationPathsAuxVector.resize(
886  permutationPathsAuxVector.size() - 1);
887 
888  const size_t idx =
890  stateSpaceBinsLastTimestepParticles[idxBinSpacePath]
891  .size();
892  k = stateSpaceBinsLastTimestepParticles[idxBinSpacePath]
893  [idx];
894  ASSERT_(k < me->m_particles.size());
895 
896  // Also erase it from the other permutation vector list:
897  oldPartIdxsStillNotPropragated.erase(std::find(
898  oldPartIdxsStillNotPropragated.begin(),
899  oldPartIdxsStillNotPropragated.end(), k));
900  }
901  else
902  {
903  // Select a particle from the previous set with a UNIFORM
904  // distribution,
905  // in such a way we will assign each particle the updated
906  // weight accounting
907  // for its last weight.
908  // The first "old_N" m_particles will be drawn using a
909  // uniform random selection
910  // without repetitions:
911  //
912  // Select a index from "oldPartIdxsStillNotPropragated" and
913  // remove it from the list:
914  if (oldPartIdxsStillNotPropragated.size())
915  {
916  const size_t idx =
918  .drawUniform32bit() %
919  oldPartIdxsStillNotPropragated.size();
920  auto it = oldPartIdxsStillNotPropragated.begin() +
921  idx; // advance(it,idx);
922  k = *it;
923  oldPartIdxsStillNotPropragated.erase(it);
924  }
925  else
926  {
927  // N>N_old -> Uniformly draw index:
929  .drawUniform32bit() %
930  me->m_particles.size();
931  }
932  }
933  }
934 
935  // Do one rejection sampling step:
936  // ---------------------------------------------
937  mrpt::poses::CPose3D newPose;
938  double newParticleLogWeight;
939  PF_SLAM_aux_perform_one_rejection_sampling_step<BINTYPE>(
940  USE_OPTIMAL_SAMPLING, doResample, maxMeanLik, k, sf, PF_options,
941  newPose, newParticleLogWeight);
942 
943  // Insert the new particle
944  newParticles.push_back(newPose.asTPose());
945  newParticlesDerivedFromIdx.push_back(k);
946  newParticlesWeight.push_back(newParticleLogWeight);
947 
948  // ----------------------------------------------------------------
949  // Now, the KLD-sampling dynamic sample size stuff:
950  // look if the particle's PATH falls into a new bin or not:
951  // ----------------------------------------------------------------
952  const PARTICLE_TYPE* part;
953  if constexpr (
955  part = me->m_particles[k].d.get();
956  else
957  part = &me->m_particles[k].d;
958 
959  BINTYPE p;
960  const mrpt::math::TPose3D newPose_s = newPose.asTPose();
961  KLF_loadBinFromParticle<PARTICLE_TYPE, BINTYPE>(
962  p, KLD_options, part, &newPose_s);
963 
964  // -----------------------------------------------------------------------------
965  // Look for the bin "p" into "stateSpaceBins": If it is not yet into
966  // the set,
967  // then we may increase the desired particle number:
968  // -----------------------------------------------------------------------------
969 
970  // Found?
971  if (stateSpaceBins.find(p) == stateSpaceBins.end())
972  {
973  // It falls into a new bin: add to the stateSpaceBins:
974  stateSpaceBins.insert(p);
975 
976  // K = K + 1
977  int K = stateSpaceBins.size();
978  if (K > 1)
979  {
980  // Update the number of m_particles!!
981  Nx = (size_t)(epsilon_1 * math::chi2inv(delta_1, K - 1));
982  // printf("k=%u \tn=%u \tNx:%u\n", k, newParticles.size(),
983  // Nx);
984  }
985  }
986 
987  N = newParticles.size();
988 
989  } while ((N < KLD_options.KLD_maxSampleSize &&
990  N < std::max(Nx, minNumSamples_KLD)) ||
991  (permutationPathsAuxVector.size() && !doResample));
992 
993  me->logStr(
995  mrpt::format(
996  "[ADAPTIVE SAMPLE SIZE] #Bins: %u \t #Particles: %u \t "
997  "Nx=%u\n",
998  static_cast<unsigned>(stateSpaceBins.size()),
999  static_cast<unsigned>(N), (unsigned)Nx));
1000  } // end adaptive sample size
1001 
1002  // ---------------------------------------------------------------------------------
1003  // Substitute old by new particle set:
1004  // Old are in "m_particles"
1005  // New are in "newParticles",
1006  // "newParticlesWeight","newParticlesDerivedFromIdx"
1007  // ---------------------------------------------------------------------------------
1008  this->PF_SLAM_implementation_replaceByNewParticleSet(
1009  me->m_particles, newParticles, newParticlesWeight,
1010  newParticlesDerivedFromIdx);
1011 
1012  // In this PF_algorithm, we must do weight normalization by ourselves:
1013  me->normalizeWeights();
1014 
1015  MRPT_END
1016 } // end of PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal
1017 
1018 /* ------------------------------------------------------------------------
1019  PF_SLAM_aux_perform_one_rejection_sampling_step
1020  ------------------------------------------------------------------------ */
1021 template <
1022  class PARTICLE_TYPE, class MYSELF,
1024 template <class BINTYPE>
1027  const bool USE_OPTIMAL_SAMPLING, const bool doResample,
1028  const double maxMeanLik,
1029  size_t k, // The particle from the old set "m_particles[]"
1030  const mrpt::obs::CSensoryFrame* sf,
1032  mrpt::poses::CPose3D& out_newPose, double& out_newParticleLogWeight)
1033 {
1034  auto* me = static_cast<MYSELF*>(this);
1035 
1036  // ADD-ON: If the 'm_pfAuxiliaryPFOptimal_estimatedProb[k]' is
1037  // **extremelly** low relative to the other m_particles,
1038  // resample only this particle with a copy of another one, uniformly:
1039  while (((USE_OPTIMAL_SAMPLING ? m_pfAuxiliaryPFOptimal_estimatedProb[k]
1040  : m_pfAuxiliaryPFStandard_estimatedProb[k]) -
1041  maxMeanLik) < -PF_options.max_loglikelihood_dyn_range)
1042  {
1043  // Select another 'k' uniformly:
1045  me->m_particles.size();
1046  me->logStr(
1048  "[PF_SLAM_aux_perform_one_rejection_sampling_step] Warning: "
1049  "Discarding very unlikely particle.");
1050  }
1051 
1052  bool pose_is_valid;
1054  getLastPose(k, pose_is_valid)); // current pose of the k'th particle
1055  // ASSERT_(pose_is_valid); Use the default (0,0,0) if path is empty.
1056 
1057  // (b) Rejection-sampling: Draw a new robot pose from x[k],
1058  // and accept it with probability p(zk|x) / maxLikelihood:
1059  // ----------------------------------------------------------------
1060  double poseLogLik;
1061  if (PF_SLAM_implementation_skipRobotMovement())
1062  {
1063  // The first robot pose in the SLAM execution: All m_particles start
1064  // at the same point (this is the lowest bound of subsequent
1065  // uncertainty):
1066  out_newPose = oldPose;
1067  poseLogLik = 0;
1068  }
1069  else
1070  {
1071  mrpt::poses::CPose3D movementDraw;
1072  if (!USE_OPTIMAL_SAMPLING)
1073  { // APF:
1074  m_movementDrawer.drawSample(movementDraw);
1075  out_newPose.composeFrom(
1076  oldPose, movementDraw); // newPose = oldPose + movementDraw;
1077  // Compute likelihood:
1078  poseLogLik = PF_SLAM_computeObservationLikelihoodForParticle(
1079  PF_options, k, *sf, out_newPose);
1080  }
1081  else
1082  { // Optimal APF with rejection sampling:
1083  // Rejection-sampling:
1084  double acceptanceProb;
1085  int timeout = 0;
1086  const int maxTries = 10000;
1087  double bestTryByNow_loglik = -std::numeric_limits<double>::max();
1088  mrpt::math::TPose3D bestTryByNow_pose;
1089  do
1090  {
1091  // Draw new robot pose:
1092  if (PF_options.pfAuxFilterOptimal_MLE &&
1093  !m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed[k])
1094  { // No! first take advantage of a good drawn value, but only
1095  // once!!
1096  m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed[k] =
1097  true;
1098  movementDraw = mrpt::poses::CPose3D(
1099  m_pfAuxiliaryPFOptimal_maxLikDrawnMovement[k]);
1100  }
1101  else
1102  {
1103  // Draw new robot pose:
1104  m_movementDrawer.drawSample(movementDraw);
1105  }
1106 
1107  out_newPose.composeFrom(
1108  oldPose,
1109  movementDraw); // out_newPose = oldPose + movementDraw;
1110 
1111  // Compute acceptance probability:
1112  poseLogLik = PF_SLAM_computeObservationLikelihoodForParticle(
1113  PF_options, k, *sf, out_newPose);
1114 
1115  if (poseLogLik > bestTryByNow_loglik)
1116  {
1117  bestTryByNow_loglik = poseLogLik;
1118  bestTryByNow_pose = out_newPose.asTPose();
1119  }
1120 
1121  double ratioLikLik = std::exp(
1122  poseLogLik - m_pfAuxiliaryPFOptimal_maxLikelihood[k]);
1123  acceptanceProb = std::min(1.0, ratioLikLik);
1124 
1125  if (ratioLikLik > 1)
1126  {
1127  m_pfAuxiliaryPFOptimal_maxLikelihood[k] =
1128  poseLogLik; // :'-( !!!
1129  // acceptanceProb = 0; // Keep searching or keep this
1130  // one?
1131  }
1132  } while (
1133  ++timeout < maxTries &&
1134  acceptanceProb <
1135  mrpt::random::getRandomGenerator().drawUniform(0.0, 0.999));
1136 
1137  if (timeout >= maxTries)
1138  {
1139  out_newPose = mrpt::poses::CPose3D(bestTryByNow_pose);
1140  poseLogLik = bestTryByNow_loglik;
1141  me->logStr(
1143  "[PF_implementation] Warning: timeout in rejection "
1144  "sampling.");
1145  }
1146  }
1147 
1148  // And its weight:
1149  if (USE_OPTIMAL_SAMPLING)
1150  { // Optimal PF
1151  if (doResample)
1152  out_newParticleLogWeight = 0; // By definition of our optimal
1153  // PF, all samples have identical
1154  // weights.
1155  else
1156  {
1157  const double weightFact =
1158  m_pfAuxiliaryPFOptimal_estimatedProb[k] *
1159  PF_options.powFactor;
1160  out_newParticleLogWeight =
1161  me->m_particles[k].log_w + weightFact;
1162  }
1163  }
1164  else
1165  { // APF:
1166  const double weightFact =
1167  (poseLogLik - m_pfAuxiliaryPFStandard_estimatedProb[k]) *
1168  PF_options.powFactor;
1169  if (doResample)
1170  out_newParticleLogWeight = weightFact;
1171  else
1172  out_newParticleLogWeight =
1173  weightFact + me->m_particles[k].log_w;
1174  }
1175  }
1176  // Done.
1177 }
1178 } // 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:775
#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.
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3727
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...
GLsizei GLsizei GLuint * obj
Definition: glext.h:4085
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:572
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
GLuint index
Definition: glext.h:4068
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
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)
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:58
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...
T::Ptr getActionByClass(const size_t &ith=0) const
Access to the i&#39;th action of a given class, or a nullptr smart pointer if there is no action of that ...
particle_storage_mode
use for CProbabilityParticle
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
GLfloat GLfloat p
Definition: glext.h:6398
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1891
bool adaptiveSampleSize
A flag that indicates whether the CParticleFilterCapable object should perform adative sample size (d...
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
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 1.9.9 Git: 1d7b2c899 Thu Nov 14 18:59:07 2019 +0100 at jue nov 14 19:15:09 CET 2019