500 const bool USE_OPTIMAL_SAMPLING )
503 typedef std::set<BINTYPE,typename BINTYPE::lt_operator> TSetStateSpaceBins;
505 MYSELF *me =
static_cast<MYSELF*
>(
this);
507 const size_t M = me->m_particles.size();
515 if (!PF_SLAM_implementation_gatherActionsCheckBothActObs<BINTYPE>(actions,sf))
529 m_pfAuxiliaryPFOptimal_maxLikDrawnMovement.resize(M);
530 m_pfAuxiliaryPFOptimal_estimatedProb.resize(M);
531 m_pfAuxiliaryPFStandard_estimatedProb.resize(M);
534 CPose3D meanRobotMovement;
535 m_movementDrawer.getSamplingMean3D(meanRobotMovement);
539 CParticleFilterCapable::TParticleProbabilityEvaluator funcOpt = &TMyClass::template PF_SLAM_particlesEvaluator_AuxPFOptimal<BINTYPE>;
540 CParticleFilterCapable::TParticleProbabilityEvaluator funcStd = &TMyClass::template PF_SLAM_particlesEvaluator_AuxPFStandard<BINTYPE>;
542 me->prepareFastDrawSample(
544 USE_OPTIMAL_SAMPLING ? funcOpt : funcStd,
550 if (USE_OPTIMAL_SAMPLING && PF_options.
verbose)
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) );
570 vector<TPose3D> newParticles;
571 vector<double> newParticlesWeight;
572 vector<size_t> newParticlesDerivedFromIdx;
580 m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed.assign(M,
false);
583 USE_OPTIMAL_SAMPLING ?
584 m_pfAuxiliaryPFOptimal_estimatedProb :
585 m_pfAuxiliaryPFStandard_estimatedProb );
592 newParticles.resize(M);
593 newParticlesWeight.resize(M);
594 newParticlesDerivedFromIdx.resize(M);
596 const bool doResample = me->ESS() < PF_options.
BETA;
598 for (
size_t i=0;i<M;i++)
606 k = me->fastDrawSample(PF_options);
612 double newParticleLogWeight;
613 PF_SLAM_aux_perform_one_rejection_sampling_step<BINTYPE>(
614 USE_OPTIMAL_SAMPLING,doResample,maxMeanLik,
617 newPose, newParticleLogWeight);
620 newParticles[i] = newPose;
621 newParticlesDerivedFromIdx[i] = k;
622 newParticlesWeight[i] = newParticleLogWeight;
635 newParticles.clear();
636 newParticlesWeight.resize(0);
637 newParticlesDerivedFromIdx.clear();
646 TSetStateSpaceBins stateSpaceBinsLastTimestep;
647 std::vector<vector_uint> stateSpaceBinsLastTimestepParticles;
648 typename MYSELF::CParticleList::iterator partIt;
649 unsigned int partIndex;
651 printf(
"[FIXED_SAMPLING] Computing...");
652 for (partIt = me->m_particles.begin(),partIndex=0; partIt!=me->m_particles.end(); ++partIt,++partIndex)
656 KLF_loadBinFromParticle<PARTICLE_TYPE,BINTYPE>(p, KLD_options,partIt->d );
659 typename TSetStateSpaceBins::iterator posFound=stateSpaceBinsLastTimestep.find(p);
660 if ( posFound == stateSpaceBinsLastTimestep.end() )
662 stateSpaceBinsLastTimestep.insert( p );
663 stateSpaceBinsLastTimestepParticles.push_back(
vector_uint(1,partIndex) );
667 const size_t idx = std::distance(stateSpaceBinsLastTimestep.begin(),posFound);
668 stateSpaceBinsLastTimestepParticles[idx].push_back( partIndex );
671 printf(
"done (%u bins in t-1)\n",(
unsigned int)stateSpaceBinsLastTimestep.size());
676 double delta_1 = 1.0 - KLD_options.
KLD_delta;
678 bool doResample = me->ESS() < 0.5;
683 size_t Nx = minNumSamples_KLD ;
685 const size_t Np1 = me->m_particles.size();
687 for (
size_t k=0;k<Np1;k++) oldPartIdxsStillNotPropragated[k]=k;
689 const size_t Np = stateSpaceBinsLastTimestepParticles.size();
691 for (
size_t k=0;k<Np;k++) permutationPathsAuxVector[k]=k;
696 permutationPathsAuxVector.begin(),
697 permutationPathsAuxVector.end(),
703 TSetStateSpaceBins stateSpaceBins;
716 k = me->fastDrawSample(PF_options);
722 if (permutationPathsAuxVector.size())
724 const size_t idxBinSpacePath = *permutationPathsAuxVector.rbegin();
725 permutationPathsAuxVector.resize(permutationPathsAuxVector.size()-1);
728 k = stateSpaceBinsLastTimestepParticles[idxBinSpacePath][idx];
729 ASSERT_(k<me->m_particles.size());
732 oldPartIdxsStillNotPropragated.erase(std::find(oldPartIdxsStillNotPropragated.begin(),oldPartIdxsStillNotPropragated.end(),k));
743 if (oldPartIdxsStillNotPropragated.size())
746 vector_size_t::iterator it = oldPartIdxsStillNotPropragated.begin() + idx;
748 oldPartIdxsStillNotPropragated.erase(it);
761 double newParticleLogWeight;
762 PF_SLAM_aux_perform_one_rejection_sampling_step<BINTYPE>(
763 USE_OPTIMAL_SAMPLING,doResample,maxMeanLik,
766 newPose, newParticleLogWeight);
769 newParticles.push_back( newPose );
770 newParticlesDerivedFromIdx.push_back( k );
771 newParticlesWeight.push_back(newParticleLogWeight);
778 const TPose3D newPose_s = newPose;
779 KLF_loadBinFromParticle<PARTICLE_TYPE,BINTYPE>( p,KLD_options, me->m_particles[k].d, &newPose_s );
787 if ( stateSpaceBins.find(p)==stateSpaceBins.end() )
790 stateSpaceBins.insert( p );
793 int K = stateSpaceBins.size();
802 N = newParticles.size();
805 N < max(Nx,minNumSamples_KLD)) ||
806 (permutationPathsAuxVector.size() && !doResample) );
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);
817 this->PF_SLAM_implementation_replaceByNewParticleSet(
819 newParticles,newParticlesWeight,newParticlesDerivedFromIdx );
823 me->normalizeWeights();
835 const bool USE_OPTIMAL_SAMPLING,
836 const bool doResample,
837 const double maxMeanLik,
842 double & out_newParticleLogWeight)
844 MYSELF *me =
static_cast<MYSELF*
>(
this);
848 while ( ( (USE_OPTIMAL_SAMPLING ? m_pfAuxiliaryPFOptimal_estimatedProb[k] : m_pfAuxiliaryPFStandard_estimatedProb[k] )
853 if (PF_options.
verbose) cout <<
"[PF_implementation] Warning: Discarding very unlikely particle" << endl;
862 if ( PF_SLAM_implementation_skipRobotMovement() )
866 out_newPose = oldPose;
871 CPose3D movementDraw;
872 if (!USE_OPTIMAL_SAMPLING)
874 m_movementDrawer.drawSample( movementDraw );
877 poseLogLik = PF_SLAM_computeObservationLikelihoodForParticle(PF_options, k,*sf,out_newPose);
882 double acceptanceProb;
884 const int maxTries=10000;
885 double bestTryByNow_loglik= -std::numeric_limits<double>::max();
886 TPose3D bestTryByNow_pose;
892 m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed[k] =
true;
893 movementDraw = CPose3D( m_pfAuxiliaryPFOptimal_maxLikDrawnMovement[k] );
898 m_movementDrawer.drawSample( movementDraw );
904 poseLogLik = PF_SLAM_computeObservationLikelihoodForParticle(PF_options, k,*sf,out_newPose);
906 if (poseLogLik>bestTryByNow_loglik)
908 bestTryByNow_loglik = poseLogLik;
909 bestTryByNow_pose = out_newPose;
912 double ratioLikLik = std::exp( poseLogLik - m_pfAuxiliaryPFOptimal_maxLikelihood[k] );
913 acceptanceProb = std::min( 1.0, ratioLikLik );
915 if ( ratioLikLik > 1)
917 m_pfAuxiliaryPFOptimal_maxLikelihood[k] = poseLogLik;
922 if (timeout>=maxTries)
924 out_newPose = bestTryByNow_pose;
925 poseLogLik = bestTryByNow_loglik;
926 if (PF_options.
verbose) cout <<
"[PF_implementation] Warning: timeout in rejection sampling." << endl;
932 if (USE_OPTIMAL_SAMPLING)
935 out_newParticleLogWeight = 0;
938 const double weightFact = m_pfAuxiliaryPFOptimal_estimatedProb[k] * PF_options.
powFactor;
939 out_newParticleLogWeight = me->m_particles[k].log_w + weightFact;
944 const double weightFact = (poseLogLik-m_pfAuxiliaryPFStandard_estimatedProb[k]) * PF_options.
powFactor;
946 out_newParticleLogWeight = weightFact;
947 else out_newParticleLogWeight = weightFact + me->m_particles[k].log_w;