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