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


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013