Main MRPT website > C++ reference for MRPT 1.4.0
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-2016, 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 #include <mrpt/slam/link_pragmas.h>
27 #include <cstdio> // printf()
28 
29 
30 /** \file PF_implementations.h
31  * This file contains the implementations of the template members declared in mrpt::slam::PF_implementation
32  */
33 
34 namespace mrpt
35 {
36  namespace slam
37  {
38  /** Auxiliary method called by PF implementations: return true if we have both action & observation,
39  * otherwise, return false AND accumulate the odometry so when we have an observation we didn't lose a thing.
40  * On return=true, the "m_movementDrawer" member is loaded and ready to draw samples of the increment of pose since last step.
41  * This method is smart enough to accumulate CActionRobotMovement2D or CActionRobotMovement3D, whatever comes in.
42  * \ingroup mrpt_slam_grp
43  */
44  template <class PARTICLE_TYPE,class MYSELF>
45  template <class BINTYPE>
47  const mrpt::obs::CActionCollection * actions,
48  const mrpt::obs::CSensoryFrame * sf )
49  {
50  MYSELF *me = static_cast<MYSELF*>(this);
51 
52  if (actions!=NULL) // A valid action?
53  {
54  {
55  mrpt::obs::CActionRobotMovement2DPtr robotMovement2D = actions->getBestMovementEstimation();
56  if (robotMovement2D.present())
57  {
58  if (m_accumRobotMovement3DIsValid) THROW_EXCEPTION("Mixing 2D and 3D actions is not allowed.")
59 
60  if (!m_accumRobotMovement2DIsValid)
61  { // First time:
62  robotMovement2D->poseChange->getMean( m_accumRobotMovement2D.rawOdometryIncrementReading );
63  m_accumRobotMovement2D.motionModelConfiguration = robotMovement2D->motionModelConfiguration;
64  }
65  else m_accumRobotMovement2D.rawOdometryIncrementReading += robotMovement2D->poseChange->getMeanVal();
66 
67  m_accumRobotMovement2DIsValid = true;
68  }
69  else // If there is no 2D action, look for a 3D action:
70  {
71  mrpt::obs::CActionRobotMovement3DPtr robotMovement3D = actions->getActionByClass<mrpt::obs::CActionRobotMovement3D>();
72  if (robotMovement3D)
73  {
74  if (m_accumRobotMovement2DIsValid) THROW_EXCEPTION("Mixing 2D and 3D actions is not allowed.")
75 
76  if (!m_accumRobotMovement3DIsValid)
77  m_accumRobotMovement3D = robotMovement3D->poseChange;
78  else m_accumRobotMovement3D += robotMovement3D->poseChange;
79  // This "+=" takes care of all the Jacobians, etc... You MUST love C++!!! ;-)
80 
81  m_accumRobotMovement3DIsValid = true;
82  }
83  else
84  return false; // We have no actions...
85  }
86  }
87  }
88 
89  const bool SFhasValidObservations = (sf==NULL) ? false : PF_SLAM_implementation_doWeHaveValidObservations(me->m_particles,sf);
90 
91  // All the things we need?
92  if (! ((m_accumRobotMovement2DIsValid || m_accumRobotMovement3DIsValid) && SFhasValidObservations))
93  return false;
94 
95  // Since we're gonna return true, load the pose-drawer:
96  // Take the accum. actions as input:
97  if (m_accumRobotMovement3DIsValid)
98  {
99  m_movementDrawer.setPosePDF( m_accumRobotMovement3D ); // <--- Set mov. drawer
100  m_accumRobotMovement3DIsValid = false; // Reset odometry for next iteration
101  }
102  else
103  {
104  mrpt::obs::CActionRobotMovement2D theResultingRobotMov;
105  theResultingRobotMov.computeFromOdometry(
106  m_accumRobotMovement2D.rawOdometryIncrementReading,
107  m_accumRobotMovement2D.motionModelConfiguration );
108 
109  m_movementDrawer.setPosePDF( theResultingRobotMov.poseChange ); // <--- Set mov. drawer
110  m_accumRobotMovement2DIsValid = false; // Reset odometry for next iteration
111  }
112  return true;
113  } // end of PF_SLAM_implementation_gatherActionsCheckBothActObs
114 
115  /** A generic implementation of the PF method "prediction_and_update_pfAuxiliaryPFOptimal" (optimal sampling with rejection sampling approximation),
116  * common to both localization and mapping.
117  *
118  * - BINTYPE: TPoseBin or whatever to discretize the sample space for KLD-sampling.
119  *
120  * This method implements optimal sampling with a rejection sampling-based approximation of the true posterior.
121  * For details, see the papers:
122  *
123  * J.-L. Blanco, J. Gonzalez, and J.-A. Fernandez-Madrigal,
124  * "An Optimal Filtering Algorithm for Non-Parametric Observation Models in
125  * Robot Localization," in Proc. IEEE International Conference on Robotics
126  * and Automation (ICRA'08), 2008, pp. 461466.
127  */
128  template <class PARTICLE_TYPE,class MYSELF>
129  template <class BINTYPE>
131  const mrpt::obs::CActionCollection * actions,
132  const mrpt::obs::CSensoryFrame * sf,
134  const TKLDParams &KLD_options)
135  {
136  // Standard and Optimal AuxiliaryPF actually have a shared implementation body:
137  PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal<BINTYPE>(actions,sf,PF_options,KLD_options, true /*Optimal PF*/ );
138  }
139 
140 
141  /** A generic implementation of the PF method "pfStandardProposal" (standard proposal distribution, that is, a simple SIS particle filter),
142  * common to both localization and mapping.
143  *
144  * - BINTYPE: TPoseBin or whatever to discretize the sample space for KLD-sampling.
145  */
146  template <class PARTICLE_TYPE,class MYSELF>
147  template <class BINTYPE>
149  const mrpt::obs::CActionCollection * actions,
150  const mrpt::obs::CSensoryFrame * sf,
152  const TKLDParams &KLD_options)
153  {
154  MRPT_START
155  typedef std::set<BINTYPE,typename BINTYPE::lt_operator> TSetStateSpaceBins;
156 
157  MYSELF *me = static_cast<MYSELF*>(this);
158 
159  // In this method we don't need the "PF_SLAM_implementation_gatherActionsCheckBothActObs" machinery,
160  // since prediction & update are two independent stages well separated for this algorithm.
161 
162  // --------------------------------------------------------------------------------------
163  // Prediction: Simply draw samples from the motion model
164  // --------------------------------------------------------------------------------------
165  if (actions)
166  {
167  // Find a robot movement estimation:
168  CPose3D motionModelMeanIncr;
169  {
170  mrpt::obs::CActionRobotMovement2DPtr robotMovement2D = actions->getBestMovementEstimation();
171  // If there is no 2D action, look for a 3D action:
172  if (robotMovement2D.present())
173  {
174  m_movementDrawer.setPosePDF( robotMovement2D->poseChange );
175  motionModelMeanIncr = robotMovement2D->poseChange->getMeanVal();
176  }
177  else
178  {
179  mrpt::obs::CActionRobotMovement3DPtr robotMovement3D = actions->getActionByClass<mrpt::obs::CActionRobotMovement3D>();
180  if (robotMovement3D)
181  {
182  m_movementDrawer.setPosePDF( robotMovement3D->poseChange );
183  robotMovement3D->poseChange.getMean( motionModelMeanIncr );
184  }
185  else { THROW_EXCEPTION("Action list does not contain any CActionRobotMovement2D or CActionRobotMovement3D object!"); }
186  }
187  }
188 
189  // Update particle poses:
190  if ( !PF_options.adaptiveSampleSize )
191  {
192  const size_t M = me->m_particles.size();
193  // -------------------------------------------------------------
194  // FIXED SAMPLE SIZE
195  // -------------------------------------------------------------
196  CPose3D incrPose;
197  for (size_t i=0;i<M;i++)
198  {
199  // Generate gaussian-distributed 2D-pose increments according to mean-cov:
200  m_movementDrawer.drawSample( incrPose );
201  CPose3D finalPose = CPose3D(*getLastPose(i)) + incrPose;
202 
203  // Update the particle with the new pose: this part is caller-dependant and must be implemented there:
204  PF_SLAM_implementation_custom_update_particle_with_new_pose( me->m_particles[i].d, TPose3D(finalPose) );
205  }
206  }
207  else
208  {
209  // -------------------------------------------------------------
210  // ADAPTIVE SAMPLE SIZE
211  // Implementation of Dieter Fox's KLD algorithm
212  // 31-Oct-2006 (JLBC): First version
213  // 19-Jan-2009 (JLBC): Rewriten within a generic template
214  // -------------------------------------------------------------
215  TSetStateSpaceBins stateSpaceBins;
216 
217  size_t Nx = KLD_options.KLD_minSampleSize;
218  const double delta_1 = 1.0 - KLD_options.KLD_delta;
219  const double epsilon_1 = 0.5 / KLD_options.KLD_epsilon;
220 
221  // Prepare data for executing "fastDrawSample"
222  me->prepareFastDrawSample(PF_options);
223 
224  // The new particle set:
225  std::vector<TPose3D> newParticles;
226  std::vector<double> newParticlesWeight;
227  std::vector<size_t> newParticlesDerivedFromIdx;
228 
229  CPose3D increment_i;
230  size_t N = 1;
231 
232  do // THE MAIN DRAW SAMPLING LOOP
233  {
234  // Draw a robot movement increment:
235  m_movementDrawer.drawSample( increment_i );
236 
237  // generate the new particle:
238  const size_t drawn_idx = me->fastDrawSample(PF_options);
239  const mrpt::poses::CPose3D newPose = CPose3D(*getLastPose(drawn_idx)) + increment_i;
240  const TPose3D newPose_s = newPose;
241 
242  // Add to the new particles list:
243  newParticles.push_back( newPose_s );
244  newParticlesWeight.push_back(0);
245  newParticlesDerivedFromIdx.push_back(drawn_idx);
246 
247  // Now, look if the particle falls in a new bin or not:
248  // --------------------------------------------------------
249  BINTYPE p;
250  KLF_loadBinFromParticle<PARTICLE_TYPE,BINTYPE>(p,KLD_options, me->m_particles[drawn_idx].d, &newPose_s);
251 
252  if (stateSpaceBins.find( p )==stateSpaceBins.end())
253  {
254  // It falls into a new bin:
255  // Add to the stateSpaceBins:
256  stateSpaceBins.insert( p );
257 
258  // K = K + 1
259  size_t K = stateSpaceBins.size();
260  if ( K>1) //&& newParticles.size() > options.KLD_minSampleSize )
261  {
262  // Update the number of m_particles!!
263  Nx = round(epsilon_1 * math::chi2inv(delta_1,K-1));
264  //printf("k=%u \tn=%u \tNx:%u\n", k, newParticles.size(), Nx);
265  }
266  }
267  N = newParticles.size();
268  } while ( N < max(Nx,(size_t)KLD_options.KLD_minSampleSize) &&
269  N < KLD_options.KLD_maxSampleSize );
270 
271  // ---------------------------------------------------------------------------------
272  // Substitute old by new particle set:
273  // Old are in "m_particles"
274  // New are in "newParticles", "newParticlesWeight","newParticlesDerivedFromIdx"
275  // ---------------------------------------------------------------------------------
276  this->PF_SLAM_implementation_replaceByNewParticleSet(
277  me->m_particles,
278  newParticles,newParticlesWeight,newParticlesDerivedFromIdx );
279 
280  } // end adaptive sample size
281  }
282 
283  if (sf)
284  {
285  const size_t M = me->m_particles.size();
286  // UPDATE STAGE
287  // ----------------------------------------------------------------------
288  // Compute all the likelihood values & update particles weight:
289  for (size_t i=0;i<M;i++)
290  {
291  const TPose3D *partPose= getLastPose(i); // Take the particle data:
292  CPose3D partPose2 = CPose3D(*partPose);
293  const double obs_log_likelihood = PF_SLAM_computeObservationLikelihoodForParticle(PF_options,i,*sf,partPose2);
294  me->m_particles[i].log_w += obs_log_likelihood * PF_options.powFactor;
295  } // for each particle "i"
296 
297  // Normalization of weights is done outside of this method automatically.
298  }
299 
300  MRPT_END
301  } // end of PF_SLAM_implementation_pfStandardProposal
302 
303  /** A generic implementation of the PF method "prediction_and_update_pfAuxiliaryPFStandard" (Auxiliary particle filter with the standard proposal),
304  * common to both localization and mapping.
305  *
306  * - BINTYPE: TPoseBin or whatever to discretize the sample space for KLD-sampling.
307  *
308  * This method is described in the paper:
309  * Pitt, M.K.; Shephard, N. (1999). "Filtering Via Simulation: Auxiliary Particle Filters".
310  * Journal of the American Statistical Association 94 (446): 590-591. doi:10.2307/2670179.
311  *
312  */
313  template <class PARTICLE_TYPE,class MYSELF>
314  template <class BINTYPE>
316  const mrpt::obs::CActionCollection * actions,
317  const mrpt::obs::CSensoryFrame * sf,
319  const TKLDParams &KLD_options)
320  {
321  // Standard and Optimal AuxiliaryPF actually have a shared implementation body:
322  PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal<BINTYPE>(actions,sf,PF_options,KLD_options, false /*APF*/ );
323  }
324 
325  /*---------------------------------------------------------------
326  PF_SLAM_particlesEvaluator_AuxPFOptimal
327  ---------------------------------------------------------------*/
328  template <class PARTICLE_TYPE,class MYSELF>
329  template <class BINTYPE>
333  size_t index,
334  const void *action,
335  const void *observation )
336  {
337  MRPT_UNUSED_PARAM(action);
338  MRPT_START
339 
340  //const PF_implementation<PARTICLE_TYPE,MYSELF> *myObj = reinterpret_cast<const PF_implementation<PARTICLE_TYPE,MYSELF>*>( obj );
341  const MYSELF *me = static_cast<const MYSELF*>(obj);
342 
343  // Compute the quantity:
344  // w[i]*p(zt|z^{t-1},x^{[i],t-1})
345  // As the Monte-Carlo approximation of the integral over all posible $x_t$.
346  // --------------------------------------------
347  double indivLik, maxLik= -1e300;
348  CPose3D maxLikDraw;
349  size_t N = PF_options.pfAuxFilterOptimal_MaximumSearchSamples;
350  ASSERT_(N>1)
351 
352  const mrpt::poses::CPose3D oldPose = *me->getLastPose(index);
353  CVectorDouble vectLiks(N,0); // The vector with the individual log-likelihoods.
354  CPose3D drawnSample;
355  for (size_t q=0;q<N;q++)
356  {
357  me->m_movementDrawer.drawSample(drawnSample);
358  CPose3D x_predict = oldPose + drawnSample;
359 
360  // Estimate the mean...
361  indivLik = me->PF_SLAM_computeObservationLikelihoodForParticle(
362  PF_options,
363  index,
364  *static_cast<const mrpt::obs::CSensoryFrame*>(observation),
365  x_predict );
366 
367  MRPT_CHECK_NORMAL_NUMBER(indivLik);
368  vectLiks[q] = indivLik;
369  if ( indivLik > maxLik )
370  { // Keep the maximum value:
371  maxLikDraw = drawnSample;
372  maxLik = indivLik;
373  }
374  }
375 
376  // This is done to avoid floating point overflow!!
377  // average_lik = \sum(e^liks) * e^maxLik / N
378  // log( average_lik ) = log( \sum(e^liks) ) + maxLik - log( N )
379  double avrgLogLik = math::averageLogLikelihood( vectLiks );
380 
381  // Save into the object:
382  me->m_pfAuxiliaryPFOptimal_estimatedProb[index] = avrgLogLik; // log( accum / N );
383  me->m_pfAuxiliaryPFOptimal_maxLikelihood[index] = maxLik;
384 
385  if (PF_options.pfAuxFilterOptimal_MLE)
386  me->m_pfAuxiliaryPFOptimal_maxLikDrawnMovement[index] = maxLikDraw;
387 
388  // and compute the resulting probability of this particle:
389  // ------------------------------------------------------------
390  return me->m_particles[index].log_w + me->m_pfAuxiliaryPFOptimal_estimatedProb[index];
391 
392  MRPT_END
393  } // end of PF_SLAM_particlesEvaluator_AuxPFOptimal
394 
395 
396  /** Compute w[i]*p(z_t | mu_t^i), with mu_t^i being
397  * the mean of the new robot pose
398  *
399  * \param action MUST be a "const CPose3D*"
400  * \param observation MUST be a "const CSensoryFrame*"
401  */
402  template <class PARTICLE_TYPE,class MYSELF>
403  template <class BINTYPE>
407  size_t index,
408  const void *action,
409  const void *observation )
410  {
411  MRPT_START
412 
413  //const PF_implementation<PARTICLE_TYPE,MYSELF> *myObj = reinterpret_cast<const PF_implementation<PARTICLE_TYPE,MYSELF>*>( obj );
414  const MYSELF *myObj = static_cast<const MYSELF*>(obj);
415 
416  // Take the previous particle weight:
417  const double cur_logweight = myObj->m_particles[index].log_w;
418  const mrpt::poses::CPose3D oldPose = *myObj->getLastPose(index);
419 
421  {
422  // Just use the mean:
423  // , take the mean of the posterior density:
424  CPose3D x_predict;
425  x_predict.composeFrom( oldPose, *static_cast<const CPose3D*>(action) );
426 
427  // and compute the obs. likelihood:
428  // --------------------------------------------
429  myObj->m_pfAuxiliaryPFStandard_estimatedProb[index] = myObj->PF_SLAM_computeObservationLikelihoodForParticle(
430  PF_options, index,
431  *static_cast<const mrpt::obs::CSensoryFrame*>(observation), x_predict );
432 
433  // Combined log_likelihood: Previous weight * obs_likelihood:
434  return cur_logweight + myObj->m_pfAuxiliaryPFStandard_estimatedProb[index];
435  }
436  else
437  {
438  // Do something similar to in Optimal sampling:
439  // Compute the quantity:
440  // w[i]*p(zt|z^{t-1},x^{[i],t-1})
441  // As the Monte-Carlo approximation of the integral over all posible $x_t$.
442  // --------------------------------------------
443  double indivLik, maxLik= -1e300;
444  CPose3D maxLikDraw;
445  size_t N = PF_options.pfAuxFilterOptimal_MaximumSearchSamples;
446  ASSERT_(N>1)
447 
448  CVectorDouble vectLiks(N,0); // The vector with the individual log-likelihoods.
449  CPose3D drawnSample;
450  for (size_t q=0;q<N;q++)
451  {
452  myObj->m_movementDrawer.drawSample(drawnSample);
453  CPose3D x_predict = oldPose + drawnSample;
454 
455  // Estimate the mean...
456  indivLik = myObj->PF_SLAM_computeObservationLikelihoodForParticle(
457  PF_options,
458  index,
459  *static_cast<const mrpt::obs::CSensoryFrame*>(observation),
460  x_predict );
461 
462  MRPT_CHECK_NORMAL_NUMBER(indivLik);
463  vectLiks[q] = indivLik;
464  if ( indivLik > maxLik )
465  { // Keep the maximum value:
466  maxLikDraw = drawnSample;
467  maxLik = indivLik;
468  }
469  }
470 
471  // This is done to avoid floating point overflow!!
472  // average_lik = \sum(e^liks) * e^maxLik / N
473  // log( average_lik ) = log( \sum(e^liks) ) + maxLik - log( N )
474  double avrgLogLik = math::averageLogLikelihood( vectLiks );
475 
476  // Save into the object:
477  myObj->m_pfAuxiliaryPFStandard_estimatedProb[index] = avrgLogLik; // log( accum / N );
478 
479  myObj->m_pfAuxiliaryPFOptimal_maxLikelihood[index] = maxLik;
480  if (PF_options.pfAuxFilterOptimal_MLE)
481  myObj->m_pfAuxiliaryPFOptimal_maxLikDrawnMovement[index] = maxLikDraw;
482 
483  // and compute the resulting probability of this particle:
484  // ------------------------------------------------------------
485  return cur_logweight + myObj->m_pfAuxiliaryPFOptimal_estimatedProb[index];
486  }
487  MRPT_END
488  }
489 
490  // USE_OPTIMAL_SAMPLING:
491  // true -> PF_SLAM_implementation_pfAuxiliaryPFOptimal
492  // false -> PF_SLAM_implementation_pfAuxiliaryPFStandard
493  template <class PARTICLE_TYPE,class MYSELF>
494  template <class BINTYPE>
496  const mrpt::obs::CActionCollection * actions,
497  const mrpt::obs::CSensoryFrame * sf,
499  const TKLDParams &KLD_options,
500  const bool USE_OPTIMAL_SAMPLING )
501  {
502  MRPT_START
503  typedef std::set<BINTYPE,typename BINTYPE::lt_operator> TSetStateSpaceBins;
504 
505  MYSELF *me = static_cast<MYSELF*>(this);
506 
507  const size_t M = me->m_particles.size();
508 
509  // ----------------------------------------------------------------------
510  // We can execute optimal PF only when we have both, an action, and
511  // a valid observation from which to compute the likelihood:
512  // Accumulate odometry/actions until we have a valid observation, then
513  // process them simultaneously.
514  // ----------------------------------------------------------------------
515  if (!PF_SLAM_implementation_gatherActionsCheckBothActObs<BINTYPE>(actions,sf))
516  return; // Nothing we can do here...
517  // OK, we have m_movementDrawer loaded and observations...let's roll!
518 
519 
520  // -------------------------------------------------------------------------------
521  // 0) Common part: Prepare m_particles "draw" and compute "fastDrawSample"
522  // -------------------------------------------------------------------------------
523  // We need the (aproximate) maximum likelihood value for each
524  // previous particle [i]:
525  // max{ p( z^t | data^[i], x_(t-1)^[i], u_(t) ) }
526  //
527 
528  m_pfAuxiliaryPFOptimal_maxLikelihood.assign(M, INVALID_LIKELIHOOD_VALUE);
529  m_pfAuxiliaryPFOptimal_maxLikDrawnMovement.resize(M);
530  m_pfAuxiliaryPFOptimal_estimatedProb.resize(M);
531  m_pfAuxiliaryPFStandard_estimatedProb.resize(M);
532 
533  // Pass the "mean" robot movement to the "weights" computing function:
534  CPose3D meanRobotMovement;
535  m_movementDrawer.getSamplingMean3D(meanRobotMovement);
536 
537  // Prepare data for executing "fastDrawSample"
538  typedef PF_implementation<PARTICLE_TYPE,MYSELF> TMyClass; // Use this longer declaration to avoid errors in old GCC.
539  CParticleFilterCapable::TParticleProbabilityEvaluator funcOpt = &TMyClass::template PF_SLAM_particlesEvaluator_AuxPFOptimal<BINTYPE>;
540  CParticleFilterCapable::TParticleProbabilityEvaluator funcStd = &TMyClass::template PF_SLAM_particlesEvaluator_AuxPFStandard<BINTYPE>;
541 
542  me->prepareFastDrawSample(
543  PF_options,
544  USE_OPTIMAL_SAMPLING ? funcOpt : funcStd,
545  &meanRobotMovement,
546  sf );
547 
548  // For USE_OPTIMAL_SAMPLING=1, m_pfAuxiliaryPFOptimal_maxLikelihood is now computed.
549 
550  if (USE_OPTIMAL_SAMPLING && PF_options.verbose)
551  {
552  printf("[prepareFastDrawSample] max (log) = %10.06f\n", math::maximum(m_pfAuxiliaryPFOptimal_estimatedProb) );
553  printf("[prepareFastDrawSample] max-mean (log) = %10.06f\n", -math::mean(m_pfAuxiliaryPFOptimal_estimatedProb) + math::maximum(m_pfAuxiliaryPFOptimal_estimatedProb) );
554  printf("[prepareFastDrawSample] max-min (log) = %10.06f\n", -math::minimum(m_pfAuxiliaryPFOptimal_estimatedProb) + math::maximum(m_pfAuxiliaryPFOptimal_estimatedProb) );
555  }
556 
557  // Now we have the vector "m_fastDrawProbability" filled out with:
558  // w[i]*p(zt|z^{t-1},x^{[i],t-1},X)
559  // where,
560  //
561  // =========== For USE_OPTIMAL_SAMPLING = true ====================
562  // X is the robot pose prior (as implemented in
563  // the aux. function "PF_SLAM_particlesEvaluator_AuxPFOptimal"),
564  // and also the "m_pfAuxiliaryPFOptimal_maxLikelihood" filled with the maximum lik. values.
565  //
566  // =========== For USE_OPTIMAL_SAMPLING = false ====================
567  // X is a single point close to the mean of the robot pose prior (as implemented in
568  // the aux. function "PF_SLAM_particlesEvaluator_AuxPFStandard").
569  //
570  vector<TPose3D> newParticles;
571  vector<double> newParticlesWeight;
572  vector<size_t> newParticlesDerivedFromIdx;
573 
574  // We need the (aproximate) maximum likelihood value for each
575  // previous particle [i]:
576  //
577  // max{ p( z^t | data^[i], x_(t-1)^[i], u_(t) ) }
578  //
579  if (PF_options.pfAuxFilterOptimal_MLE)
580  m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed.assign(M, false);
581 
582  const double maxMeanLik = math::maximum(
583  USE_OPTIMAL_SAMPLING ?
584  m_pfAuxiliaryPFOptimal_estimatedProb :
585  m_pfAuxiliaryPFStandard_estimatedProb );
586 
587  if ( !PF_options.adaptiveSampleSize )
588  {
589  // ----------------------------------------------------------------------
590  // 1) FIXED SAMPLE SIZE VERSION
591  // ----------------------------------------------------------------------
592  newParticles.resize(M);
593  newParticlesWeight.resize(M);
594  newParticlesDerivedFromIdx.resize(M);
595 
596  const bool doResample = me->ESS() < PF_options.BETA;
597 
598  for (size_t i=0;i<M;i++)
599  {
600  size_t k;
601 
602  // Generate a new particle:
603  // (a) Draw a "t-1" m_particles' index:
604  // ----------------------------------------------------------------
605  if (doResample)
606  k = me->fastDrawSample(PF_options); // Based on weights of last step only!
607  else k = i;
608 
609  // Do one rejection sampling step:
610  // ---------------------------------------------
611  CPose3D newPose;
612  double newParticleLogWeight;
613  PF_SLAM_aux_perform_one_rejection_sampling_step<BINTYPE>(
614  USE_OPTIMAL_SAMPLING,doResample,maxMeanLik,
615  k,
616  sf,PF_options,
617  newPose, newParticleLogWeight);
618 
619  // Insert the new particle
620  newParticles[i] = newPose;
621  newParticlesDerivedFromIdx[i] = k;
622  newParticlesWeight[i] = newParticleLogWeight;
623 
624  } // for i
625  } // end fixed sample size
626  else
627  {
628  // -------------------------------------------------------------------------------------------------
629  // 2) ADAPTIVE SAMPLE SIZE VERSION
630  //
631  // Implementation of Dieter Fox's KLD algorithm
632  // JLBC (3/OCT/2006)
633  // -------------------------------------------------------------------------------------------------
634  // The new particle set:
635  newParticles.clear();
636  newParticlesWeight.resize(0);
637  newParticlesDerivedFromIdx.clear();
638 
639  // ------------------------------------------------------------------------------
640  // 2.1) PRELIMINARY STAGE: Build a list of pairs<TPathBin,vector_uint> with the
641  // indexes of m_particles that fall into each multi-dimensional-path bins
642  // //The bins will be saved into "stateSpaceBinsLastTimestep", and the list
643  // //of corresponding m_particles (in the last timestep), in "stateSpaceBinsLastTimestepParticles"
644  // - Added JLBC (01/DEC/2006)
645  // ------------------------------------------------------------------------------
646  TSetStateSpaceBins stateSpaceBinsLastTimestep;
647  std::vector<vector_uint> stateSpaceBinsLastTimestepParticles;
648  typename MYSELF::CParticleList::iterator partIt;
649  unsigned int partIndex;
650 
651  printf( "[FIXED_SAMPLING] Computing...");
652  for (partIt = me->m_particles.begin(),partIndex=0; partIt!=me->m_particles.end(); ++partIt,++partIndex)
653  {
654  // Load the bin from the path data:
655  BINTYPE p;
656  KLF_loadBinFromParticle<PARTICLE_TYPE,BINTYPE>(p, KLD_options,partIt->d );
657 
658  // Is it a new bin?
659  typename TSetStateSpaceBins::iterator posFound=stateSpaceBinsLastTimestep.find(p);
660  if ( posFound == stateSpaceBinsLastTimestep.end() )
661  { // Yes, create a new pair <bin,index_list> in the list:
662  stateSpaceBinsLastTimestep.insert( p );
663  stateSpaceBinsLastTimestepParticles.push_back( vector_uint(1,partIndex) );
664  }
665  else
666  { // No, add the particle's index to the existing entry:
667  const size_t idx = std::distance(stateSpaceBinsLastTimestep.begin(),posFound);
668  stateSpaceBinsLastTimestepParticles[idx].push_back( partIndex );
669  }
670  }
671  printf( "done (%u bins in t-1)\n",(unsigned int)stateSpaceBinsLastTimestep.size());
672 
673  // ------------------------------------------------------------------------------
674  // 2.2) THE MAIN KLD-BASED DRAW SAMPLING LOOP
675  // ------------------------------------------------------------------------------
676  double delta_1 = 1.0 - KLD_options.KLD_delta;
677  double epsilon_1 = 0.5 / KLD_options.KLD_epsilon;
678  bool doResample = me->ESS() < 0.5;
679  //double maxLik = math::maximum(m_pfAuxiliaryPFOptimal_maxLikelihood); // For normalization purposes only
680 
681  // The desired dynamic number of m_particles (to be modified dynamically below):
682  const size_t minNumSamples_KLD = max((size_t)KLD_options.KLD_minSampleSize, (size_t)round(KLD_options.KLD_minSamplesPerBin*stateSpaceBinsLastTimestep.size()) );
683  size_t Nx = minNumSamples_KLD ;
684 
685  const size_t Np1 = me->m_particles.size();
686  vector_size_t oldPartIdxsStillNotPropragated(Np1); // Use a list since we'll use "erase" a lot here.
687  for (size_t k=0;k<Np1;k++) oldPartIdxsStillNotPropragated[k]=k; //.push_back(k);
688 
689  const size_t Np = stateSpaceBinsLastTimestepParticles.size();
690  vector_size_t permutationPathsAuxVector(Np);
691  for (size_t k=0;k<Np;k++) permutationPathsAuxVector[k]=k;
692 
693  // Instead of picking randomly from "permutationPathsAuxVector", we can shuffle it now just once,
694  // then pick in sequence from the tail and resize the container:
695  std::random_shuffle(
696  permutationPathsAuxVector.begin(),
697  permutationPathsAuxVector.end(),
699 
700  size_t k = 0;
701  size_t N = 0;
702 
703  TSetStateSpaceBins stateSpaceBins;
704 
705  do // "N" is the index of the current "new particle":
706  {
707  // Generate a new particle:
708  //
709  // (a) Propagate the last set of m_particles, and only if the
710  // desired number of m_particles in this step is larger,
711  // perform a UNIFORM sampling from the last set. In this way
712  // the new weights can be computed in the same way for all m_particles.
713  // ---------------------------------------------------------------------------
714  if (doResample)
715  {
716  k = me->fastDrawSample(PF_options); // Based on weights of last step only!
717  }
718  else
719  {
720  // Assure that at least one particle per "discrete-path" is taken (if the
721  // number of samples allows it)
722  if (permutationPathsAuxVector.size())
723  {
724  const size_t idxBinSpacePath = *permutationPathsAuxVector.rbegin();
725  permutationPathsAuxVector.resize(permutationPathsAuxVector.size()-1);
726 
727  const size_t idx = mrpt::random::randomGenerator.drawUniform32bit() % stateSpaceBinsLastTimestepParticles[idxBinSpacePath].size();
728  k = stateSpaceBinsLastTimestepParticles[idxBinSpacePath][idx];
729  ASSERT_(k<me->m_particles.size());
730 
731  // Also erase it from the other permutation vector list:
732  oldPartIdxsStillNotPropragated.erase(std::find(oldPartIdxsStillNotPropragated.begin(),oldPartIdxsStillNotPropragated.end(),k));
733  }
734  else
735  {
736  // Select a particle from the previous set with a UNIFORM distribution,
737  // in such a way we will assign each particle the updated weight accounting
738  // for its last weight.
739  // The first "old_N" m_particles will be drawn using a uniform random selection
740  // without repetitions:
741  //
742  // Select a index from "oldPartIdxsStillNotPropragated" and remove it from the list:
743  if (oldPartIdxsStillNotPropragated.size())
744  {
745  const size_t idx = mrpt::random::randomGenerator.drawUniform32bit() % oldPartIdxsStillNotPropragated.size();
746  vector_size_t::iterator it = oldPartIdxsStillNotPropragated.begin() + idx; //advance(it,idx);
747  k = *it;
748  oldPartIdxsStillNotPropragated.erase(it);
749  }
750  else
751  {
752  // N>N_old -> Uniformly draw index:
753  k = mrpt::random::randomGenerator.drawUniform32bit() % me->m_particles.size();
754  }
755  }
756  }
757 
758  // Do one rejection sampling step:
759  // ---------------------------------------------
760  CPose3D newPose;
761  double newParticleLogWeight;
762  PF_SLAM_aux_perform_one_rejection_sampling_step<BINTYPE>(
763  USE_OPTIMAL_SAMPLING,doResample,maxMeanLik,
764  k,
765  sf,PF_options,
766  newPose, newParticleLogWeight);
767 
768  // Insert the new particle
769  newParticles.push_back( newPose );
770  newParticlesDerivedFromIdx.push_back( k );
771  newParticlesWeight.push_back(newParticleLogWeight);
772 
773  // ----------------------------------------------------------------
774  // Now, the KLD-sampling dynamic sample size stuff:
775  // look if the particle's PATH falls into a new bin or not:
776  // ----------------------------------------------------------------
777  BINTYPE p;
778  const TPose3D newPose_s = newPose;
779  KLF_loadBinFromParticle<PARTICLE_TYPE,BINTYPE>( p,KLD_options, me->m_particles[k].d, &newPose_s );
780 
781  // -----------------------------------------------------------------------------
782  // Look for the bin "p" into "stateSpaceBins": If it is not yet into the set,
783  // then we may increase the desired particle number:
784  // -----------------------------------------------------------------------------
785 
786  // Found?
787  if ( stateSpaceBins.find(p)==stateSpaceBins.end() )
788  {
789  // It falls into a new bin: add to the stateSpaceBins:
790  stateSpaceBins.insert( p );
791 
792  // K = K + 1
793  int K = stateSpaceBins.size();
794  if ( K>1 )
795  {
796  // Update the number of m_particles!!
797  Nx = (size_t) (epsilon_1 * math::chi2inv(delta_1,K-1));
798  //printf("k=%u \tn=%u \tNx:%u\n", k, newParticles.size(), Nx);
799  }
800  }
801 
802  N = newParticles.size();
803 
804  } while (( N < KLD_options.KLD_maxSampleSize &&
805  N < max(Nx,minNumSamples_KLD)) ||
806  (permutationPathsAuxVector.size() && !doResample) );
807 
808  printf("\n[ADAPTIVE SAMPLE SIZE] #Bins: %u \t #Particles: %u \t Nx=%u\n", static_cast<unsigned>(stateSpaceBins.size()),static_cast<unsigned>(N), (unsigned)Nx);
809  } // end adaptive sample size
810 
811 
812  // ---------------------------------------------------------------------------------
813  // Substitute old by new particle set:
814  // Old are in "m_particles"
815  // New are in "newParticles", "newParticlesWeight","newParticlesDerivedFromIdx"
816  // ---------------------------------------------------------------------------------
817  this->PF_SLAM_implementation_replaceByNewParticleSet(
818  me->m_particles,
819  newParticles,newParticlesWeight,newParticlesDerivedFromIdx );
820 
821 
822  // In this PF_algorithm, we must do weight normalization by ourselves:
823  me->normalizeWeights();
824 
825  MRPT_END
826  } // end of PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal
827 
828 
829  /* ------------------------------------------------------------------------
830  PF_SLAM_aux_perform_one_rejection_sampling_step
831  ------------------------------------------------------------------------ */
832  template <class PARTICLE_TYPE,class MYSELF>
833  template <class BINTYPE>
835  const bool USE_OPTIMAL_SAMPLING,
836  const bool doResample,
837  const double maxMeanLik,
838  size_t k, // The particle from the old set "m_particles[]"
839  const mrpt::obs::CSensoryFrame * sf,
841  mrpt::poses::CPose3D & out_newPose,
842  double & out_newParticleLogWeight)
843  {
844  MYSELF *me = static_cast<MYSELF*>(this);
845 
846  // ADD-ON: If the 'm_pfAuxiliaryPFOptimal_estimatedProb[k]' is **extremelly** low relative to the other m_particles,
847  // resample only this particle with a copy of another one, uniformly:
848  while ( ( (USE_OPTIMAL_SAMPLING ? m_pfAuxiliaryPFOptimal_estimatedProb[k] : m_pfAuxiliaryPFStandard_estimatedProb[k] )
849  -maxMeanLik) < -PF_options.max_loglikelihood_dyn_range )
850  {
851  // Select another 'k' uniformly:
852  k = mrpt::random::randomGenerator.drawUniform32bit() % me->m_particles.size();
853  if (PF_options.verbose) cout << "[PF_implementation] Warning: Discarding very unlikely particle" << endl;
854  }
855 
856  const mrpt::poses::CPose3D oldPose = *getLastPose(k); // Get the current pose of the k'th particle
857 
858  // (b) Rejection-sampling: Draw a new robot pose from x[k],
859  // and accept it with probability p(zk|x) / maxLikelihood:
860  // ----------------------------------------------------------------
861  double poseLogLik;
862  if ( PF_SLAM_implementation_skipRobotMovement() )
863  {
864  // The first robot pose in the SLAM execution: All m_particles start
865  // at the same point (this is the lowest bound of subsequent uncertainty):
866  out_newPose = oldPose;
867  poseLogLik = 0;
868  }
869  else
870  {
871  CPose3D movementDraw;
872  if (!USE_OPTIMAL_SAMPLING)
873  { // APF:
874  m_movementDrawer.drawSample( movementDraw );
875  out_newPose.composeFrom(oldPose, movementDraw); // newPose = oldPose + movementDraw;
876  // Compute likelihood:
877  poseLogLik = PF_SLAM_computeObservationLikelihoodForParticle(PF_options, k,*sf,out_newPose);
878  }
879  else
880  { // Optimal APF with rejection sampling:
881  // Rejection-sampling:
882  double acceptanceProb;
883  int timeout = 0;
884  const int maxTries=10000;
885  double bestTryByNow_loglik= -std::numeric_limits<double>::max();
886  TPose3D bestTryByNow_pose;
887  do
888  {
889  // Draw new robot pose:
890  if (PF_options.pfAuxFilterOptimal_MLE && !m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed[k])
891  { // No! first take advantage of a good drawn value, but only once!!
892  m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed[k] = true;
893  movementDraw = CPose3D( m_pfAuxiliaryPFOptimal_maxLikDrawnMovement[k] );
894  }
895  else
896  {
897  // Draw new robot pose:
898  m_movementDrawer.drawSample( movementDraw );
899  }
900 
901  out_newPose.composeFrom(oldPose, movementDraw); // out_newPose = oldPose + movementDraw;
902 
903  // Compute acceptance probability:
904  poseLogLik = PF_SLAM_computeObservationLikelihoodForParticle(PF_options, k,*sf,out_newPose);
905 
906  if (poseLogLik>bestTryByNow_loglik)
907  {
908  bestTryByNow_loglik = poseLogLik;
909  bestTryByNow_pose = out_newPose;
910  }
911 
912  double ratioLikLik = std::exp( poseLogLik - m_pfAuxiliaryPFOptimal_maxLikelihood[k] );
913  acceptanceProb = std::min( 1.0, ratioLikLik );
914 
915  if ( ratioLikLik > 1)
916  {
917  m_pfAuxiliaryPFOptimal_maxLikelihood[k] = poseLogLik; // :'-( !!!
918  //acceptanceProb = 0; // Keep searching or keep this one?
919  }
920  } while ( ++timeout<maxTries && acceptanceProb < mrpt::random::randomGenerator.drawUniform(0.0,0.999) );
921 
922  if (timeout>=maxTries)
923  {
924  out_newPose = bestTryByNow_pose;
925  poseLogLik = bestTryByNow_loglik;
926  if (PF_options.verbose) cout << "[PF_implementation] Warning: timeout in rejection sampling." << endl;
927  }
928 
929  }
930 
931  // And its weight:
932  if (USE_OPTIMAL_SAMPLING)
933  { // Optimal PF
934  if (doResample)
935  out_newParticleLogWeight = 0; // By definition of our optimal PF, all samples have identical weights.
936  else
937  {
938  const double weightFact = m_pfAuxiliaryPFOptimal_estimatedProb[k] * PF_options.powFactor;
939  out_newParticleLogWeight = me->m_particles[k].log_w + weightFact;
940  }
941  }
942  else
943  { // APF:
944  const double weightFact = (poseLogLik-m_pfAuxiliaryPFStandard_estimatedProb[k]) * PF_options.powFactor;
945  if (doResample)
946  out_newParticleLogWeight = weightFact;
947  else out_newParticleLogWeight = weightFact + me->m_particles[k].log_w;
948  }
949 
950  }
951  // Done.
952  } // end PF_SLAM_aux_perform_one_rejection_sampling_step
953 
954 
955  } // end namespace
956 } // end namespace
957 
958 #endif
mrpt::utils::round
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:26
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: mrpt_macros.h:110
iterator
Scalar * iterator
Definition: eigen_plugins.h:23
mrpt::bayes::CParticleFilter::TParticleFilterOptions::adaptiveSampleSize
bool adaptiveSampleSize
A flag that indicates whether the CParticleFilterCapable object should perform adative sample size (d...
Definition: CParticleFilter.h:87
mrpt::random::randomGenerator
BASE_IMPEXP CRandomGenerator randomGenerator
A static instance of a CRandomGenerator class, for use in single-thread applications.
mrpt::bayes::CParticleFilter::TParticleFilterOptions::pfAuxFilterOptimal_MaximumSearchSamples
unsigned int pfAuxFilterOptimal_MaximumSearchSamples
In the algorithm "CParticleFilter::pfAuxiliaryPFOptimal" (and in "CParticleFilter::pfAuxiliaryPFStand...
Definition: CParticleFilter.h:93
mrpt::math::maximum
CONTAINER::Scalar maximum(const CONTAINER &v)
Definition: ops_containers.h:110
mrpt::vector_uint
std::vector< uint32_t > vector_uint
Definition: types_simple.h:28
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:46
mrpt::poses::CPose3DPDFGaussian::getMean
void getMean(CPose3D &mean_pose) const MRPT_OVERRIDE
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF).
Definition: CPose3DPDFGaussian.h:85
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:834
mrpt::obs::CActionRobotMovement3D::poseChange
poses::CPose3DPDFGaussian poseChange
The 3D pose change probabilistic estimation.
Definition: obs/CActionRobotMovement3D.h:52
mrpt::slam::TKLDParams
Option set for KLD algorithm.
Definition: TKLDParams.h:22
mrpt::random::CRandomGenerator::drawUniform32bit
uint32_t drawUniform32bit()
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm,...
mrpt::obs::CActionRobotMovement2D::poseChange
mrpt::poses::CPosePDFPtr poseChange
The 2D pose change probabilistic estimation.
Definition: obs/CActionRobotMovement2D.h:48
mrpt::math::mean
double mean(const CONTAINER &v)
Computes the mean value of a vector.
Definition: ops_containers.h:178
MRPT_CHECK_NORMAL_NUMBER
#define MRPT_CHECK_NORMAL_NUMBER(val)
Definition: mrpt_macros.h:262
CParticleFilterData.h
mrpt::math::distance
double BASE_IMPEXP distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
MRPT_UNUSED_PARAM
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
Definition: mrpt_macros.h:290
mrpt::obs::CActionRobotMovement3D
Represents a probabilistic 3D (6D) movement.
Definition: obs/CActionRobotMovement3D.h:28
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:40
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:94
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:102
mrpt::slam::TKLDParams::KLD_maxSampleSize
unsigned int KLD_maxSampleSize
Definition: TKLDParams.h:37
INVALID_LIKELIHOOD_VALUE
#define INVALID_LIKELIHOOD_VALUE
Definition: CParticleFilterCapable.h:19
PF_implementations_data.h
mrpt::obs::CActionCollection
Declares a class for storing a collection of robot actions.
Definition: obs/CActionCollection.h:30
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:16
mrpt::obs::CActionRobotMovement2D
Represents a probabilistic 2D movement of the robot mobile base.
Definition: obs/CActionRobotMovement2D.h:29
MRPT_END
#define MRPT_END
Definition: mrpt_macros.h:353
CActionRobotMovement3D.h
mrpt::math::CVectorDouble
dynamic_vector< double > CVectorDouble
Column vector, like Eigen::MatrixXd, but automatically initialized to zeros since construction.
Definition: eigen_frwds.h:37
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:330
random.h
mrpt::obs::CSensoryFrame
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: obs/CSensoryFrame.h:52
MRPT_START
#define MRPT_START
Definition: mrpt_macros.h:349
mrpt::bayes::CParticleFilterCapable
This virtual class defines the interface that any particles based PDF class must implement in order t...
Definition: CParticleFilterCapable.h:27
mrpt::slam::TKLDParams::KLD_epsilon
double KLD_epsilon
Definition: TKLDParams.h:32
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:109
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:495
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:88
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
distributions.h
mrpt::obs::CActionCollection::getBestMovementEstimation
CActionRobotMovement2DPtr getBestMovementEstimation() const
Returns the best pose increment estimator in the collection, based on the determinant of its pose cha...
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...
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::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:404
mrpt::slam::TKLDParams::KLD_delta
double KLD_delta
Definition: TKLDParams.h:32
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:37
TKLDParams.h
mrpt::obs::CActionCollection::getActionByClass
T::SmartPtr getActionByClass(const size_t &ith=0) const
Access to the i'th action of a given class, or a NULL smart pointer if there is no action of that cla...
Definition: obs/CActionCollection.h:144
mrpt::random::random_generator_for_STL
ptrdiff_t random_generator_for_STL(ptrdiff_t i)
A random number generator for usage in STL algorithms expecting a function like this (eg,...
Definition: RandomGenerators.h:306
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...
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:148
ASSERT_
#define ASSERT_(f)
Definition: mrpt_macros.h:261
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:130
CActionCollection.h
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:315
mrpt::vector_size_t
std::vector< size_t > vector_size_t
Definition: types_simple.h:25
mrpt::bayes::CParticleFilter::TParticleFilterOptions::verbose
bool verbose
Enable extra messages for each PF iteration (Default=false)
Definition: CParticleFilter.h:111
CActionRobotMovement2D.h
mrpt::bayes::CParticleFilter::TParticleFilterOptions::pfAuxFilterOptimal_MLE
bool pfAuxFilterOptimal_MLE
(Default=false) In the algorithm "CParticleFilter::pfAuxiliaryPFOptimal", if set to true,...
Definition: CParticleFilter.h:112
mrpt::math::minimum
CONTAINER::Scalar minimum(const CONTAINER &v)
Definition: ops_containers.h:111
mrpt::bayes::CParticleFilter::TParticleFilterOptions
The configuration of a particle filter.
Definition: CParticleFilter.h:80
mrpt::math::averageLogLikelihood
double BASE_IMPEXP averageLogLikelihood(const CVectorDouble &logLikelihoods)
A numerically-stable method to compute average likelihood values with strongly different ranges (unwe...
mrpt::math::chi2inv
double BASE_IMPEXP chi2inv(double P, unsigned int dim=1)
The "quantile" of the Chi-Square distribution, for dimension "dim" and probability 0<P<1 (the inverse...



Page generated by Doxygen 1.8.16 for MRPT 1.4.0 SVN: at Mon Oct 14 22:32:58 UTC 2019