10 #ifndef PF_implementations_H 11 #define PF_implementations_H 44 class PARTICLE_TYPE,
class MYSELF,
46 template <
class BINTYPE>
52 MYSELF* me =
static_cast<MYSELF*
>(
this);
54 if (actions !=
nullptr)
60 if (m_accumRobotMovement3DIsValid)
63 ASSERT_(robotMovement2D->poseChange);
64 if (!m_accumRobotMovement2DIsValid)
66 robotMovement2D->poseChange->getMean(
67 m_accumRobotMovement2D.rawOdometryIncrementReading);
68 m_accumRobotMovement2D.motionModelConfiguration =
69 robotMovement2D->motionModelConfiguration;
72 m_accumRobotMovement2D.rawOdometryIncrementReading +=
73 robotMovement2D->poseChange->getMeanVal();
75 m_accumRobotMovement2DIsValid =
true;
83 if (m_accumRobotMovement2DIsValid)
86 if (!m_accumRobotMovement3DIsValid)
87 m_accumRobotMovement3D = robotMovement3D->poseChange;
89 m_accumRobotMovement3D += robotMovement3D->poseChange;
93 m_accumRobotMovement3DIsValid =
true;
100 const bool SFhasValidObservations =
101 (sf ==
nullptr) ?
false 102 : PF_SLAM_implementation_doWeHaveValidObservations(
103 me->m_particles, sf);
106 if (!((m_accumRobotMovement2DIsValid || m_accumRobotMovement3DIsValid) &&
107 SFhasValidObservations))
112 if (m_accumRobotMovement3DIsValid)
114 m_movementDrawer.setPosePDF(
115 m_accumRobotMovement3D);
116 m_accumRobotMovement3DIsValid =
123 m_accumRobotMovement2D.rawOdometryIncrementReading,
124 m_accumRobotMovement2D.motionModelConfiguration);
127 m_movementDrawer.setPosePDF(
129 m_accumRobotMovement2DIsValid =
152 template <
class PARTICLE_TYPE,
class MYSELF,
154 template <
class BINTYPE>
164 PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal<BINTYPE>(
165 actions, sf, PF_options, KLD_options,
true );
175 template <
class PARTICLE_TYPE,
class MYSELF,
177 template <
class BINTYPE>
186 using TSetStateSpaceBins = std::set<BINTYPE, typename BINTYPE::lt_operator>;
188 MYSELF* me =
static_cast<MYSELF*
>(
this);
208 ASSERT_(robotMovement2D->poseChange);
209 m_movementDrawer.setPosePDF(
210 robotMovement2D->poseChange.get_ptr());
212 robotMovement2D->poseChange->getMeanVal());
221 m_movementDrawer.setPosePDF(robotMovement3D->poseChange);
222 robotMovement3D->poseChange.getMean(motionModelMeanIncr);
227 "Action list does not contain any " 228 "CActionRobotMovement2D or CActionRobotMovement3D " 237 const size_t M = me->m_particles.size();
242 for (
size_t i = 0; i < M; i++)
246 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;
315 part = me->m_particles[drawn_idx].d.get();
316 else part = &me->m_particles[drawn_idx].d;
319 KLF_loadBinFromParticle<PARTICLE_TYPE, BINTYPE>(
320 p, KLD_options, part, &newPose_s);
322 if (stateSpaceBins.find(
p) == stateSpaceBins.end())
326 stateSpaceBins.insert(
p);
329 size_t K = stateSpaceBins.size();
339 N = newParticles.size();
349 this->PF_SLAM_implementation_replaceByNewParticleSet(
350 me->m_particles, newParticles, newParticlesWeight,
351 newParticlesDerivedFromIdx);
358 const size_t M = me->m_particles.size();
362 for (
size_t i = 0; i < M; i++)
366 getLastPose(i, pose_is_valid);
368 const double obs_log_likelihood =
369 PF_SLAM_computeObservationLikelihoodForParticle(
370 PF_options, i, *sf, partPose2);
371 me->m_particles[i].log_w +=
372 obs_log_likelihood * PF_options.
powFactor;
397 template <
class PARTICLE_TYPE,
class MYSELF,
399 template <
class BINTYPE>
409 PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal<BINTYPE>(
410 actions, sf, PF_options, KLD_options,
false );
416 template <
class PARTICLE_TYPE,
class MYSELF,
418 template <
class BINTYPE>
423 const void* action,
const void* observation)
430 const MYSELF* me =
static_cast<const MYSELF*
>(
obj);
436 double indivLik, maxLik = -1e300;
447 for (
size_t q = 0;
q < N;
q++)
449 me->m_movementDrawer.drawSample(drawnSample);
453 indivLik = me->PF_SLAM_computeObservationLikelihoodForParticle(
455 *static_cast<const mrpt::obs::CSensoryFrame*>(observation),
459 vectLiks[
q] = indivLik;
460 if (indivLik > maxLik)
462 maxLikDraw = drawnSample;
473 me->m_pfAuxiliaryPFOptimal_estimatedProb[
index] =
475 me->m_pfAuxiliaryPFOptimal_maxLikelihood[
index] = maxLik;
478 me->m_pfAuxiliaryPFOptimal_maxLikDrawnMovement[
index] =
483 return me->m_particles[
index].log_w +
484 me->m_pfAuxiliaryPFOptimal_estimatedProb[
index];
495 template <
class PARTICLE_TYPE,
class MYSELF,
497 template <
class BINTYPE>
502 const void* action,
const void* observation)
508 const MYSELF* myObj =
static_cast<const MYSELF*
>(
obj);
511 const double cur_logweight = myObj->m_particles[
index].log_w;
522 oldPose, *static_cast<const mrpt::poses::CPose3D*>(action));
526 myObj->m_pfAuxiliaryPFStandard_estimatedProb[
index] =
527 myObj->PF_SLAM_computeObservationLikelihoodForParticle(
529 *static_cast<const mrpt::obs::CSensoryFrame*>(observation),
533 return cur_logweight +
534 myObj->m_pfAuxiliaryPFStandard_estimatedProb[
index];
544 double indivLik, maxLik = -1e300;
552 for (
size_t q = 0;
q < N;
q++)
554 myObj->m_movementDrawer.drawSample(drawnSample);
558 indivLik = myObj->PF_SLAM_computeObservationLikelihoodForParticle(
560 *static_cast<const mrpt::obs::CSensoryFrame*>(observation),
564 vectLiks[
q] = indivLik;
565 if (indivLik > maxLik)
567 maxLikDraw = drawnSample;
578 myObj->m_pfAuxiliaryPFStandard_estimatedProb[
index] =
581 myObj->m_pfAuxiliaryPFOptimal_maxLikelihood[
index] = maxLik;
583 myObj->m_pfAuxiliaryPFOptimal_maxLikDrawnMovement[
index] =
588 return cur_logweight +
589 myObj->m_pfAuxiliaryPFOptimal_estimatedProb[
index];
597 template <
class PARTICLE_TYPE,
class MYSELF,
599 template <
class BINTYPE>
605 const TKLDParams& KLD_options,
const bool USE_OPTIMAL_SAMPLING)
608 using TSetStateSpaceBins = std::set<BINTYPE, typename BINTYPE::lt_operator>;
610 MYSELF* me =
static_cast<MYSELF*
>(
this);
612 const size_t M = me->m_particles.size();
620 if (!PF_SLAM_implementation_gatherActionsCheckBothActObs<BINTYPE>(
635 m_pfAuxiliaryPFOptimal_maxLikDrawnMovement.resize(M);
636 m_pfAuxiliaryPFOptimal_estimatedProb.resize(M);
637 m_pfAuxiliaryPFStandard_estimatedProb.resize(M);
641 m_movementDrawer.getSamplingMean3D(meanRobotMovement);
646 &TMyClass::template PF_SLAM_particlesEvaluator_AuxPFOptimal<BINTYPE>;
648 &TMyClass::template PF_SLAM_particlesEvaluator_AuxPFStandard<BINTYPE>;
650 me->prepareFastDrawSample(
651 PF_options, USE_OPTIMAL_SAMPLING ? funcOpt : funcStd,
652 &meanRobotMovement, sf);
657 if (USE_OPTIMAL_SAMPLING &&
663 "[prepareFastDrawSample] max (log) = %10.06f\n",
668 "[prepareFastDrawSample] max-mean (log) = %10.06f\n",
669 -
math::mean(m_pfAuxiliaryPFOptimal_estimatedProb) +
674 "[prepareFastDrawSample] max-min (log) = %10.06f\n",
694 std::vector<mrpt::math::TPose3D> newParticles;
695 std::vector<double> newParticlesWeight;
696 std::vector<size_t> newParticlesDerivedFromIdx;
704 m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed.assign(M,
false);
707 USE_OPTIMAL_SAMPLING ? m_pfAuxiliaryPFOptimal_estimatedProb
708 : m_pfAuxiliaryPFStandard_estimatedProb);
715 newParticles.resize(M);
716 newParticlesWeight.resize(M);
717 newParticlesDerivedFromIdx.resize(M);
719 const bool doResample = me->ESS() < PF_options.
BETA;
721 for (
size_t i = 0; i < M; i++)
729 k = me->fastDrawSample(
737 double newParticleLogWeight;
738 PF_SLAM_aux_perform_one_rejection_sampling_step<BINTYPE>(
739 USE_OPTIMAL_SAMPLING, doResample, maxMeanLik, k, sf, PF_options,
740 newPose, newParticleLogWeight);
743 newParticles[i] = newPose.asTPose();
744 newParticlesDerivedFromIdx[i] = k;
745 newParticlesWeight[i] = newParticleLogWeight;
758 newParticles.clear();
759 newParticlesWeight.resize(0);
760 newParticlesDerivedFromIdx.clear();
774 TSetStateSpaceBins stateSpaceBinsLastTimestep;
775 std::vector<std::vector<uint32_t>> stateSpaceBinsLastTimestepParticles;
777 unsigned int partIndex;
780 for (partIt = me->m_particles.begin(), partIndex = 0;
781 partIt != me->m_particles.end(); ++partIt, ++partIndex)
784 const PARTICLE_TYPE *part;
786 part = partIt->d.get();
787 else part = &partIt->d;
790 KLF_loadBinFromParticle<PARTICLE_TYPE, BINTYPE>(
791 p, KLD_options, part);
795 stateSpaceBinsLastTimestep.find(
p);
796 if (posFound == stateSpaceBinsLastTimestep.end())
798 stateSpaceBinsLastTimestep.insert(
p);
799 stateSpaceBinsLastTimestepParticles.push_back(
800 std::vector<uint32_t>(1, partIndex));
806 stateSpaceBinsLastTimestepParticles[idx].push_back(partIndex);
812 "[FIXED_SAMPLING] done (%u bins in t-1)\n",
813 (
unsigned int)stateSpaceBinsLastTimestep.size()));
818 double delta_1 = 1.0 - KLD_options.
KLD_delta;
820 bool doResample = me->ESS() < PF_options.
BETA;
827 const size_t minNumSamples_KLD = std::max(
831 stateSpaceBinsLastTimestep.size()));
832 size_t Nx = minNumSamples_KLD;
834 const size_t Np1 = me->m_particles.size();
835 std::vector<size_t> oldPartIdxsStillNotPropragated(
837 for (
size_t k = 0; k < Np1; k++)
838 oldPartIdxsStillNotPropragated[k] = k;
840 const size_t Np = stateSpaceBinsLastTimestepParticles.size();
841 std::vector<size_t> permutationPathsAuxVector(Np);
842 for (
size_t k = 0; k < Np; k++) permutationPathsAuxVector[k] = k;
848 permutationPathsAuxVector.begin(), permutationPathsAuxVector.end());
853 TSetStateSpaceBins stateSpaceBins;
867 k = me->fastDrawSample(
875 if (permutationPathsAuxVector.size())
877 const size_t idxBinSpacePath =
878 *permutationPathsAuxVector.rbegin();
879 permutationPathsAuxVector.resize(
880 permutationPathsAuxVector.size() - 1);
884 stateSpaceBinsLastTimestepParticles[idxBinSpacePath]
886 k = stateSpaceBinsLastTimestepParticles[idxBinSpacePath]
888 ASSERT_(k < me->m_particles.size());
891 oldPartIdxsStillNotPropragated.erase(
893 oldPartIdxsStillNotPropragated.begin(),
894 oldPartIdxsStillNotPropragated.end(), k));
909 if (oldPartIdxsStillNotPropragated.size())
914 oldPartIdxsStillNotPropragated.size();
916 oldPartIdxsStillNotPropragated.begin() +
919 oldPartIdxsStillNotPropragated.erase(it);
926 me->m_particles.size();
934 double newParticleLogWeight;
935 PF_SLAM_aux_perform_one_rejection_sampling_step<BINTYPE>(
936 USE_OPTIMAL_SAMPLING, doResample, maxMeanLik, k, sf, PF_options,
937 newPose, newParticleLogWeight);
940 newParticles.push_back(newPose.asTPose());
941 newParticlesDerivedFromIdx.push_back(k);
942 newParticlesWeight.push_back(newParticleLogWeight);
948 const PARTICLE_TYPE *part;
950 part = me->m_particles[k].d.get();
951 else part = &me->m_particles[k].d;
955 KLF_loadBinFromParticle<PARTICLE_TYPE, BINTYPE>(
956 p, KLD_options, part, &newPose_s);
965 if (stateSpaceBins.find(
p) == stateSpaceBins.end())
968 stateSpaceBins.insert(
p);
971 int K = stateSpaceBins.
size();
981 N = newParticles.size();
984 N < std::max(Nx, minNumSamples_KLD)) ||
985 (permutationPathsAuxVector.size() && !doResample));
990 "[ADAPTIVE SAMPLE SIZE] #Bins: %u \t #Particles: %u \t " 992 static_cast<unsigned>(stateSpaceBins.size()),
993 static_cast<unsigned>(N), (unsigned)Nx));
1002 this->PF_SLAM_implementation_replaceByNewParticleSet(
1003 me->m_particles, newParticles, newParticlesWeight,
1004 newParticlesDerivedFromIdx);
1007 me->normalizeWeights();
1015 template <
class PARTICLE_TYPE,
class MYSELF,
1017 template <
class BINTYPE>
1020 const bool USE_OPTIMAL_SAMPLING,
const bool doResample,
1021 const double maxMeanLik,
1027 MYSELF* me =
static_cast<MYSELF*
>(
this);
1032 while (((USE_OPTIMAL_SAMPLING ? m_pfAuxiliaryPFOptimal_estimatedProb[k]
1033 : m_pfAuxiliaryPFStandard_estimatedProb[k]) -
1038 me->m_particles.size();
1041 "[PF_SLAM_aux_perform_one_rejection_sampling_step] Warning: " 1042 "Discarding very unlikely particle.");
1047 getLastPose(k, pose_is_valid));
1054 if (PF_SLAM_implementation_skipRobotMovement())
1059 out_newPose = oldPose;
1065 if (!USE_OPTIMAL_SAMPLING)
1067 m_movementDrawer.drawSample(movementDraw);
1069 oldPose, movementDraw);
1071 poseLogLik = PF_SLAM_computeObservationLikelihoodForParticle(
1072 PF_options, k, *sf, out_newPose);
1077 double acceptanceProb;
1079 const int maxTries = 10000;
1080 double bestTryByNow_loglik = -std::numeric_limits<double>::max();
1086 !m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed[k])
1089 m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed[k] =
1092 m_pfAuxiliaryPFOptimal_maxLikDrawnMovement[k]);
1097 m_movementDrawer.drawSample(movementDraw);
1105 poseLogLik = PF_SLAM_computeObservationLikelihoodForParticle(
1106 PF_options, k, *sf, out_newPose);
1108 if (poseLogLik > bestTryByNow_loglik)
1110 bestTryByNow_loglik = poseLogLik;
1111 bestTryByNow_pose = out_newPose.
asTPose();
1114 double ratioLikLik = std::exp(
1115 poseLogLik - m_pfAuxiliaryPFOptimal_maxLikelihood[k]);
1116 acceptanceProb =
std::min(1.0, ratioLikLik);
1118 if (ratioLikLik > 1)
1120 m_pfAuxiliaryPFOptimal_maxLikelihood[k] =
1126 ++timeout < maxTries &&
1130 if (timeout >= maxTries)
1133 poseLogLik = bestTryByNow_loglik;
1136 "[PF_implementation] Warning: timeout in rejection " 1142 if (USE_OPTIMAL_SAMPLING)
1145 out_newParticleLogWeight = 0;
1150 const double weightFact =
1151 m_pfAuxiliaryPFOptimal_estimatedProb[k] *
1153 out_newParticleLogWeight =
1154 me->m_particles[k].log_w + weightFact;
1159 const double weightFact =
1160 (poseLogLik - m_pfAuxiliaryPFStandard_estimatedProb[k]) *
1163 out_newParticleLogWeight = weightFact;
1165 out_newParticleLogWeight =
1166 weightFact + me->m_particles[k].log_w;
uint32_t drawUniform32bit()
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm, in the whole range of 32-bit integers.
mrpt::math::TPose3D asTPose() const
const_iterator find(const KEY &key) const
#define THROW_EXCEPTION(msg)
#define INVALID_LIKELIHOOD_VALUE
unsigned int pfAuxFilterOptimal_MaximumSearchSamples
In the algorithm "CParticleFilter::pfAuxiliaryPFOptimal" (and in "CParticleFilter::pfAuxiliaryPFStand...
void shuffle(RandomIt first, RandomIt last, URBG &&g)
Uniform shuffle a sequence.
GLdouble GLdouble GLdouble GLdouble q
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...
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Option set for KLD algorithm.
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...
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...
GLsizei GLsizei GLuint * obj
Declares a class for storing a collection of robot actions.
A set of common data shared by PF implementations for both SLAM and localization. ...
CONTAINER::Scalar minimum(const CONTAINER &v)
#define ASSERT_(f)
Defines an assertion mechanism.
Represents a probabilistic 3D (6D) movement.
Represents a probabilistic 2D movement of the robot mobile base.
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)
unsigned int KLD_minSampleSize
Parameters for the KLD adaptive sample size algorithm (see Dieter Fox's papers), which is used only i...
CActionRobotMovement2D::Ptr getBestMovementEstimation() const
Returns the best pose increment estimator in the collection, based on the determinant of its pose cha...
#define MRPT_CHECK_NORMAL_NUMBER(v)
Throws an exception if the number is NaN, IND, or +/-INF, or return the same number otherwise...
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...
double KLD_minSamplesPerBin
(Default: KLD_minSamplesPerBin=0) The minimum number of samples will be the maximum of KLD_minSampleS...
double averageLogLikelihood(const CVectorDouble &logLikelihoods)
A numerically-stable method to compute average likelihood values with strongly different ranges (unwe...
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
This virtual class defines the interface that any particles based PDF class must implement in order t...
bool pfAuxFilterStandard_FirstStageWeightsMonteCarlo
Only for PF_algorithm==pfAuxiliaryPFStandard: If false, the APF will predict the first stage weights ...
CONTAINER::Scalar maximum(const CONTAINER &v)
double BETA
The resampling of particles will be performed when ESS (in range [0,1]) < BETA (default is 0...
unsigned int KLD_maxSampleSize
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_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...
double chi2inv(double P, unsigned int dim=1)
The "quantile" of the Chi-Square distribution, for dimension "dim" and probability 0<P<1 (the inverse...
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...
mrpt::containers::deepcopy_poly_ptr< mrpt::poses::CPosePDF::Ptr > poseChange
The 2D pose change probabilistic estimation.
double max_loglikelihood_dyn_range
Only for PF_algorithm=pfAuxiliaryPFOptimal: If a given particle has a max_likelihood (from the a-prio...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
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.
double powFactor
An optional step to "smooth" dramatic changes in the observation model to affect the variance of the ...
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
The configuration of a particle filter.
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
double mean(const CONTAINER &v)
Computes the mean value of a vector.
bool pfAuxFilterOptimal_MLE
(Default=false) In the algorithm "CParticleFilter::pfAuxiliaryPFOptimal", if set to true...
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...
T::Ptr getActionByClass(const size_t &ith=0) const
Access to the i'th action of a given class, or a nullptr smart pointer if there is no action of that ...
void logStr(const VerbosityLevel level, const std::string &msg_str) const
Main method to add the specified message string to the logger.
particle_storage_mode
use for CProbabilityParticle
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
double distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.
bool adaptiveSampleSize
A flag that indicates whether the CParticleFilterCapable object should perform adative sample size (d...
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
int round(const T value)
Returns the closer integer (int) to x.