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


Page generated by Doxygen 1.8.14 for MRPT 1.1.0 SVN: at lun oct 28 00:54:49 CET 2019 Hosted on:
SourceForge.net Logo