00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 #ifndef PF_implementations_H
00030 #define PF_implementations_H
00031
00032 #include <mrpt/utils/stl_extensions.h>
00033 #include <mrpt/bayes/CParticleFilterData.h>
00034 #include <mrpt/random.h>
00035 #include <mrpt/slam/CActionCollection.h>
00036 #include <mrpt/slam/CActionRobotMovement3D.h>
00037 #include <mrpt/slam/CActionRobotMovement2D.h>
00038 #include <mrpt/slam/TKLDParams.h>
00039
00040 #include <mrpt/math/distributions.h>
00041
00042 #include <mrpt/slam/PF_implementations_data.h>
00043
00044 #include <mrpt/slam/link_pragmas.h>
00045
00046
00047
00048
00049
00050
00051 namespace mrpt
00052 {
00053 namespace slam
00054 {
00055 using namespace std;
00056 using namespace mrpt::utils;
00057 using namespace mrpt::random;
00058 using namespace mrpt::poses;
00059 using namespace mrpt::bayes;
00060 using namespace mrpt::math;
00061
00062
00063
00064
00065
00066
00067 template <class PARTICLE_TYPE,class MYSELF>
00068 template <class BINTYPE>
00069 bool PF_implementation<PARTICLE_TYPE,MYSELF>::PF_SLAM_implementation_gatherActionsCheckBothActObs(
00070 const CActionCollection * actions,
00071 const CSensoryFrame * sf )
00072 {
00073 MYSELF *me = static_cast<MYSELF*>(this);
00074
00075 if (actions!=NULL)
00076 {
00077 {
00078 CActionRobotMovement2DPtr robotMovement2D = actions->getBestMovementEstimation();
00079 if (robotMovement2D.present())
00080 {
00081 if (m_accumRobotMovement3DIsValid) THROW_EXCEPTION("Mixing 2D and 3D actions is not allowed.")
00082
00083 if (!m_accumRobotMovement2DIsValid)
00084 {
00085 robotMovement2D->poseChange->getMean( m_accumRobotMovement2D.rawOdometryIncrementReading );
00086 m_accumRobotMovement2D.motionModelConfiguration = robotMovement2D->motionModelConfiguration;
00087 }
00088 else m_accumRobotMovement2D.rawOdometryIncrementReading += robotMovement2D->poseChange->getMeanVal();
00089
00090 m_accumRobotMovement2DIsValid = true;
00091 }
00092 else
00093 {
00094 CActionRobotMovement3DPtr robotMovement3D = actions->getActionByClass<CActionRobotMovement3D>();
00095 if (robotMovement3D)
00096 {
00097 if (m_accumRobotMovement2DIsValid) THROW_EXCEPTION("Mixing 2D and 3D actions is not allowed.")
00098
00099 if (!m_accumRobotMovement3DIsValid)
00100 m_accumRobotMovement3D = robotMovement3D->poseChange;
00101 else m_accumRobotMovement3D += robotMovement3D->poseChange;
00102
00103
00104 m_accumRobotMovement3DIsValid = true;
00105 }
00106 else
00107 return false;
00108 }
00109 }
00110 }
00111
00112 const bool SFhasValidObservations = (sf==NULL) ? false : PF_SLAM_implementation_doWeHaveValidObservations(me->m_particles,sf);
00113
00114
00115 if (! ((m_accumRobotMovement2DIsValid || m_accumRobotMovement3DIsValid) && SFhasValidObservations))
00116 return false;
00117
00118
00119
00120 if (m_accumRobotMovement3DIsValid)
00121 {
00122 m_movementDrawer.setPosePDF( m_accumRobotMovement3D );
00123 m_accumRobotMovement3DIsValid = false;
00124 }
00125 else
00126 {
00127 CActionRobotMovement2D theResultingRobotMov;
00128 theResultingRobotMov.computeFromOdometry(
00129 m_accumRobotMovement2D.rawOdometryIncrementReading,
00130 m_accumRobotMovement2D.motionModelConfiguration );
00131
00132 m_movementDrawer.setPosePDF( theResultingRobotMov.poseChange );
00133 m_accumRobotMovement2DIsValid = false;
00134 }
00135 return true;
00136 }
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151 template <class PARTICLE_TYPE,class MYSELF>
00152 template <class BINTYPE>
00153 void PF_implementation<PARTICLE_TYPE,MYSELF>::PF_SLAM_implementation_pfAuxiliaryPFOptimal(
00154 const CActionCollection * actions,
00155 const CSensoryFrame * sf,
00156 const CParticleFilter::TParticleFilterOptions &PF_options,
00157 const TKLDParams &KLD_options)
00158 {
00159
00160 PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal<BINTYPE>(actions,sf,PF_options,KLD_options, true );
00161 }
00162
00163
00164
00165
00166
00167
00168
00169 template <class PARTICLE_TYPE,class MYSELF>
00170 template <class BINTYPE>
00171 void PF_implementation<PARTICLE_TYPE,MYSELF>::PF_SLAM_implementation_pfStandardProposal(
00172 const CActionCollection * actions,
00173 const CSensoryFrame * sf,
00174 const CParticleFilter::TParticleFilterOptions &PF_options,
00175 const TKLDParams &KLD_options)
00176 {
00177 MRPT_START
00178 typedef std::set<BINTYPE,typename BINTYPE::lt_operator> TSetStateSpaceBins;
00179
00180 MYSELF *me = static_cast<MYSELF*>(this);
00181
00182
00183
00184
00185
00186
00187
00188 if (actions)
00189 {
00190
00191 CPose3D motionModelMeanIncr;
00192 {
00193 CActionRobotMovement2DPtr robotMovement2D = actions->getBestMovementEstimation();
00194
00195 if (robotMovement2D.present())
00196 {
00197 m_movementDrawer.setPosePDF( robotMovement2D->poseChange );
00198 motionModelMeanIncr = robotMovement2D->poseChange->getMeanVal();
00199 }
00200 else
00201 {
00202 CActionRobotMovement3DPtr robotMovement3D = actions->getActionByClass<CActionRobotMovement3D>();
00203 if (robotMovement3D)
00204 {
00205 m_movementDrawer.setPosePDF( robotMovement3D->poseChange );
00206 robotMovement3D->poseChange.getMean( motionModelMeanIncr );
00207 }
00208 else { THROW_EXCEPTION("Action list does not contain any CActionRobotMovement2D or CActionRobotMovement3D object!"); }
00209 }
00210 }
00211
00212
00213 if ( !PF_options.adaptiveSampleSize )
00214 {
00215 const size_t M = me->m_particles.size();
00216
00217
00218
00219 CPose3D incrPose;
00220 for (size_t i=0;i<M;i++)
00221 {
00222
00223 m_movementDrawer.drawSample( incrPose );
00224 CPose3D finalPose = CPose3D(*getLastPose(i)) + incrPose;
00225
00226
00227 PF_SLAM_implementation_custom_update_particle_with_new_pose( me->m_particles[i].d, TPose3D(finalPose) );
00228 }
00229 }
00230 else
00231 {
00232
00233
00234
00235
00236
00237
00238 TSetStateSpaceBins stateSpaceBins;
00239
00240 size_t Nx = KLD_options.KLD_minSampleSize;
00241 const double delta_1 = 1.0 - KLD_options.KLD_delta;
00242 const double epsilon_1 = 0.5 / KLD_options.KLD_epsilon;
00243
00244
00245 me->prepareFastDrawSample(PF_options);
00246
00247
00248 std::vector<TPose3D> newParticles;
00249 vector_double newParticlesWeight;
00250 std::vector<size_t> newParticlesDerivedFromIdx;
00251
00252 CPose3D increment_i;
00253 size_t N = 1;
00254
00255 do
00256 {
00257
00258 m_movementDrawer.drawSample( increment_i );
00259
00260
00261 const size_t drawn_idx = me->fastDrawSample(PF_options);
00262 const CPose3D newPose = CPose3D(*getLastPose(drawn_idx)) + increment_i;
00263 const TPose3D newPose_s = newPose;
00264
00265
00266 newParticles.push_back( newPose_s );
00267 newParticlesWeight.push_back(0);
00268 newParticlesDerivedFromIdx.push_back(drawn_idx);
00269
00270
00271
00272 BINTYPE p;
00273 KLF_loadBinFromParticle<PARTICLE_TYPE,BINTYPE>(p,KLD_options, me->m_particles[drawn_idx].d, &newPose_s);
00274
00275 if (stateSpaceBins.find( p )==stateSpaceBins.end())
00276 {
00277
00278
00279 stateSpaceBins.insert( p );
00280
00281
00282 size_t K = stateSpaceBins.size();
00283 if ( K>1)
00284 {
00285
00286 Nx = round(epsilon_1 * math::chi2inv(delta_1,K-1));
00287
00288 }
00289 }
00290 N = newParticles.size();
00291 } while ( N < max(Nx,(size_t)KLD_options.KLD_minSampleSize) &&
00292 N < KLD_options.KLD_maxSampleSize );
00293
00294
00295
00296
00297
00298
00299 this->PF_SLAM_implementation_replaceByNewParticleSet(
00300 me->m_particles,
00301 newParticles,newParticlesWeight,newParticlesDerivedFromIdx );
00302
00303 }
00304 }
00305
00306 if (sf)
00307 {
00308 const size_t M = me->m_particles.size();
00309
00310
00311
00312 for (size_t i=0;i<M;i++)
00313 {
00314 const TPose3D *partPose= getLastPose(i);
00315 CPose3D partPose2 = CPose3D(*partPose);
00316 const double obs_log_likelihood = PF_SLAM_computeObservationLikelihoodForParticle(PF_options,i,*sf,partPose2);
00317 me->m_particles[i].log_w += obs_log_likelihood * PF_options.powFactor;
00318 }
00319
00320
00321 }
00322
00323 MRPT_END
00324 }
00325
00326
00327
00328
00329
00330
00331
00332
00333
00334
00335
00336 template <class PARTICLE_TYPE,class MYSELF>
00337 template <class BINTYPE>
00338 void PF_implementation<PARTICLE_TYPE,MYSELF>::PF_SLAM_implementation_pfAuxiliaryPFStandard(
00339 const CActionCollection * actions,
00340 const CSensoryFrame * sf,
00341 const CParticleFilter::TParticleFilterOptions &PF_options,
00342 const TKLDParams &KLD_options)
00343 {
00344
00345 PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal<BINTYPE>(actions,sf,PF_options,KLD_options, false );
00346 }
00347
00348
00349
00350
00351 template <class PARTICLE_TYPE,class MYSELF>
00352 template <class BINTYPE>
00353 double PF_implementation<PARTICLE_TYPE,MYSELF>::PF_SLAM_particlesEvaluator_AuxPFOptimal(
00354 const CParticleFilter::TParticleFilterOptions &PF_options,
00355 const CParticleFilterCapable *obj,
00356 size_t index,
00357 const void *action,
00358 const void *observation )
00359 {
00360 MRPT_START
00361
00362
00363 const MYSELF *me = static_cast<const MYSELF*>(obj);
00364
00365
00366
00367
00368
00369 double indivLik, maxLik= -1e300;
00370 CPose3D maxLikDraw;
00371 size_t N = PF_options.pfAuxFilterOptimal_MaximumSearchSamples;
00372 ASSERT_(N>1)
00373
00374 const CPose3D oldPose = *me->getLastPose(index);
00375 vector_double vectLiks(N,0);
00376 CPose3D drawnSample;
00377 for (size_t q=0;q<N;q++)
00378 {
00379 me->m_movementDrawer.drawSample(drawnSample);
00380 CPose3D x_predict = oldPose + drawnSample;
00381
00382
00383 indivLik = me->PF_SLAM_computeObservationLikelihoodForParticle(
00384 PF_options,
00385 index,
00386 *static_cast<const CSensoryFrame*>(observation),
00387 x_predict );
00388
00389 MRPT_CHECK_NORMAL_NUMBER(indivLik);
00390 vectLiks[q] = indivLik;
00391 if ( indivLik > maxLik )
00392 {
00393 maxLikDraw = drawnSample;
00394 maxLik = indivLik;
00395 }
00396 }
00397
00398
00399
00400
00401 double avrgLogLik = math::averageLogLikelihood( vectLiks );
00402
00403
00404 me->m_pfAuxiliaryPFOptimal_estimatedProb[index] = avrgLogLik;
00405 me->m_pfAuxiliaryPFOptimal_maxLikelihood[index] = maxLik;
00406
00407 if (PF_options.pfAuxFilterOptimal_MLE)
00408 me->m_pfAuxiliaryPFOptimal_maxLikDrawnMovement[index] = maxLikDraw;
00409
00410
00411
00412 return me->m_particles[index].log_w + me->m_pfAuxiliaryPFOptimal_estimatedProb[index];
00413
00414 MRPT_END
00415 }
00416
00417
00418
00419
00420
00421
00422
00423
00424 template <class PARTICLE_TYPE,class MYSELF>
00425 template <class BINTYPE>
00426 double PF_implementation<PARTICLE_TYPE,MYSELF>::PF_SLAM_particlesEvaluator_AuxPFStandard(
00427 const CParticleFilter::TParticleFilterOptions &PF_options,
00428 const CParticleFilterCapable *obj,
00429 size_t index,
00430 const void *action,
00431 const void *observation )
00432 {
00433 MRPT_START
00434
00435
00436 const MYSELF *myObj = static_cast<const MYSELF*>(obj);
00437
00438
00439 const double cur_logweight = myObj->m_particles[index].log_w;
00440 const CPose3D oldPose = *myObj->getLastPose(index);
00441
00442 if (!PF_options.pfAuxFilterStandard_FirstStageWeightsMonteCarlo)
00443 {
00444
00445
00446 CPose3D x_predict;
00447 x_predict.composeFrom( oldPose, *static_cast<const CPose3D*>(action) );
00448
00449
00450
00451 myObj->m_pfAuxiliaryPFStandard_estimatedProb[index] = myObj->PF_SLAM_computeObservationLikelihoodForParticle(
00452 PF_options, index,
00453 *static_cast<const CSensoryFrame*>(observation), x_predict );
00454
00455
00456 return cur_logweight + myObj->m_pfAuxiliaryPFStandard_estimatedProb[index];
00457 }
00458 else
00459 {
00460
00461
00462
00463
00464
00465 double indivLik, maxLik= -1e300;
00466 CPose3D maxLikDraw;
00467 size_t N = PF_options.pfAuxFilterOptimal_MaximumSearchSamples;
00468 ASSERT_(N>1)
00469
00470 vector_double vectLiks(N,0);
00471 CPose3D drawnSample;
00472 for (size_t q=0;q<N;q++)
00473 {
00474 myObj->m_movementDrawer.drawSample(drawnSample);
00475 CPose3D x_predict = oldPose + drawnSample;
00476
00477
00478 indivLik = myObj->PF_SLAM_computeObservationLikelihoodForParticle(
00479 PF_options,
00480 index,
00481 *static_cast<const CSensoryFrame*>(observation),
00482 x_predict );
00483
00484 MRPT_CHECK_NORMAL_NUMBER(indivLik);
00485 vectLiks[q] = indivLik;
00486 if ( indivLik > maxLik )
00487 {
00488 maxLikDraw = drawnSample;
00489 maxLik = indivLik;
00490 }
00491 }
00492
00493
00494
00495
00496 double avrgLogLik = math::averageLogLikelihood( vectLiks );
00497
00498
00499 myObj->m_pfAuxiliaryPFStandard_estimatedProb[index] = avrgLogLik;
00500
00501 myObj->m_pfAuxiliaryPFOptimal_maxLikelihood[index] = maxLik;
00502 if (PF_options.pfAuxFilterOptimal_MLE)
00503 myObj->m_pfAuxiliaryPFOptimal_maxLikDrawnMovement[index] = maxLikDraw;
00504
00505
00506
00507 return cur_logweight + myObj->m_pfAuxiliaryPFOptimal_estimatedProb[index];
00508 }
00509 MRPT_END
00510 }
00511
00512
00513
00514
00515 template <class PARTICLE_TYPE,class MYSELF>
00516 template <class BINTYPE>
00517 void PF_implementation<PARTICLE_TYPE,MYSELF>::PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal(
00518 const CActionCollection * actions,
00519 const CSensoryFrame * sf,
00520 const CParticleFilter::TParticleFilterOptions &PF_options,
00521 const TKLDParams &KLD_options,
00522 const bool USE_OPTIMAL_SAMPLING )
00523 {
00524 MRPT_START
00525 typedef std::set<BINTYPE,typename BINTYPE::lt_operator> TSetStateSpaceBins;
00526
00527 MYSELF *me = static_cast<MYSELF*>(this);
00528
00529 const size_t M = me->m_particles.size();
00530
00531
00532
00533
00534
00535
00536
00537 if (!PF_SLAM_implementation_gatherActionsCheckBothActObs<BINTYPE>(actions,sf))
00538 return;
00539
00540
00541
00542
00543
00544
00545
00546
00547
00548
00549
00550 m_pfAuxiliaryPFOptimal_maxLikelihood.assign(M, INVALID_LIKELIHOOD_VALUE);
00551 m_pfAuxiliaryPFOptimal_maxLikDrawnMovement.resize(M);
00552
00553 m_pfAuxiliaryPFOptimal_estimatedProb.resize(M);
00554
00555 m_pfAuxiliaryPFStandard_estimatedProb.resize(M);
00556
00557
00558 CPose3D meanRobotMovement;
00559 m_movementDrawer.getSamplingMean3D(meanRobotMovement);
00560
00561
00562 typedef PF_implementation<PARTICLE_TYPE,MYSELF> TMyClass;
00563 CParticleFilterCapable::TParticleProbabilityEvaluator funcOpt = &TMyClass::template PF_SLAM_particlesEvaluator_AuxPFOptimal<BINTYPE>;
00564 CParticleFilterCapable::TParticleProbabilityEvaluator funcStd = &TMyClass::template PF_SLAM_particlesEvaluator_AuxPFStandard<BINTYPE>;
00565
00566 me->prepareFastDrawSample(
00567 PF_options,
00568 USE_OPTIMAL_SAMPLING ? funcOpt : funcStd,
00569 &meanRobotMovement,
00570 sf );
00571
00572
00573
00574 if (USE_OPTIMAL_SAMPLING && PF_options.verbose)
00575 {
00576 printf("[prepareFastDrawSample] max (log) = %10.06f\n", math::maximum(m_pfAuxiliaryPFOptimal_estimatedProb) );
00577 printf("[prepareFastDrawSample] max-mean (log) = %10.06f\n", -math::mean(m_pfAuxiliaryPFOptimal_estimatedProb) + math::maximum(m_pfAuxiliaryPFOptimal_estimatedProb) );
00578 printf("[prepareFastDrawSample] max-min (log) = %10.06f\n", -math::minimum(m_pfAuxiliaryPFOptimal_estimatedProb) + math::maximum(m_pfAuxiliaryPFOptimal_estimatedProb) );
00579 }
00580
00581
00582
00583
00584
00585
00586
00587
00588
00589
00590
00591
00592
00593
00594 vector<TPose3D> newParticles;
00595 vector<double> newParticlesWeight;
00596 vector<size_t> newParticlesDerivedFromIdx;
00597
00598
00599
00600
00601
00602
00603 if (PF_options.pfAuxFilterOptimal_MLE)
00604 m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed.assign(M, false);
00605
00606 const double maxMeanLik = math::maximum(
00607 USE_OPTIMAL_SAMPLING ?
00608 m_pfAuxiliaryPFOptimal_estimatedProb :
00609 m_pfAuxiliaryPFStandard_estimatedProb );
00610
00611 if ( !PF_options.adaptiveSampleSize )
00612 {
00613
00614
00615
00616 newParticles.resize(M);
00617 newParticlesWeight.resize(M);
00618 newParticlesDerivedFromIdx.resize(M);
00619
00620 const bool doResample = me->ESS() < PF_options.BETA;
00621
00622 for (size_t i=0;i<M;i++)
00623 {
00624 size_t k;
00625
00626
00627
00628
00629 if (doResample)
00630 k = me->fastDrawSample(PF_options);
00631 else k = i;
00632
00633
00634
00635 CPose3D newPose;
00636 double newParticleLogWeight;
00637 PF_SLAM_aux_perform_one_rejection_sampling_step<BINTYPE>(
00638 USE_OPTIMAL_SAMPLING,doResample,maxMeanLik,
00639 k,
00640 sf,PF_options,
00641 newPose, newParticleLogWeight);
00642
00643
00644 newParticles[i] = newPose;
00645 newParticlesDerivedFromIdx[i] = k;
00646 newParticlesWeight[i] = newParticleLogWeight;
00647
00648 }
00649 }
00650 else
00651 {
00652
00653
00654
00655
00656
00657
00658
00659 newParticles.clear();
00660 newParticlesWeight.clear();
00661 newParticlesDerivedFromIdx.clear();
00662
00663
00664
00665
00666
00667
00668
00669
00670 TSetStateSpaceBins stateSpaceBinsLastTimestep;
00671 std::vector<vector_uint> stateSpaceBinsLastTimestepParticles;
00672 typename MYSELF::CParticleList::iterator partIt;
00673 unsigned int partIndex;
00674
00675 printf( "[FIXED_SAMPLING] Computing...");
00676 for (partIt = me->m_particles.begin(),partIndex=0; partIt!=me->m_particles.end(); ++partIt,++partIndex)
00677 {
00678
00679 BINTYPE p;
00680 KLF_loadBinFromParticle<PARTICLE_TYPE,BINTYPE>(p, KLD_options,partIt->d );
00681
00682
00683 typename TSetStateSpaceBins::iterator posFound=stateSpaceBinsLastTimestep.find(p);
00684 if ( posFound == stateSpaceBinsLastTimestep.end() )
00685 {
00686 stateSpaceBinsLastTimestep.insert( p );
00687 stateSpaceBinsLastTimestepParticles.push_back( vector_uint(1,partIndex) );
00688 }
00689 else
00690 {
00691 const size_t idx = std::distance(stateSpaceBinsLastTimestep.begin(),posFound);
00692 stateSpaceBinsLastTimestepParticles[idx].push_back( partIndex );
00693 }
00694 }
00695 printf( "done (%u bins in t-1)\n",(unsigned int)stateSpaceBinsLastTimestep.size());
00696
00697
00698
00699
00700 double delta_1 = 1.0 - KLD_options.KLD_delta;
00701 double epsilon_1 = 0.5 / KLD_options.KLD_epsilon;
00702 bool doResample = me->ESS() < 0.5;
00703
00704
00705
00706 const size_t minNumSamples_KLD = max((size_t)KLD_options.KLD_minSampleSize, (size_t)round(KLD_options.KLD_minSamplesPerBin*stateSpaceBinsLastTimestep.size()) );
00707 size_t Nx = minNumSamples_KLD ;
00708
00709 const size_t Np1 = me->m_particles.size();
00710 vector_size_t oldPartIdxsStillNotPropragated(Np1);
00711 for (size_t k=0;k<Np1;k++) oldPartIdxsStillNotPropragated[k]=k;
00712
00713 const size_t Np = stateSpaceBinsLastTimestepParticles.size();
00714 vector_size_t permutationPathsAuxVector(Np);
00715 for (size_t k=0;k<Np;k++) permutationPathsAuxVector[k]=k;
00716
00717
00718
00719 std::random_shuffle(
00720 permutationPathsAuxVector.begin(),
00721 permutationPathsAuxVector.end(),
00722 mrpt::random::random_generator_for_STL );
00723
00724 size_t k = 0;
00725 size_t N = 0;
00726
00727 TSetStateSpaceBins stateSpaceBins;
00728
00729 do
00730 {
00731
00732
00733
00734
00735
00736
00737
00738 if (doResample)
00739 {
00740 k = me->fastDrawSample(PF_options);
00741 }
00742 else
00743 {
00744
00745
00746 if (permutationPathsAuxVector.size())
00747 {
00748 const size_t idxBinSpacePath = *permutationPathsAuxVector.rbegin();
00749 permutationPathsAuxVector.resize(permutationPathsAuxVector.size()-1);
00750
00751 const size_t idx = randomGenerator.drawUniform32bit() % stateSpaceBinsLastTimestepParticles[idxBinSpacePath].size();
00752 k = stateSpaceBinsLastTimestepParticles[idxBinSpacePath][idx];
00753 ASSERT_(k<me->m_particles.size());
00754
00755
00756 oldPartIdxsStillNotPropragated.erase(std::find(oldPartIdxsStillNotPropragated.begin(),oldPartIdxsStillNotPropragated.end(),k));
00757 }
00758 else
00759 {
00760
00761
00762
00763
00764
00765
00766
00767 if (oldPartIdxsStillNotPropragated.size())
00768 {
00769 const size_t idx = randomGenerator.drawUniform32bit() % oldPartIdxsStillNotPropragated.size();
00770 vector_size_t::iterator it = oldPartIdxsStillNotPropragated.begin() + idx;
00771 k = *it;
00772 oldPartIdxsStillNotPropragated.erase(it);
00773 }
00774 else
00775 {
00776
00777 k = randomGenerator.drawUniform32bit() % me->m_particles.size();
00778 }
00779 }
00780 }
00781
00782
00783
00784 CPose3D newPose;
00785 double newParticleLogWeight;
00786 PF_SLAM_aux_perform_one_rejection_sampling_step<BINTYPE>(
00787 USE_OPTIMAL_SAMPLING,doResample,maxMeanLik,
00788 k,
00789 sf,PF_options,
00790 newPose, newParticleLogWeight);
00791
00792
00793 newParticles.push_back( newPose );
00794 newParticlesDerivedFromIdx.push_back( k );
00795 newParticlesWeight.push_back(newParticleLogWeight);
00796
00797
00798
00799
00800
00801 BINTYPE p;
00802 const TPose3D newPose_s = newPose;
00803 KLF_loadBinFromParticle<PARTICLE_TYPE,BINTYPE>( p,KLD_options, me->m_particles[k].d, &newPose_s );
00804
00805
00806
00807
00808
00809
00810
00811 if ( stateSpaceBins.find(p)==stateSpaceBins.end() )
00812 {
00813
00814 stateSpaceBins.insert( p );
00815
00816
00817 int K = stateSpaceBins.size();
00818 if ( K>1 )
00819 {
00820
00821 Nx = (size_t) (epsilon_1 * math::chi2inv(delta_1,K-1));
00822
00823 }
00824 }
00825
00826 N = newParticles.size();
00827
00828 } while (( N < KLD_options.KLD_maxSampleSize &&
00829 N < max(Nx,minNumSamples_KLD)) ||
00830 (permutationPathsAuxVector.size() && !doResample) );
00831
00832 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);
00833 }
00834
00835
00836
00837
00838
00839
00840
00841 this->PF_SLAM_implementation_replaceByNewParticleSet(
00842 me->m_particles,
00843 newParticles,newParticlesWeight,newParticlesDerivedFromIdx );
00844
00845
00846
00847 me->normalizeWeights();
00848
00849 MRPT_END
00850 }
00851
00852
00853
00854
00855
00856 template <class PARTICLE_TYPE,class MYSELF>
00857 template <class BINTYPE>
00858 void PF_implementation<PARTICLE_TYPE,MYSELF>::PF_SLAM_aux_perform_one_rejection_sampling_step(
00859 const bool USE_OPTIMAL_SAMPLING,
00860 const bool doResample,
00861 const double maxMeanLik,
00862 size_t k,
00863 const CSensoryFrame * sf,
00864 const CParticleFilter::TParticleFilterOptions &PF_options,
00865 CPose3D & out_newPose,
00866 double & out_newParticleLogWeight)
00867 {
00868 MYSELF *me = static_cast<MYSELF*>(this);
00869
00870
00871
00872 while ( ( (USE_OPTIMAL_SAMPLING ? m_pfAuxiliaryPFOptimal_estimatedProb[k] : m_pfAuxiliaryPFStandard_estimatedProb[k] )
00873 -maxMeanLik) < -PF_options.max_loglikelihood_dyn_range )
00874 {
00875
00876 k = randomGenerator.drawUniform32bit() % me->m_particles.size();
00877 if (PF_options.verbose) cout << "[PF_implementation] Warning: Discarding very unlikely particle" << endl;
00878 }
00879
00880 const CPose3D oldPose = *getLastPose(k);
00881
00882
00883
00884
00885 double poseLogLik;
00886 if ( PF_SLAM_implementation_skipRobotMovement() )
00887 {
00888
00889
00890 out_newPose = oldPose;
00891 poseLogLik = 0;
00892 }
00893 else
00894 {
00895 CPose3D movementDraw;
00896 if (!USE_OPTIMAL_SAMPLING)
00897 {
00898 m_movementDrawer.drawSample( movementDraw );
00899 out_newPose.composeFrom(oldPose, movementDraw);
00900
00901 poseLogLik = PF_SLAM_computeObservationLikelihoodForParticle(PF_options, k,*sf,out_newPose);
00902 }
00903 else
00904 {
00905
00906 double acceptanceProb;
00907 int timeout = 0;
00908 const int maxTries=10000;
00909 double bestTryByNow_loglik= -std::numeric_limits<double>::max();
00910 TPose3D bestTryByNow_pose;
00911 do
00912 {
00913
00914 if (PF_options.pfAuxFilterOptimal_MLE && !m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed[k])
00915 {
00916 m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed[k] = true;
00917 movementDraw = CPose3D( m_pfAuxiliaryPFOptimal_maxLikDrawnMovement[k] );
00918 }
00919 else
00920 {
00921
00922 m_movementDrawer.drawSample( movementDraw );
00923 }
00924
00925 out_newPose.composeFrom(oldPose, movementDraw);
00926
00927
00928 poseLogLik = PF_SLAM_computeObservationLikelihoodForParticle(PF_options, k,*sf,out_newPose);
00929
00930 if (poseLogLik>bestTryByNow_loglik)
00931 {
00932 bestTryByNow_loglik = poseLogLik;
00933 bestTryByNow_pose = out_newPose;
00934 }
00935
00936 double ratioLikLik = std::exp( poseLogLik - m_pfAuxiliaryPFOptimal_maxLikelihood[k] );
00937 acceptanceProb = std::min( 1.0, ratioLikLik );
00938
00939 if ( ratioLikLik > 1)
00940 {
00941 m_pfAuxiliaryPFOptimal_maxLikelihood[k] = poseLogLik;
00942
00943 }
00944 } while ( ++timeout<maxTries && acceptanceProb < randomGenerator.drawUniform(0.0,0.999) );
00945
00946 if (timeout>=maxTries)
00947 {
00948 out_newPose = bestTryByNow_pose;
00949 poseLogLik = bestTryByNow_loglik;
00950 if (PF_options.verbose) cout << "[PF_implementation] Warning: timeout in rejection sampling." << endl;
00951 }
00952
00953 }
00954
00955
00956 if (USE_OPTIMAL_SAMPLING)
00957 {
00958 if (doResample)
00959 out_newParticleLogWeight = 0;
00960 else
00961 {
00962 const double weightFact = m_pfAuxiliaryPFOptimal_estimatedProb[k] * PF_options.powFactor;
00963 out_newParticleLogWeight = me->m_particles[k].log_w + weightFact;
00964 }
00965 }
00966 else
00967 {
00968 const double weightFact = (poseLogLik-m_pfAuxiliaryPFStandard_estimatedProb[k]) * PF_options.powFactor;
00969 if (doResample)
00970 out_newParticleLogWeight = weightFact;
00971 else out_newParticleLogWeight = weightFact + me->m_particles[k].log_w;
00972 }
00973
00974 }
00975
00976 }
00977
00978
00979 }
00980 }
00981
00982 #endif