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



Page generated by Doxygen 1.8.14 for MRPT 1.0.2 SVN: at lun oct 28 00:52:41 CET 2019 Hosted on:
SourceForge.net Logo