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