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



Page generated by Doxygen 1.9.6 for MRPT 1.4.0 SVN: at Wed Feb 15 01:46:32 UTC 2023