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



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