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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019