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;
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:764
#define MRPT_START
Definition: exceptions.h:241
#define min(a, b)
const_iterator find(const KEY &key) const
Definition: ts_hash_map.h:213
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
#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:562
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:294
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:43
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...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:84
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 ...
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
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:23
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 ...
void logStr(const VerbosityLevel level, const std::string &msg_str) const
Main method to add the specified message string to the logger.
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:1889
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:23



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: a429c6f2f Mon Aug 19 15:45:44 2019 +0200 at lun ago 19 15:50:16 CEST 2019