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 
13 #include <mrpt/bayes/CParticleFilterCapable.h>
14 #include <mrpt/bayes/CParticleFilterData.h>
15 #include <mrpt/random.h>
16 #include <mrpt/obs/CActionCollection.h>
17 #include <mrpt/obs/CActionRobotMovement3D.h>
18 #include <mrpt/obs/CActionRobotMovement2D.h>
19 #include <mrpt/slam/TKLDParams.h>
20 
21 #include <mrpt/math/distributions.h> // chi2inv
22 #include <mrpt/math/data_utils.h> // averageLogLikelihood()
23 
24 #include <mrpt/slam/PF_implementations_data.h>
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,
47  mrpt::bayes::particle_storage_mode STORAGE>
48 template <class BINTYPE>
49 bool PF_implementation<PARTICLE_TYPE, MYSELF, STORAGE>::
50  PF_SLAM_implementation_gatherActionsCheckBothActObs(
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  {
58  mrpt::obs::CActionRobotMovement2D::Ptr robotMovement2D =
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  {
81  mrpt::obs::CActionRobotMovement3D::Ptr robotMovement3D =
82  actions->getActionByClass<mrpt::obs::CActionRobotMovement3D>();
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,
155  mrpt::bayes::particle_storage_mode STORAGE>
156 template <class BINTYPE>
157 void PF_implementation<PARTICLE_TYPE, MYSELF, STORAGE>::
158  PF_SLAM_implementation_pfAuxiliaryPFOptimal(
159  const mrpt::obs::CActionCollection* actions,
160  const mrpt::obs::CSensoryFrame* sf,
161  const mrpt::bayes::CParticleFilter::TParticleFilterOptions& PF_options,
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,
178  mrpt::bayes::particle_storage_mode STORAGE>
179 template <class BINTYPE>
180 void PF_implementation<PARTICLE_TYPE, MYSELF, STORAGE>::
181  PF_SLAM_implementation_pfStandardProposal(
182  const mrpt::obs::CActionCollection* actions,
183  const mrpt::obs::CSensoryFrame* sf,
184  const mrpt::bayes::CParticleFilter::TParticleFilterOptions& PF_options,
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  {
205  mrpt::obs::CActionRobotMovement2D::Ptr robotMovement2D =
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  {
218  mrpt::obs::CActionRobotMovement3D::Ptr robotMovement3D =
219  actions
220  ->getActionByClass<mrpt::obs::CActionRobotMovement3D>();
221  if (robotMovement3D)
222  {
223  m_movementDrawer.setPosePDF(robotMovement3D->poseChange);
224  robotMovement3D->poseChange.getMean(motionModelMeanIncr);
225  }
226  else
227  {
228  THROW_EXCEPTION(
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 =
303  mrpt::poses::CPose3D(
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,
400  mrpt::bayes::particle_storage_mode STORAGE>
401 template <class BINTYPE>
402 void PF_implementation<PARTICLE_TYPE, MYSELF, STORAGE>::
403  PF_SLAM_implementation_pfAuxiliaryPFStandard(
404  const mrpt::obs::CActionCollection* actions,
405  const mrpt::obs::CSensoryFrame* sf,
406  const mrpt::bayes::CParticleFilter::TParticleFilterOptions& PF_options,
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,
419  mrpt::bayes::particle_storage_mode STORAGE>
420 template <class BINTYPE>
421 double PF_implementation<PARTICLE_TYPE, MYSELF, STORAGE>::
422  PF_SLAM_particlesEvaluator_AuxPFOptimal(
423  const mrpt::bayes::CParticleFilter::TParticleFilterOptions& PF_options,
424  const mrpt::bayes::CParticleFilterCapable* obj, size_t index,
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,
498  mrpt::bayes::particle_storage_mode STORAGE>
499 template <class BINTYPE>
500 double PF_implementation<PARTICLE_TYPE, MYSELF, STORAGE>::
501  PF_SLAM_particlesEvaluator_AuxPFStandard(
502  const mrpt::bayes::CParticleFilter::TParticleFilterOptions& PF_options,
503  const mrpt::bayes::CParticleFilterCapable* obj, size_t index,
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 
518  if (!PF_options.pfAuxFilterStandard_FirstStageWeightsMonteCarlo)
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,
600  mrpt::bayes::particle_storage_mode STORAGE>
601 template <class BINTYPE>
602 void PF_implementation<PARTICLE_TYPE, MYSELF, STORAGE>::
603  PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal(
604  const mrpt::obs::CActionCollection* actions,
605  const mrpt::obs::CSensoryFrame* sf,
606  const mrpt::bayes::CParticleFilter::TParticleFilterOptions& PF_options,
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"
646  using TMyClass = PF_implementation<PARTICLE_TYPE, MYSELF, STORAGE>;
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(
663  mrpt::system::LVL_DEBUG,
664  mrpt::format(
665  "[prepareFastDrawSample] max (log) = %10.06f\n",
666  math::maximum(m_pfAuxiliaryPFOptimal_estimatedProb)));
667  me->logStr(
668  mrpt::system::LVL_DEBUG,
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(
674  mrpt::system::LVL_DEBUG,
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(
812  mrpt::system::LVL_DEBUG,
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:
849  mrpt::random::shuffle(
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 =
885  mrpt::random::getRandomGenerator().drawUniform32bit() %
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 =
914  mrpt::random::getRandomGenerator()
915  .drawUniform32bit() %
916  oldPartIdxsStillNotPropragated.size();
917  std::vector<size_t>::iterator it =
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:
926  k = mrpt::random::getRandomGenerator()
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(
990  mrpt::system::LVL_DEBUG,
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,
1018  mrpt::bayes::particle_storage_mode STORAGE>
1019 template <class BINTYPE>
1020 void PF_implementation<PARTICLE_TYPE, MYSELF, STORAGE>::
1021  PF_SLAM_aux_perform_one_rejection_sampling_step(
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,
1026  const mrpt::bayes::CParticleFilter::TParticleFilterOptions& PF_options,
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:
1039  k = mrpt::random::getRandomGenerator().drawUniform32bit() %
1040  me->m_particles.size();
1041  me->logStr(
1042  mrpt::system::LVL_DEBUG,
1043  "[PF_SLAM_aux_perform_one_rejection_sampling_step] Warning: "
1044  "Discarding very unlikely particle.");
1045  }
1046 
1047  bool pose_is_valid;
1048  const mrpt::poses::CPose3D oldPose = mrpt::poses::CPose3D(
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(
1137  mrpt::system::LVL_WARN,
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
mrpt::system::COutputLogger::logStr
void logStr(const VerbosityLevel level, const std::string &msg_str) const
Main method to add the specified message string to the logger.
Definition: COutputLogger.cpp:61
mrpt::bayes::CParticleFilter::TParticleFilterOptions::adaptiveSampleSize
bool adaptiveSampleSize
A flag that indicates whether the CParticleFilterCapable object should perform adative sample size (d...
Definition: CParticleFilter.h:115
mrpt::poses::CPose3D::composeFrom
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
mrpt::bayes::CParticleFilter::TParticleFilterOptions::pfAuxFilterOptimal_MaximumSearchSamples
unsigned int pfAuxFilterOptimal_MaximumSearchSamples
In the algorithm "CParticleFilter::pfAuxiliaryPFOptimal" (and in "CParticleFilter::pfAuxiliaryPFStand...
Definition: CParticleFilter.h:129
mrpt::math::maximum
CONTAINER::Scalar maximum(const CONTAINER &v)
Definition: ops_containers.h:139
q
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3721
mrpt::slam::PF_implementation::PF_SLAM_implementation_pfAuxiliaryPFStandard
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...
Definition: PF_implementations.h:403
mrpt::obs::CActionCollection::getActionByClass
T::Ptr getActionByClass(const size_t &ith=0) const
Access to the i'th action of a given class, or a nullptr smart pointer if there is no action of that ...
Definition: CActionCollection.h:131
mrpt::math::dynamic_vector
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction.
Definition: eigen_frwds.h:44
mrpt::poses::CPose3DPDFGaussian::getMean
void getMean(CPose3D &mean_pose) const override
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF).
Definition: CPose3DPDFGaussian.h:91
mrpt::math::chi2inv
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
mrpt::slam::TKLDParams
Option set for KLD algorithm.
Definition: TKLDParams.h:20
mrpt::math::mean
double mean(const CONTAINER &v)
Computes the mean value of a vector.
Definition: ops_containers.h:234
CParticleFilterData.h
obj
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
mrpt::bayes::particle_storage_mode
particle_storage_mode
use for CProbabilityParticle
Definition: CProbabilityParticle.h:20
mrpt::obs::CActionRobotMovement3D
Represents a probabilistic 3D (6D) movement.
Definition: CActionRobotMovement3D.h:29
MRPT_UNUSED_PARAM
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
mrpt::containers::find
const_iterator find(const KEY &key) const
Definition: ts_hash_map.h:219
mrpt::slam::TKLDParams::KLD_minSamplesPerBin
double KLD_minSamplesPerBin
(Default: KLD_minSamplesPerBin=0) The minimum number of samples will be the maximum of KLD_minSampleS...
Definition: TKLDParams.h:45
data_utils.h
mrpt::bayes::CParticleFilter::TParticleFilterOptions::powFactor
double powFactor
An optional step to "smooth" dramatic changes in the observation model to affect the variance of the ...
Definition: CParticleFilter.h:133
mrpt::bayes::CParticleFilter::TParticleFilterOptions::max_loglikelihood_dyn_range
double max_loglikelihood_dyn_range
Only for PF_algorithm=pfAuxiliaryPFOptimal: If a given particle has a max_likelihood (from the a-prio...
Definition: CParticleFilter.h:148
mrpt::slam::TKLDParams::KLD_maxSampleSize
unsigned int KLD_maxSampleSize
Definition: TKLDParams.h:40
INVALID_LIKELIHOOD_VALUE
#define INVALID_LIKELIHOOD_VALUE
Definition: CParticleFilterCapable.h:20
PF_implementations_data.h
mrpt::obs::CActionCollection
Declares a class for storing a collection of robot actions.
Definition: CActionCollection.h:28
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:30
mrpt::math::distance
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1891
mrpt::slam::PF_implementation::PF_SLAM_particlesEvaluator_AuxPFStandard
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.
Definition: PF_implementations.h:501
mrpt::obs::CActionRobotMovement2D
Represents a probabilistic 2D movement of the robot mobile base.
Definition: CActionRobotMovement2D.h:32
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
p
GLfloat GLfloat p
Definition: glext.h:6305
mrpt::obs::CActionRobotMovement2D::Ptr
std::shared_ptr< CActionRobotMovement2D > Ptr
Definition: CActionRobotMovement2D.h:34
CActionCollection.h
MRPT_CHECK_NORMAL_NUMBER
#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
random.h
mrpt::slam::PF_implementation::PF_SLAM_particlesEvaluator_AuxPFOptimal
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)
Definition: PF_implementations.h:422
mrpt::obs::CSensoryFrame
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:54
CActionRobotMovement2D.h
mrpt::bayes::CParticleFilterCapable
This virtual class defines the interface that any particles based PDF class must implement in order t...
Definition: CParticleFilterCapable.h:33
mrpt::round
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:23
mrpt::slam::TKLDParams::KLD_epsilon
double KLD_epsilon
Definition: TKLDParams.h:34
mrpt::bayes::CParticleFilter::TParticleFilterOptions::pfAuxFilterStandard_FirstStageWeightsMonteCarlo
bool pfAuxFilterStandard_FirstStageWeightsMonteCarlo
Only for PF_algorithm==pfAuxiliaryPFStandard: If false, the APF will predict the first stage weights ...
Definition: CParticleFilter.h:159
MRPT_START
#define MRPT_START
Definition: exceptions.h:262
mrpt::format
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
mrpt::bayes::CParticleFilter::TParticleFilterOptions::BETA
double BETA
The resampling of particles will be performed when ESS (in range [0,1]) < BETA (default is 0....
Definition: CParticleFilter.h:118
mrpt::random::shuffle
void shuffle(RandomIt first, RandomIt last, URBG &&g)
Uniform shuffle a sequence.
Definition: random_shuffle.h:23
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
index
GLuint index
Definition: glext.h:4054
mrpt::poses::CPose3D::asTPose
mrpt::math::TPose3D asTPose() const
Definition: CPose3D.cpp:1046
CActionRobotMovement3D.h
distributions.h
mrpt::slam::PF_implementation
A set of common data shared by PF implementations for both SLAM and localization.
Definition: PF_implementations_data.h:40
mrpt::system::LVL_WARN
@ LVL_WARN
Definition: system/COutputLogger.h:32
mrpt::math::TPose3D
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: lightweight_geom_data.h:603
mrpt::slam::PF_implementation::PF_SLAM_implementation_gatherActionsCheckBothActObs
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,...
Definition: PF_implementations.h:50
mrpt::slam::TKLDParams::KLD_delta
double KLD_delta
Definition: TKLDParams.h:34
CParticleFilterCapable.h
mrpt::slam::TKLDParams::KLD_minSampleSize
unsigned int KLD_minSampleSize
Parameters for the KLD adaptive sample size algorithm (see Dieter Fox's papers), which is used only i...
Definition: TKLDParams.h:40
mrpt::obs::CActionCollection::getBestMovementEstimation
CActionRobotMovement2D::Ptr getBestMovementEstimation() const
Returns the best pose increment estimator in the collection, based on the determinant of its pose cha...
Definition: CActionCollection.cpp:93
TKLDParams.h
mrpt::obs::CActionRobotMovement3D::poseChange
mrpt::poses::CPose3DPDFGaussian poseChange
The 3D pose change probabilistic estimation.
Definition: CActionRobotMovement3D.h:49
mrpt::slam::PF_implementation::PF_SLAM_implementation_pfAuxiliaryPFOptimal
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...
Definition: PF_implementations.h:158
mrpt::system::LVL_DEBUG
@ LVL_DEBUG
Definition: system/COutputLogger.h:30
min
#define min(a, b)
Definition: rplidar_driver.cpp:42
mrpt::random::getRandomGenerator
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:19
MRPT_END
#define MRPT_END
Definition: exceptions.h:266
mrpt::slam::PF_implementation::PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal
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...
Definition: PF_implementations.h:603
mrpt::bayes::particle_storage_mode::POINTER
@ POINTER
mrpt::obs::CActionRobotMovement3D::Ptr
std::shared_ptr< CActionRobotMovement3D > Ptr
Definition: CActionRobotMovement3D.h:31
mrpt::obs::CActionRobotMovement2D::poseChange
mrpt::containers::deepcopy_poly_ptr< mrpt::poses::CPosePDF::Ptr > poseChange
The 2D pose change probabilistic estimation.
Definition: CActionRobotMovement2D.h:49
iterator
Scalar * iterator
Definition: eigen_plugins.h:26
mrpt::obs::CActionRobotMovement2D::computeFromOdometry
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...
Definition: CActionRobotMovement2D.cpp:381
mrpt::bayes::CParticleFilter::TParticleFilterOptions::pfAuxFilterOptimal_MLE
bool pfAuxFilterOptimal_MLE
(Default=false) In the algorithm "CParticleFilter::pfAuxiliaryPFOptimal", if set to true,...
Definition: CParticleFilter.h:165
mrpt::math::minimum
CONTAINER::Scalar minimum(const CONTAINER &v)
Definition: ops_containers.h:144
mrpt::containers::deepcopy_poly_ptr::get_ptr
const T & get_ptr() const
Definition: deepcopy_poly_ptr.h:92
mrpt::slam::PF_implementation::PF_SLAM_aux_perform_one_rejection_sampling_step
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)
Definition: PF_implementations.h:1021
mrpt::math::averageLogLikelihood
double averageLogLikelihood(const CVectorDouble &logLikelihoods)
A numerically-stable method to compute average likelihood values with strongly different ranges (unwe...
Definition: math.cpp:291
mrpt::bayes::CParticleFilter::TParticleFilterOptions
The configuration of a particle filter.
Definition: CParticleFilter.h:102
mrpt::slam::PF_implementation::PF_SLAM_implementation_pfStandardProposal
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,...
Definition: PF_implementations.h:181
mrpt::random::CRandomGenerator::drawUniform32bit
uint32_t drawUniform32bit()
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm,...
Definition: RandomGenerator.cpp:31


Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at mié 12 jul 2023 10:03:34 CEST