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 
13 #include <mrpt/bayes/CParticleFilterCapable.h>
14 #include <mrpt/bayes/CParticleFilterData.h>
15 #include <mrpt/random.h>
16 #include <mrpt/obs/CActionCollection.h>
17 #include <mrpt/obs/CActionRobotMovement3D.h>
18 #include <mrpt/obs/CActionRobotMovement2D.h>
19 #include <mrpt/slam/TKLDParams.h>
20 
21 #include <mrpt/math/distributions.h> // chi2inv
22 #include <mrpt/math/data_utils.h> // averageLogLikelihood()
23 
24 #include <mrpt/slam/PF_implementations_data.h>
25 
26 #include <mrpt/slam/link_pragmas.h>
27 
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>
44  bool PF_implementation<PARTICLE_TYPE,MYSELF>::PF_SLAM_implementation_gatherActionsCheckBothActObs(
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>
130  void PF_implementation<PARTICLE_TYPE,MYSELF>::PF_SLAM_implementation_pfAuxiliaryPFOptimal(
131  const mrpt::obs::CActionCollection * actions,
132  const mrpt::obs::CSensoryFrame * sf,
133  const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options,
134  const TKLDParams &KLD_options)
135  {
136  // Standard and Optimal AuxiliaryPF actually have a shared implementation body:
137  PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal<BINTYPE>(actions,sf,PF_options,KLD_options, true /*Optimal PF*/ );
138  }
139 
140 
141  /** A generic implementation of the PF method "pfStandardProposal" (standard proposal distribution, that is, a simple SIS particle filter),
142  * common to both localization and mapping.
143  *
144  * - BINTYPE: TPoseBin or whatever to discretize the sample space for KLD-sampling.
145  */
146  template <class PARTICLE_TYPE,class MYSELF>
147  template <class BINTYPE>
148  void PF_implementation<PARTICLE_TYPE,MYSELF>::PF_SLAM_implementation_pfStandardProposal(
149  const mrpt::obs::CActionCollection * actions,
150  const mrpt::obs::CSensoryFrame * sf,
151  const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options,
152  const TKLDParams &KLD_options)
153  {
154  MRPT_START
155  typedef std::set<BINTYPE,typename BINTYPE::lt_operator> TSetStateSpaceBins;
156 
157  MYSELF *me = static_cast<MYSELF*>(this);
158 
159  // In this method we don't need the "PF_SLAM_implementation_gatherActionsCheckBothActObs" machinery,
160  // since prediction & update are two independent stages well separated for this algorithm.
161 
162  // --------------------------------------------------------------------------------------
163  // Prediction: Simply draw samples from the motion model
164  // --------------------------------------------------------------------------------------
165  if (actions)
166  {
167  // Find a robot movement estimation:
168  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>
321  void PF_implementation<PARTICLE_TYPE,MYSELF>::PF_SLAM_implementation_pfAuxiliaryPFStandard(
322  const mrpt::obs::CActionCollection * actions,
323  const mrpt::obs::CSensoryFrame * sf,
324  const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options,
325  const TKLDParams &KLD_options)
326  {
327  // Standard and Optimal AuxiliaryPF actually have a shared implementation body:
328  PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal<BINTYPE>(actions,sf,PF_options,KLD_options, false /*APF*/ );
329  }
330 
331  /*---------------------------------------------------------------
332  PF_SLAM_particlesEvaluator_AuxPFOptimal
333  ---------------------------------------------------------------*/
334  template <class PARTICLE_TYPE,class MYSELF>
335  template <class BINTYPE>
336  double PF_implementation<PARTICLE_TYPE,MYSELF>::PF_SLAM_particlesEvaluator_AuxPFOptimal(
337  const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options,
338  const mrpt::bayes::CParticleFilterCapable *obj,
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>
412  double PF_implementation<PARTICLE_TYPE,MYSELF>::PF_SLAM_particlesEvaluator_AuxPFStandard(
413  const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options,
414  const mrpt::bayes::CParticleFilterCapable *obj,
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 
430  if (!PF_options.pfAuxFilterStandard_FirstStageWeightsMonteCarlo)
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>
505  void PF_implementation<PARTICLE_TYPE,MYSELF>::PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal(
506  const mrpt::obs::CActionCollection * actions,
507  const mrpt::obs::CSensoryFrame * sf,
508  const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options,
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(),
708  mrpt::random::random_generator_for_STL );
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>
844  void PF_implementation<PARTICLE_TYPE,MYSELF>::PF_SLAM_aux_perform_one_rejection_sampling_step(
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,
850  const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options,
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
mrpt::random::CRandomGenerator::drawUniform32bit
uint32_t drawUniform32bit()
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm, in the whole range of 32-bit integers.
Definition: RandomGenerator.cpp:84
mrpt::slam::PF_implementation::PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal
void PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal(const mrpt::obs::CActionCollection *actions, const mrpt::obs::CSensoryFrame *sf, const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options, const TKLDParams &KLD_options, const bool USE_OPTIMAL_SAMPLING)
The shared implementation body of two PF methods: APF and Optimal-APF, depending on USE_OPTIMAL_SAMPL...
Definition: PF_implementations.h:505
mrpt::obs::CActionRobotMovement3D::poseChange
mrpt::poses::CPose3DPDFGaussian poseChange
The 3D pose change probabilistic estimation.
Definition: obs/CActionRobotMovement3D.h:46
mrpt::obs::CActionRobotMovement2D::poseChange
mrpt::utils::poly_ptr_ptr< mrpt::poses::CPosePDFPtr > poseChange
The 2D pose change probabilistic estimation.
Definition: obs/CActionRobotMovement2D.h:48
min
#define min(a, b)
Definition: rplidar_driver.cpp:43
mrpt::vector_uint
std::vector< uint32_t > vector_uint
Definition: types_simple.h:28
mrpt::slam::PF_implementation::PF_SLAM_particlesEvaluator_AuxPFOptimal
static double PF_SLAM_particlesEvaluator_AuxPFOptimal(const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options, const mrpt::bayes::CParticleFilterCapable *obj, size_t index, const void *action, const void *observation)
Definition: PF_implementations.h:336
INVALID_LIKELIHOOD_VALUE
#define INVALID_LIKELIHOOD_VALUE
Definition: CParticleFilterCapable.h:19
mrpt::bayes::CParticleFilter::TParticleFilterOptions::pfAuxFilterOptimal_MaximumSearchSamples
unsigned int pfAuxFilterOptimal_MaximumSearchSamples
In the algorithm "CParticleFilter::pfAuxiliaryPFOptimal" (and in "CParticleFilter::pfAuxiliaryPFStand...
Definition: CParticleFilter.h:93
MRPT_CHECK_NORMAL_NUMBER
#define MRPT_CHECK_NORMAL_NUMBER(val)
Definition: maps/CColouredPointsMap.h:279
mrpt::slam::PF_implementation::PF_SLAM_implementation_pfAuxiliaryPFStandard
void PF_SLAM_implementation_pfAuxiliaryPFStandard(const mrpt::obs::CActionCollection *actions, const mrpt::obs::CSensoryFrame *sf, const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options, const TKLDParams &KLD_options)
A generic implementation of the PF method "prediction_and_update_pfAuxiliaryPFStandard" (Auxiliary pa...
Definition: PF_implementations.h:321
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: maps/CColouredPointsMap.h:154
q
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3626
mrpt::random::randomGenerator
BASE_IMPEXP CRandomGenerator randomGenerator
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:23
iterator
Scalar * iterator
Definition: eigen_plugins.h:23
data_utils.h
mrpt::math::dynamic_vector
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
mrpt::slam::TKLDParams
Option set for KLD algorithm.
Definition: TKLDParams.h:22
TKLDParams.h
mrpt::obs::CActionRobotMovement2D::computeFromOdometry
void computeFromOdometry(const mrpt::poses::CPose2D &odometryIncrement, const TMotionModelOptions &options)
Computes the PDF of the pose increment from an odometry reading and according to the given motion mod...
Definition: CActionRobotMovement2D.cpp:378
PF_implementations_data.h
mrpt::slam::PF_implementation::PF_SLAM_implementation_gatherActionsCheckBothActObs
bool PF_SLAM_implementation_gatherActionsCheckBothActObs(const mrpt::obs::CActionCollection *actions, const mrpt::obs::CSensoryFrame *sf)
Auxiliary method called by PF implementations: return true if we have both action & observation...
Definition: PF_implementations.h:44
mrpt::utils::find
const_iterator find(const KEY &key) const
Definition: ts_hash_map.h:139
obj
GLsizei GLsizei GLuint * obj
Definition: glext.h:3902
mrpt::obs::CActionCollection
Declares a class for storing a collection of robot actions.
Definition: obs/CActionCollection.h:31
mrpt::slam::PF_implementation
A set of common data shared by PF implementations for both SLAM and localization. ...
Definition: PF_implementations_data.h:41
mrpt::math::minimum
CONTAINER::Scalar minimum(const CONTAINER &v)
Definition: ops_containers.h:112
mrpt::obs::CActionRobotMovement3D
Represents a probabilistic 3D (6D) movement.
Definition: obs/CActionRobotMovement3D.h:27
mrpt::obs::CActionRobotMovement2D
Represents a probabilistic 2D movement of the robot mobile base.
Definition: obs/CActionRobotMovement2D.h:32
mrpt::slam::PF_implementation::PF_SLAM_aux_perform_one_rejection_sampling_step
void PF_SLAM_aux_perform_one_rejection_sampling_step(const bool USE_OPTIMAL_SAMPLING, const bool doResample, const double maxMeanLik, size_t k, const mrpt::obs::CSensoryFrame *sf, const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options, mrpt::poses::CPose3D &out_newPose, double &out_newParticleLogWeight)
Definition: PF_implementations.h:844
mrpt::slam::TKLDParams::KLD_minSampleSize
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
mrpt::slam::PF_implementation::PF_SLAM_implementation_pfAuxiliaryPFOptimal
void PF_SLAM_implementation_pfAuxiliaryPFOptimal(const mrpt::obs::CActionCollection *actions, const mrpt::obs::CSensoryFrame *sf, const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options, const TKLDParams &KLD_options)
A generic implementation of the PF method "prediction_and_update_pfAuxiliaryPFOptimal" (optimal sampl...
Definition: PF_implementations.h:130
CParticleFilterData.h
MRPT_END
#define MRPT_END
Definition: maps/CColouredPointsMap.h:370
MRPT_UNUSED_PARAM
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
Definition: maps/CColouredPointsMap.h:307
mrpt::poses::CPose3D::composeFrom
void composeFrom(const CPose3D &A, const CPose3D &B)
Makes "this = A (+) B"; this method is slightly more efficient than "this= A + B;" since it avoids th...
Definition: CPose3D.cpp:607
mrpt::slam::TKLDParams::KLD_minSamplesPerBin
double KLD_minSamplesPerBin
(Default: KLD_minSamplesPerBin=0) The minimum number of samples will be the maximum of KLD_minSampleS...
Definition: TKLDParams.h:40
mrpt::math::averageLogLikelihood
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
index
GLuint index
Definition: glext.h:3891
mrpt::slam::PF_implementation::PF_SLAM_particlesEvaluator_AuxPFStandard
static double PF_SLAM_particlesEvaluator_AuxPFStandard(const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options, const mrpt::bayes::CParticleFilterCapable *obj, size_t index, const void *action, const void *observation)
Compute w[i]*p(z_t | mu_t^i), with mu_t^i being the mean of the new robot pose.
Definition: PF_implementations.h:412
mrpt::obs::CSensoryFrame
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: obs/CSensoryFrame.h:52
mrpt::bayes::CParticleFilterCapable
This virtual class defines the interface that any particles based PDF class must implement in order t...
Definition: CParticleFilterCapable.h:27
mrpt::mrpt::format
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
mrpt::bayes::CParticleFilter::TParticleFilterOptions::pfAuxFilterStandard_FirstStageWeightsMonteCarlo
bool pfAuxFilterStandard_FirstStageWeightsMonteCarlo
Only for PF_algorithm==pfAuxiliaryPFStandard: If false, the APF will predict the first stage weights ...
Definition: CParticleFilter.h:109
mrpt::math::maximum
CONTAINER::Scalar maximum(const CONTAINER &v)
Definition: ops_containers.h:111
mrpt::bayes::CParticleFilter::TParticleFilterOptions::BETA
double BETA
The resampling of particles will be performed when ESS (in range [0,1]) < BETA (default is 0...
Definition: CParticleFilter.h:88
mrpt::random::random_generator_for_STL
ptrdiff_t random_generator_for_STL(ptrdiff_t i)
A random number generator for usage in STL algorithms expecting a function like this (eg...
Definition: RandomGenerators.h:306
mrpt::bayes::CParticleFilterCapable::TParticleProbabilityEvaluator
double(* TParticleProbabilityEvaluator)(const bayes::CParticleFilter::TParticleFilterOptions &PF_options, const CParticleFilterCapable *obj, size_t index, const void *action, const void *observation)
A callback function type for evaluating the probability of m_particles of being selected, used in "fastDrawSample".
Definition: CParticleFilterCapable.h:54
mrpt::slam::TKLDParams::KLD_maxSampleSize
unsigned int KLD_maxSampleSize
Definition: TKLDParams.h:37
mrpt::poses::CPose3DPDFGaussian::getMean
void getMean(CPose3D &mean_pose) const MRPT_OVERRIDE
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF).
Definition: CPose3DPDFGaussian.h:85
MRPT_START
#define MRPT_START
Definition: maps/CColouredPointsMap.h:366
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:16
CParticleFilterCapable.h
mrpt::math::chi2inv
double BASE_IMPEXP chi2inv(double P, unsigned int dim=1)
The "quantile" of the Chi-Square distribution, for dimension "dim" and probability 0<P<1 (the inverse...
Definition: math.cpp:967
mrpt::utils::poly_ptr_ptr::get_ptr
const T & get_ptr() const
Definition: poly_ptr_ptr.h:87
mrpt::bayes::CParticleFilter::TParticleFilterOptions::max_loglikelihood_dyn_range
double max_loglikelihood_dyn_range
Only for PF_algorithm=pfAuxiliaryPFOptimal: If a given particle has a max_likelihood (from the a-prio...
Definition: CParticleFilter.h:102
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
mrpt::bayes::CParticleFilter::TParticleFilterOptions::powFactor
double powFactor
An optional step to "smooth" dramatic changes in the observation model to affect the variance of the ...
Definition: CParticleFilter.h:94
mrpt::bayes::CParticleFilter::TParticleFilterOptions
The configuration of a particle filter.
Definition: CParticleFilter.h:80
mrpt::obs::CActionCollection::getBestMovementEstimation
CActionRobotMovement2DPtr getBestMovementEstimation() const
Returns the best pose increment estimator in the collection, based on the determinant of its pose cha...
Definition: CActionCollection.cpp:133
mrpt::math::TPose3D
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: lightweight_geom_data.h:406
mrpt::math::mean
double mean(const CONTAINER &v)
Computes the mean value of a vector.
Definition: ops_containers.h:179
ASSERT_
#define ASSERT_(f)
Definition: maps/CColouredPointsMap.h:278
mrpt::utils::round
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:26
mrpt::vector_size_t
std::vector< size_t > vector_size_t
Definition: types_simple.h:25
mrpt::bayes::CParticleFilter::TParticleFilterOptions::pfAuxFilterOptimal_MLE
bool pfAuxFilterOptimal_MLE
(Default=false) In the algorithm "CParticleFilter::pfAuxiliaryPFOptimal", if set to true...
Definition: CParticleFilter.h:111
mrpt::obs::CActionCollection::getActionByClass
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...
Definition: obs/CActionCollection.h:127
link_pragmas.h
mrpt::slam::PF_implementation::PF_SLAM_implementation_pfStandardProposal
void PF_SLAM_implementation_pfStandardProposal(const mrpt::obs::CActionCollection *actions, const mrpt::obs::CSensoryFrame *sf, const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options, const TKLDParams &KLD_options)
A generic implementation of the PF method "pfStandardProposal" (standard proposal distribution...
Definition: PF_implementations.h:148
p
GLfloat GLfloat p
Definition: glext.h:5587
mrpt::math::distance
double BASE_IMPEXP distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
Definition: geometry.cpp:1504
mrpt::bayes::CParticleFilter::TParticleFilterOptions::adaptiveSampleSize
bool adaptiveSampleSize
A flag that indicates whether the CParticleFilterCapable object should perform adative sample size (d...
Definition: CParticleFilter.h:87


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