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



Page generated by Doxygen 1.8.14 for MRPT 1.5.9 Git: 690a4699f Wed Apr 15 19:29:53 2020 +0200 at miƩ abr 15 19:30:12 CEST 2020