42 class PARTICLE_TYPE,
class MYSELF,
44 template <
class BINTYPE>
50 auto* me =
static_cast<MYSELF*
>(
this);
52 if (actions !=
nullptr)
58 if (m_accumRobotMovement3DIsValid)
61 ASSERT_(robotMovement2D->poseChange);
62 if (!m_accumRobotMovement2DIsValid)
64 robotMovement2D->poseChange->getMean(
65 m_accumRobotMovement2D.rawOdometryIncrementReading);
66 m_accumRobotMovement2D.motionModelConfiguration =
67 robotMovement2D->motionModelConfiguration;
70 m_accumRobotMovement2D.rawOdometryIncrementReading +=
71 robotMovement2D->poseChange->getMeanVal();
73 m_accumRobotMovement2DIsValid =
true;
81 if (m_accumRobotMovement2DIsValid)
84 if (!m_accumRobotMovement3DIsValid)
85 m_accumRobotMovement3D = robotMovement3D->poseChange;
87 m_accumRobotMovement3D += robotMovement3D->poseChange;
91 m_accumRobotMovement3DIsValid =
true;
98 const bool SFhasValidObservations =
99 (sf ==
nullptr) ?
false
100 : PF_SLAM_implementation_doWeHaveValidObservations(
101 me->m_particles, sf);
104 if (!((m_accumRobotMovement2DIsValid || m_accumRobotMovement3DIsValid) &&
105 SFhasValidObservations))
110 if (m_accumRobotMovement3DIsValid)
112 m_movementDrawer.setPosePDF(
113 m_accumRobotMovement3D);
114 m_accumRobotMovement3DIsValid =
121 m_accumRobotMovement2D.rawOdometryIncrementReading,
122 m_accumRobotMovement2D.motionModelConfiguration);
125 m_movementDrawer.setPosePDF(
127 m_accumRobotMovement2DIsValid =
151 class PARTICLE_TYPE,
class MYSELF,
153 template <
class BINTYPE>
163 PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal<BINTYPE>(
164 actions, sf, PF_options, KLD_options,
true );
175 class PARTICLE_TYPE,
class MYSELF,
177 template <
class BINTYPE>
186 using TSetStateSpaceBins = std::set<BINTYPE, typename BINTYPE::lt_operator>;
188 auto* me =
static_cast<MYSELF*
>(
this);
208 ASSERT_(robotMovement2D->poseChange);
209 m_movementDrawer.setPosePDF(*robotMovement2D->poseChange);
211 robotMovement2D->poseChange->getMeanVal());
220 m_movementDrawer.setPosePDF(robotMovement3D->poseChange);
226 "Action list does not contain any "
227 "CActionRobotMovement2D or CActionRobotMovement3D "
236 const size_t M = me->m_particles.size();
241 for (
size_t i = 0; i < M; i++)
245 m_movementDrawer.drawSample(incrPose);
256 PF_SLAM_implementation_custom_update_particle_with_new_pose(
257 me->m_particles[i].d.get(), finalPose.
asTPose());
261 PF_SLAM_implementation_custom_update_particle_with_new_pose(
262 &me->m_particles[i].d, finalPose.
asTPose());
274 TSetStateSpaceBins stateSpaceBins;
277 const double delta_1 = 1.0 - KLD_options.
KLD_delta;
278 const double epsilon_1 = 0.5 / KLD_options.
KLD_epsilon;
281 me->prepareFastDrawSample(PF_options);
284 std::vector<mrpt::math::TPose3D> newParticles;
285 std::vector<double> newParticlesWeight;
286 std::vector<size_t> newParticlesDerivedFromIdx;
294 m_movementDrawer.drawSample(increment_i);
297 const size_t drawn_idx = me->fastDrawSample(PF_options);
302 getLastPose(drawn_idx, pose_is_valid)) +
307 newParticles.push_back(newPose_s);
308 newParticlesWeight.push_back(0);
309 newParticlesDerivedFromIdx.push_back(drawn_idx);
313 const PARTICLE_TYPE* part;
316 part = me->m_particles[drawn_idx].d.get();
318 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_lik =
371 PF_SLAM_computeObservationLikelihoodForParticle(
372 PF_options, i, *sf, partPose2);
373 ASSERT_(!std::isnan(obs_log_lik) && std::isfinite(obs_log_lik));
374 me->m_particles[i].log_w += obs_log_lik * PF_options.
powFactor;
400 class PARTICLE_TYPE,
class MYSELF,
402 template <
class BINTYPE>
412 PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal<BINTYPE>(
413 actions, sf, PF_options, KLD_options,
false );
420 class PARTICLE_TYPE,
class MYSELF,
422 template <
class BINTYPE>
427 [[maybe_unused]]
const void* action,
const void* observation)
433 const auto* me =
static_cast<const MYSELF*
>(obj);
439 double indivLik, maxLik = -1e300;
450 for (
size_t q = 0; q < N; q++)
452 me->m_movementDrawer.drawSample(drawnSample);
456 indivLik = me->PF_SLAM_computeObservationLikelihoodForParticle(
462 vectLiks[q] = indivLik;
463 if (indivLik > maxLik)
465 maxLikDraw = drawnSample;
476 me->m_pfAuxiliaryPFOptimal_estimatedProb[index] =
478 me->m_pfAuxiliaryPFOptimal_maxLikelihood[index] = maxLik;
481 me->m_pfAuxiliaryPFOptimal_maxLikDrawnMovement[index] =
486 return me->m_particles[index].log_w +
487 me->m_pfAuxiliaryPFOptimal_estimatedProb[index];
499 class PARTICLE_TYPE,
class MYSELF,
501 template <
class BINTYPE>
506 const void* action,
const void* observation)
512 const auto* myObj =
static_cast<const MYSELF*
>(obj);
515 const double cur_logweight = myObj->m_particles[index].log_w;
530 myObj->m_pfAuxiliaryPFStandard_estimatedProb[index] =
531 myObj->PF_SLAM_computeObservationLikelihoodForParticle(
537 return cur_logweight +
538 myObj->m_pfAuxiliaryPFStandard_estimatedProb[index];
548 double indivLik, maxLik = -1e300;
556 for (
size_t q = 0; q < N; q++)
558 myObj->m_movementDrawer.drawSample(drawnSample);
562 indivLik = myObj->PF_SLAM_computeObservationLikelihoodForParticle(
568 vectLiks[q] = indivLik;
569 if (indivLik > maxLik)
571 maxLikDraw = drawnSample;
582 myObj->m_pfAuxiliaryPFStandard_estimatedProb[index] =
585 myObj->m_pfAuxiliaryPFOptimal_maxLikelihood[index] = maxLik;
587 myObj->m_pfAuxiliaryPFOptimal_maxLikDrawnMovement[index] =
592 return cur_logweight +
593 myObj->m_pfAuxiliaryPFOptimal_estimatedProb[index];
602 class PARTICLE_TYPE,
class MYSELF,
604 template <
class BINTYPE>
610 const TKLDParams& KLD_options,
const bool USE_OPTIMAL_SAMPLING)
613 using TSetStateSpaceBins = std::set<BINTYPE, typename BINTYPE::lt_operator>;
615 auto* me =
static_cast<MYSELF*
>(
this);
617 const size_t M = me->m_particles.size();
625 if (!PF_SLAM_implementation_gatherActionsCheckBothActObs<BINTYPE>(
640 m_pfAuxiliaryPFOptimal_maxLikDrawnMovement.resize(M);
641 m_pfAuxiliaryPFOptimal_estimatedProb.resize(M);
642 m_pfAuxiliaryPFStandard_estimatedProb.resize(M);
646 m_movementDrawer.getSamplingMean3D(meanRobotMovement);
651 &TMyClass::template PF_SLAM_particlesEvaluator_AuxPFOptimal<BINTYPE>;
653 &TMyClass::template PF_SLAM_particlesEvaluator_AuxPFStandard<BINTYPE>;
655 me->prepareFastDrawSample(
656 PF_options, USE_OPTIMAL_SAMPLING ? funcOpt : funcStd,
657 &meanRobotMovement, sf);
662 if (USE_OPTIMAL_SAMPLING &&
668 "[prepareFastDrawSample] max (log) = %10.06f\n",
673 "[prepareFastDrawSample] max-mean (log) = %10.06f\n",
674 -
math::mean(m_pfAuxiliaryPFOptimal_estimatedProb) +
679 "[prepareFastDrawSample] max-min (log) = %10.06f\n",
699 std::vector<mrpt::math::TPose3D> newParticles;
700 std::vector<double> newParticlesWeight;
701 std::vector<size_t> newParticlesDerivedFromIdx;
709 m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed.assign(M,
false);
712 USE_OPTIMAL_SAMPLING ? m_pfAuxiliaryPFOptimal_estimatedProb
713 : m_pfAuxiliaryPFStandard_estimatedProb);
720 newParticles.resize(M);
721 newParticlesWeight.resize(M);
722 newParticlesDerivedFromIdx.resize(M);
724 const bool doResample = me->ESS() < PF_options.
BETA;
726 for (
size_t i = 0; i < M; i++)
734 k = me->fastDrawSample(
742 double newParticleLogWeight = 0;
743 PF_SLAM_aux_perform_one_rejection_sampling_step<BINTYPE>(
744 USE_OPTIMAL_SAMPLING, doResample, maxMeanLik, k, sf, PF_options,
745 newPose, newParticleLogWeight);
748 newParticles[i] = newPose.
asTPose();
749 newParticlesDerivedFromIdx[i] = k;
750 newParticlesWeight[i] = newParticleLogWeight;
763 newParticles.clear();
764 newParticlesWeight.resize(0);
765 newParticlesDerivedFromIdx.clear();
779 TSetStateSpaceBins stateSpaceBinsLastTimestep;
780 std::vector<std::vector<uint32_t>> stateSpaceBinsLastTimestepParticles;
781 typename MYSELF::CParticleList::iterator partIt;
782 unsigned int partIndex;
785 for (partIt = me->m_particles.begin(), partIndex = 0;
786 partIt != me->m_particles.end(); ++partIt, ++partIndex)
789 const PARTICLE_TYPE* part;
792 part = partIt->d.get();
797 KLF_loadBinFromParticle<PARTICLE_TYPE, BINTYPE>(
798 p, KLD_options, part);
801 auto posFound = stateSpaceBinsLastTimestep.find(p);
802 if (posFound == stateSpaceBinsLastTimestep.end())
804 stateSpaceBinsLastTimestep.insert(p);
805 stateSpaceBinsLastTimestepParticles.emplace_back(1, partIndex);
811 stateSpaceBinsLastTimestepParticles[idx].push_back(partIndex);
817 "[FIXED_SAMPLING] done (%u bins in t-1)\n",
818 (
unsigned int)stateSpaceBinsLastTimestep.size()));
823 double delta_1 = 1.0 - KLD_options.
KLD_delta;
825 bool doResample = me->ESS() < PF_options.
BETA;
832 const size_t minNumSamples_KLD = std::max(
836 stateSpaceBinsLastTimestep.size()));
837 size_t Nx = minNumSamples_KLD;
839 const size_t Np1 = me->m_particles.size();
840 std::vector<size_t> oldPartIdxsStillNotPropragated(
842 for (
size_t k = 0; k < Np1; k++)
843 oldPartIdxsStillNotPropragated[k] = k;
845 const size_t Np = stateSpaceBinsLastTimestepParticles.size();
846 std::vector<size_t> permutationPathsAuxVector(Np);
847 for (
size_t k = 0; k < Np; k++) permutationPathsAuxVector[k] = k;
853 permutationPathsAuxVector.begin(), permutationPathsAuxVector.end());
858 TSetStateSpaceBins stateSpaceBins;
872 k = me->fastDrawSample(
880 if (permutationPathsAuxVector.size())
882 const size_t idxBinSpacePath =
883 *permutationPathsAuxVector.rbegin();
884 permutationPathsAuxVector.resize(
885 permutationPathsAuxVector.size() - 1);
889 stateSpaceBinsLastTimestepParticles[idxBinSpacePath]
891 k = stateSpaceBinsLastTimestepParticles[idxBinSpacePath]
893 ASSERT_(k < me->m_particles.size());
896 oldPartIdxsStillNotPropragated.erase(
std::find(
897 oldPartIdxsStillNotPropragated.begin(),
898 oldPartIdxsStillNotPropragated.end(), k));
913 if (oldPartIdxsStillNotPropragated.size())
918 oldPartIdxsStillNotPropragated.size();
919 auto it = oldPartIdxsStillNotPropragated.begin() +
922 oldPartIdxsStillNotPropragated.erase(it);
929 me->m_particles.size();
937 double newParticleLogWeight;
938 PF_SLAM_aux_perform_one_rejection_sampling_step<BINTYPE>(
939 USE_OPTIMAL_SAMPLING, doResample, maxMeanLik, k, sf, PF_options,
940 newPose, newParticleLogWeight);
943 newParticles.push_back(newPose.
asTPose());
944 newParticlesDerivedFromIdx.push_back(k);
945 newParticlesWeight.push_back(newParticleLogWeight);
951 const PARTICLE_TYPE* part;
954 part = me->m_particles[k].d.get();
956 part = &me->m_particles[k].d;
960 KLF_loadBinFromParticle<PARTICLE_TYPE, BINTYPE>(
961 p, KLD_options, part, &newPose_s);
970 if (stateSpaceBins.find(p) == stateSpaceBins.end())
973 stateSpaceBins.insert(p);
976 int K = stateSpaceBins.size();
986 N = newParticles.size();
989 N < std::max(Nx, minNumSamples_KLD)) ||
990 (permutationPathsAuxVector.size() && !doResample));
995 "[ADAPTIVE SAMPLE SIZE] #Bins: %u \t #Particles: %u \t "
997 static_cast<unsigned>(stateSpaceBins.size()),
998 static_cast<unsigned>(N), (
unsigned)Nx));
1007 this->PF_SLAM_implementation_replaceByNewParticleSet(
1008 me->m_particles, newParticles, newParticlesWeight,
1009 newParticlesDerivedFromIdx);
1012 me->normalizeWeights();
1021 class PARTICLE_TYPE,
class MYSELF,
1023 template <
class BINTYPE>
1026 const bool USE_OPTIMAL_SAMPLING,
const bool doResample,
1027 const double maxMeanLik,
1033 auto* me =
static_cast<MYSELF*
>(
this);
1038 while (((USE_OPTIMAL_SAMPLING ? m_pfAuxiliaryPFOptimal_estimatedProb[k]
1039 : m_pfAuxiliaryPFStandard_estimatedProb[k]) -
1044 me->m_particles.size();
1047 "[PF_SLAM_aux_perform_one_rejection_sampling_step] Warning: "
1048 "Discarding very unlikely particle.");
1053 getLastPose(k, pose_is_valid));
1060 if (PF_SLAM_implementation_skipRobotMovement())
1065 out_newPose = oldPose;
1071 if (!USE_OPTIMAL_SAMPLING)
1073 m_movementDrawer.drawSample(movementDraw);
1075 oldPose, movementDraw);
1077 poseLogLik = PF_SLAM_computeObservationLikelihoodForParticle(
1078 PF_options, k, *sf, out_newPose);
1083 double acceptanceProb;
1085 const int maxTries = 10000;
1086 double bestTryByNow_loglik = -std::numeric_limits<double>::max();
1092 !m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed[k])
1095 m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed[k] =
1098 m_pfAuxiliaryPFOptimal_maxLikDrawnMovement[k]);
1103 m_movementDrawer.drawSample(movementDraw);
1111 poseLogLik = PF_SLAM_computeObservationLikelihoodForParticle(
1112 PF_options, k, *sf, out_newPose);
1114 if (poseLogLik > bestTryByNow_loglik)
1116 bestTryByNow_loglik = poseLogLik;
1117 bestTryByNow_pose = out_newPose.
asTPose();
1120 double ratioLikLik = std::exp(
1121 poseLogLik - m_pfAuxiliaryPFOptimal_maxLikelihood[k]);
1122 acceptanceProb = std::min(1.0, ratioLikLik);
1124 if (ratioLikLik > 1)
1126 m_pfAuxiliaryPFOptimal_maxLikelihood[k] =
1132 ++timeout < maxTries &&
1136 if (timeout >= maxTries)
1139 poseLogLik = bestTryByNow_loglik;
1142 "[PF_implementation] Warning: timeout in rejection "
1148 if (USE_OPTIMAL_SAMPLING)
1151 out_newParticleLogWeight = 0;
1156 const double weightFact =
1157 m_pfAuxiliaryPFOptimal_estimatedProb[k] *
1159 out_newParticleLogWeight =
1160 me->m_particles[k].log_w + weightFact;
1165 const double weightFact =
1166 (poseLogLik - m_pfAuxiliaryPFStandard_estimatedProb[k]) *
1169 out_newParticleLogWeight = weightFact;
1171 out_newParticleLogWeight =
1172 weightFact + me->m_particles[k].log_w;