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