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