10 #ifndef PF_implementations_H
11 #define PF_implementations_H
46 class PARTICLE_TYPE,
class MYSELF,
48 template <
class BINTYPE>
54 MYSELF* me =
static_cast<MYSELF*
>(
this);
56 if (actions !=
nullptr)
62 if (m_accumRobotMovement3DIsValid)
65 ASSERT_(robotMovement2D->poseChange);
66 if (!m_accumRobotMovement2DIsValid)
68 robotMovement2D->poseChange->getMean(
69 m_accumRobotMovement2D.rawOdometryIncrementReading);
70 m_accumRobotMovement2D.motionModelConfiguration =
71 robotMovement2D->motionModelConfiguration;
74 m_accumRobotMovement2D.rawOdometryIncrementReading +=
75 robotMovement2D->poseChange->getMeanVal();
77 m_accumRobotMovement2DIsValid =
true;
85 if (m_accumRobotMovement2DIsValid)
88 if (!m_accumRobotMovement3DIsValid)
89 m_accumRobotMovement3D = robotMovement3D->poseChange;
91 m_accumRobotMovement3D += robotMovement3D->poseChange;
95 m_accumRobotMovement3DIsValid =
true;
102 const bool SFhasValidObservations =
103 (sf ==
nullptr) ?
false
104 : PF_SLAM_implementation_doWeHaveValidObservations(
105 me->m_particles, sf);
108 if (!((m_accumRobotMovement2DIsValid || m_accumRobotMovement3DIsValid) &&
109 SFhasValidObservations))
114 if (m_accumRobotMovement3DIsValid)
116 m_movementDrawer.setPosePDF(
117 m_accumRobotMovement3D);
118 m_accumRobotMovement3DIsValid =
125 m_accumRobotMovement2D.rawOdometryIncrementReading,
126 m_accumRobotMovement2D.motionModelConfiguration);
129 m_movementDrawer.setPosePDF(
131 m_accumRobotMovement2DIsValid =
154 template <
class PARTICLE_TYPE,
class MYSELF,
156 template <
class BINTYPE>
166 PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal<BINTYPE>(
167 actions, sf, PF_options, KLD_options,
true );
177 template <
class PARTICLE_TYPE,
class MYSELF,
179 template <
class BINTYPE>
188 using TSetStateSpaceBins = std::set<BINTYPE, typename BINTYPE::lt_operator>;
190 MYSELF* me =
static_cast<MYSELF*
>(
this);
210 ASSERT_(robotMovement2D->poseChange);
211 m_movementDrawer.setPosePDF(
212 robotMovement2D->poseChange.get_ptr());
214 robotMovement2D->poseChange->getMeanVal());
223 m_movementDrawer.setPosePDF(robotMovement3D->poseChange);
229 "Action list does not contain any "
230 "CActionRobotMovement2D or CActionRobotMovement3D "
239 const size_t M = me->m_particles.size();
244 for (
size_t i = 0; i < M; i++)
248 m_movementDrawer.drawSample(incrPose);
258 PF_SLAM_implementation_custom_update_particle_with_new_pose(
259 me->m_particles[i].d.get(), finalPose.
asTPose());
263 PF_SLAM_implementation_custom_update_particle_with_new_pose(
264 &me->m_particles[i].d, finalPose.
asTPose());
276 TSetStateSpaceBins stateSpaceBins;
279 const double delta_1 = 1.0 - KLD_options.
KLD_delta;
280 const double epsilon_1 = 0.5 / KLD_options.
KLD_epsilon;
283 me->prepareFastDrawSample(PF_options);
286 std::vector<mrpt::math::TPose3D> newParticles;
287 std::vector<double> newParticlesWeight;
288 std::vector<size_t> newParticlesDerivedFromIdx;
296 m_movementDrawer.drawSample(increment_i);
299 const size_t drawn_idx = me->fastDrawSample(PF_options);
304 getLastPose(drawn_idx, pose_is_valid)) +
309 newParticles.push_back(newPose_s);
310 newParticlesWeight.push_back(0);
311 newParticlesDerivedFromIdx.push_back(drawn_idx);
315 const PARTICLE_TYPE *part;
317 part = me->m_particles[drawn_idx].d.get();
318 else part = &me->m_particles[drawn_idx].d;
321 KLF_loadBinFromParticle<PARTICLE_TYPE, BINTYPE>(
322 p, KLD_options, part, &newPose_s);
324 if (stateSpaceBins.find(
p) == stateSpaceBins.end())
328 stateSpaceBins.insert(
p);
331 size_t K = stateSpaceBins.size();
341 N = newParticles.size();
351 this->PF_SLAM_implementation_replaceByNewParticleSet(
352 me->m_particles, newParticles, newParticlesWeight,
353 newParticlesDerivedFromIdx);
360 const size_t M = me->m_particles.size();
364 for (
size_t i = 0; i < M; i++)
368 getLastPose(i, pose_is_valid);
370 const double obs_log_likelihood =
371 PF_SLAM_computeObservationLikelihoodForParticle(
372 PF_options, i, *sf, partPose2);
373 me->m_particles[i].log_w +=
374 obs_log_likelihood * PF_options.
powFactor;
399 template <
class PARTICLE_TYPE,
class MYSELF,
401 template <
class BINTYPE>
411 PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal<BINTYPE>(
412 actions, sf, PF_options, KLD_options,
false );
418 template <
class PARTICLE_TYPE,
class MYSELF,
420 template <
class BINTYPE>
425 const void* action,
const void* observation)
432 const MYSELF* me =
static_cast<const MYSELF*
>(
obj);
438 double indivLik, maxLik = -1e300;
449 for (
size_t q = 0;
q < N;
q++)
451 me->m_movementDrawer.drawSample(drawnSample);
455 indivLik = me->PF_SLAM_computeObservationLikelihoodForParticle(
461 vectLiks[
q] = indivLik;
462 if (indivLik > maxLik)
464 maxLikDraw = drawnSample;
475 me->m_pfAuxiliaryPFOptimal_estimatedProb[
index] =
477 me->m_pfAuxiliaryPFOptimal_maxLikelihood[
index] = maxLik;
480 me->m_pfAuxiliaryPFOptimal_maxLikDrawnMovement[
index] =
485 return me->m_particles[
index].log_w +
486 me->m_pfAuxiliaryPFOptimal_estimatedProb[
index];
497 template <
class PARTICLE_TYPE,
class MYSELF,
499 template <
class BINTYPE>
504 const void* action,
const void* observation)
510 const MYSELF* myObj =
static_cast<const MYSELF*
>(
obj);
513 const double cur_logweight = myObj->m_particles[
index].log_w;
528 myObj->m_pfAuxiliaryPFStandard_estimatedProb[
index] =
529 myObj->PF_SLAM_computeObservationLikelihoodForParticle(
535 return cur_logweight +
536 myObj->m_pfAuxiliaryPFStandard_estimatedProb[
index];
546 double indivLik, maxLik = -1e300;
554 for (
size_t q = 0;
q < N;
q++)
556 myObj->m_movementDrawer.drawSample(drawnSample);
560 indivLik = myObj->PF_SLAM_computeObservationLikelihoodForParticle(
566 vectLiks[
q] = indivLik;
567 if (indivLik > maxLik)
569 maxLikDraw = drawnSample;
580 myObj->m_pfAuxiliaryPFStandard_estimatedProb[
index] =
583 myObj->m_pfAuxiliaryPFOptimal_maxLikelihood[
index] = maxLik;
585 myObj->m_pfAuxiliaryPFOptimal_maxLikDrawnMovement[
index] =
590 return cur_logweight +
591 myObj->m_pfAuxiliaryPFOptimal_estimatedProb[
index];
599 template <
class PARTICLE_TYPE,
class MYSELF,
601 template <
class BINTYPE>
607 const TKLDParams& KLD_options,
const bool USE_OPTIMAL_SAMPLING)
610 using TSetStateSpaceBins = std::set<BINTYPE, typename BINTYPE::lt_operator>;
612 MYSELF* me =
static_cast<MYSELF*
>(
this);
614 const size_t M = me->m_particles.size();
622 if (!PF_SLAM_implementation_gatherActionsCheckBothActObs<BINTYPE>(
637 m_pfAuxiliaryPFOptimal_maxLikDrawnMovement.resize(M);
638 m_pfAuxiliaryPFOptimal_estimatedProb.resize(M);
639 m_pfAuxiliaryPFStandard_estimatedProb.resize(M);
643 m_movementDrawer.getSamplingMean3D(meanRobotMovement);
648 &TMyClass::template PF_SLAM_particlesEvaluator_AuxPFOptimal<BINTYPE>;
650 &TMyClass::template PF_SLAM_particlesEvaluator_AuxPFStandard<BINTYPE>;
652 me->prepareFastDrawSample(
653 PF_options, USE_OPTIMAL_SAMPLING ? funcOpt : funcStd,
654 &meanRobotMovement, sf);
659 if (USE_OPTIMAL_SAMPLING &&
665 "[prepareFastDrawSample] max (log) = %10.06f\n",
670 "[prepareFastDrawSample] max-mean (log) = %10.06f\n",
671 -
math::mean(m_pfAuxiliaryPFOptimal_estimatedProb) +
676 "[prepareFastDrawSample] max-min (log) = %10.06f\n",
696 std::vector<mrpt::math::TPose3D> newParticles;
697 std::vector<double> newParticlesWeight;
698 std::vector<size_t> newParticlesDerivedFromIdx;
706 m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed.assign(M,
false);
709 USE_OPTIMAL_SAMPLING ? m_pfAuxiliaryPFOptimal_estimatedProb
710 : m_pfAuxiliaryPFStandard_estimatedProb);
717 newParticles.resize(M);
718 newParticlesWeight.resize(M);
719 newParticlesDerivedFromIdx.resize(M);
721 const bool doResample = me->ESS() < PF_options.
BETA;
723 for (
size_t i = 0; i < M; i++)
731 k = me->fastDrawSample(
739 double newParticleLogWeight;
740 PF_SLAM_aux_perform_one_rejection_sampling_step<BINTYPE>(
741 USE_OPTIMAL_SAMPLING, doResample, maxMeanLik, k, sf, PF_options,
742 newPose, newParticleLogWeight);
745 newParticles[i] = newPose.
asTPose();
746 newParticlesDerivedFromIdx[i] = k;
747 newParticlesWeight[i] = newParticleLogWeight;
760 newParticles.clear();
761 newParticlesWeight.resize(0);
762 newParticlesDerivedFromIdx.clear();
776 TSetStateSpaceBins stateSpaceBinsLastTimestep;
777 std::vector<std::vector<uint32_t>> stateSpaceBinsLastTimestepParticles;
779 unsigned int partIndex;
782 for (partIt = me->m_particles.begin(), partIndex = 0;
783 partIt != me->m_particles.end(); ++partIt, ++partIndex)
786 const PARTICLE_TYPE *part;
788 part = partIt->d.get();
789 else part = &partIt->d;
792 KLF_loadBinFromParticle<PARTICLE_TYPE, BINTYPE>(
793 p, KLD_options, part);
797 stateSpaceBinsLastTimestep.find(
p);
798 if (posFound == stateSpaceBinsLastTimestep.end())
800 stateSpaceBinsLastTimestep.insert(
p);
801 stateSpaceBinsLastTimestepParticles.push_back(
802 std::vector<uint32_t>(1, partIndex));
808 stateSpaceBinsLastTimestepParticles[idx].push_back(partIndex);
814 "[FIXED_SAMPLING] done (%u bins in t-1)\n",
815 (
unsigned int)stateSpaceBinsLastTimestep.size()));
820 double delta_1 = 1.0 - KLD_options.
KLD_delta;
822 bool doResample = me->ESS() < PF_options.
BETA;
829 const size_t minNumSamples_KLD = std::max(
833 stateSpaceBinsLastTimestep.size()));
834 size_t Nx = minNumSamples_KLD;
836 const size_t Np1 = me->m_particles.size();
837 std::vector<size_t> oldPartIdxsStillNotPropragated(
839 for (
size_t k = 0; k < Np1; k++)
840 oldPartIdxsStillNotPropragated[k] = k;
842 const size_t Np = stateSpaceBinsLastTimestepParticles.size();
843 std::vector<size_t> permutationPathsAuxVector(Np);
844 for (
size_t k = 0; k < Np; k++) permutationPathsAuxVector[k] = k;
850 permutationPathsAuxVector.begin(), permutationPathsAuxVector.end());
855 TSetStateSpaceBins stateSpaceBins;
869 k = me->fastDrawSample(
877 if (permutationPathsAuxVector.size())
879 const size_t idxBinSpacePath =
880 *permutationPathsAuxVector.rbegin();
881 permutationPathsAuxVector.resize(
882 permutationPathsAuxVector.size() - 1);
886 stateSpaceBinsLastTimestepParticles[idxBinSpacePath]
888 k = stateSpaceBinsLastTimestepParticles[idxBinSpacePath]
890 ASSERT_(k < me->m_particles.size());
893 oldPartIdxsStillNotPropragated.erase(
895 oldPartIdxsStillNotPropragated.begin(),
896 oldPartIdxsStillNotPropragated.end(), k));
911 if (oldPartIdxsStillNotPropragated.size())
916 oldPartIdxsStillNotPropragated.size();
918 oldPartIdxsStillNotPropragated.begin() +
921 oldPartIdxsStillNotPropragated.erase(it);
928 me->m_particles.size();
936 double newParticleLogWeight;
937 PF_SLAM_aux_perform_one_rejection_sampling_step<BINTYPE>(
938 USE_OPTIMAL_SAMPLING, doResample, maxMeanLik, k, sf, PF_options,
939 newPose, newParticleLogWeight);
942 newParticles.push_back(newPose.
asTPose());
943 newParticlesDerivedFromIdx.push_back(k);
944 newParticlesWeight.push_back(newParticleLogWeight);
950 const PARTICLE_TYPE *part;
952 part = me->m_particles[k].d.get();
953 else part = &me->m_particles[k].d;
957 KLF_loadBinFromParticle<PARTICLE_TYPE, BINTYPE>(
958 p, KLD_options, part, &newPose_s);
967 if (stateSpaceBins.find(
p) == stateSpaceBins.end())
970 stateSpaceBins.insert(
p);
973 int K = stateSpaceBins.size();
983 N = newParticles.size();
986 N < std::max(Nx, minNumSamples_KLD)) ||
987 (permutationPathsAuxVector.size() && !doResample));
992 "[ADAPTIVE SAMPLE SIZE] #Bins: %u \t #Particles: %u \t "
994 static_cast<unsigned>(stateSpaceBins.size()),
995 static_cast<unsigned>(N), (
unsigned)Nx));
1004 this->PF_SLAM_implementation_replaceByNewParticleSet(
1005 me->m_particles, newParticles, newParticlesWeight,
1006 newParticlesDerivedFromIdx);
1009 me->normalizeWeights();
1017 template <
class PARTICLE_TYPE,
class MYSELF,
1019 template <
class BINTYPE>
1022 const bool USE_OPTIMAL_SAMPLING,
const bool doResample,
1023 const double maxMeanLik,
1029 MYSELF* me =
static_cast<MYSELF*
>(
this);
1034 while (((USE_OPTIMAL_SAMPLING ? m_pfAuxiliaryPFOptimal_estimatedProb[k]
1035 : m_pfAuxiliaryPFStandard_estimatedProb[k]) -
1040 me->m_particles.size();
1043 "[PF_SLAM_aux_perform_one_rejection_sampling_step] Warning: "
1044 "Discarding very unlikely particle.");
1049 getLastPose(k, pose_is_valid));
1056 if (PF_SLAM_implementation_skipRobotMovement())
1061 out_newPose = oldPose;
1067 if (!USE_OPTIMAL_SAMPLING)
1069 m_movementDrawer.drawSample(movementDraw);
1071 oldPose, movementDraw);
1073 poseLogLik = PF_SLAM_computeObservationLikelihoodForParticle(
1074 PF_options, k, *sf, out_newPose);
1079 double acceptanceProb;
1081 const int maxTries = 10000;
1082 double bestTryByNow_loglik = -std::numeric_limits<double>::max();
1088 !m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed[k])
1091 m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed[k] =
1094 m_pfAuxiliaryPFOptimal_maxLikDrawnMovement[k]);
1099 m_movementDrawer.drawSample(movementDraw);
1107 poseLogLik = PF_SLAM_computeObservationLikelihoodForParticle(
1108 PF_options, k, *sf, out_newPose);
1110 if (poseLogLik > bestTryByNow_loglik)
1112 bestTryByNow_loglik = poseLogLik;
1113 bestTryByNow_pose = out_newPose.
asTPose();
1116 double ratioLikLik = std::exp(
1117 poseLogLik - m_pfAuxiliaryPFOptimal_maxLikelihood[k]);
1118 acceptanceProb =
std::min(1.0, ratioLikLik);
1120 if (ratioLikLik > 1)
1122 m_pfAuxiliaryPFOptimal_maxLikelihood[k] =
1128 ++timeout < maxTries &&
1132 if (timeout >= maxTries)
1135 poseLogLik = bestTryByNow_loglik;
1138 "[PF_implementation] Warning: timeout in rejection "
1144 if (USE_OPTIMAL_SAMPLING)
1147 out_newParticleLogWeight = 0;
1152 const double weightFact =
1153 m_pfAuxiliaryPFOptimal_estimatedProb[k] *
1155 out_newParticleLogWeight =
1156 me->m_particles[k].log_w + weightFact;
1161 const double weightFact =
1162 (poseLogLik - m_pfAuxiliaryPFStandard_estimatedProb[k]) *
1165 out_newParticleLogWeight = weightFact;
1167 out_newParticleLogWeight =
1168 weightFact + me->m_particles[k].log_w;