10 #ifndef PF_implementations_H 11 #define PF_implementations_H 42 template <
class PARTICLE_TYPE,
class MY
SELF>
43 template <
class BINTYPE>
48 MYSELF *me =
static_cast<MYSELF*
>(
this);
54 if (robotMovement2D.present())
56 if (m_accumRobotMovement3DIsValid)
THROW_EXCEPTION(
"Mixing 2D and 3D actions is not allowed.")
57 ASSERT_(robotMovement2D->poseChange);
59 if (!m_accumRobotMovement2DIsValid)
61 robotMovement2D->poseChange->getMean( m_accumRobotMovement2D.rawOdometryIncrementReading );
62 m_accumRobotMovement2D.motionModelConfiguration = robotMovement2D->motionModelConfiguration;
64 else m_accumRobotMovement2D.rawOdometryIncrementReading += robotMovement2D->poseChange->getMeanVal();
66 m_accumRobotMovement2DIsValid =
true;
73 if (m_accumRobotMovement2DIsValid)
THROW_EXCEPTION(
"Mixing 2D and 3D actions is not allowed.")
75 if (!m_accumRobotMovement3DIsValid)
76 m_accumRobotMovement3D = robotMovement3D->poseChange;
77 else m_accumRobotMovement3D += robotMovement3D->poseChange;
80 m_accumRobotMovement3DIsValid =
true;
88 const bool SFhasValidObservations = (sf==NULL) ?
false : PF_SLAM_implementation_doWeHaveValidObservations(me->m_particles,sf);
91 if (! ((m_accumRobotMovement2DIsValid || m_accumRobotMovement3DIsValid) && SFhasValidObservations))
96 if (m_accumRobotMovement3DIsValid)
98 m_movementDrawer.setPosePDF( m_accumRobotMovement3D );
99 m_accumRobotMovement3DIsValid =
false;
105 m_accumRobotMovement2D.rawOdometryIncrementReading,
106 m_accumRobotMovement2D.motionModelConfiguration );
110 m_accumRobotMovement2DIsValid =
false;
128 template <
class PARTICLE_TYPE,
class MY
SELF>
129 template <
class BINTYPE>
137 PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal<BINTYPE>(actions,sf,PF_options,KLD_options,
true );
146 template <
class PARTICLE_TYPE,
class MY
SELF>
147 template <
class BINTYPE>
155 typedef std::set<BINTYPE,typename BINTYPE::lt_operator> TSetStateSpaceBins;
157 MYSELF *me =
static_cast<MYSELF*
>(
this);
172 if (robotMovement2D.present())
174 ASSERT_(robotMovement2D->poseChange);
175 m_movementDrawer.setPosePDF( robotMovement2D->poseChange.get_ptr() );
183 m_movementDrawer.setPosePDF( robotMovement3D->poseChange );
186 else {
THROW_EXCEPTION(
"Action list does not contain any CActionRobotMovement2D or CActionRobotMovement3D object!"); }
193 const size_t M = me->m_particles.size();
198 for (
size_t i=0;i<M;i++)
201 m_movementDrawer.drawSample( incrPose );
207 PF_SLAM_implementation_custom_update_particle_with_new_pose( me->m_particles[i].d.get(), finalPose );
218 TSetStateSpaceBins stateSpaceBins;
221 const double delta_1 = 1.0 - KLD_options.
KLD_delta;
222 const double epsilon_1 = 0.5 / KLD_options.
KLD_epsilon;
225 me->prepareFastDrawSample(PF_options);
228 std::vector<mrpt::math::TPose3D> newParticles;
229 std::vector<double> newParticlesWeight;
230 std::vector<size_t> newParticlesDerivedFromIdx;
238 m_movementDrawer.drawSample( increment_i );
241 const size_t drawn_idx = me->fastDrawSample(PF_options);
248 newParticles.push_back( newPose_s );
249 newParticlesWeight.push_back(0);
250 newParticlesDerivedFromIdx.push_back(drawn_idx);
255 KLF_loadBinFromParticle<PARTICLE_TYPE,BINTYPE>(
p,KLD_options, me->m_particles[drawn_idx].d.get(), &newPose_s);
257 if (stateSpaceBins.find(
p )==stateSpaceBins.end())
261 stateSpaceBins.insert(
p );
264 size_t K = stateSpaceBins.size();
272 N = newParticles.size();
281 this->PF_SLAM_implementation_replaceByNewParticleSet(
283 newParticles,newParticlesWeight,newParticlesDerivedFromIdx );
290 const size_t M = me->m_particles.size();
294 for (
size_t i=0;i<M;i++)
299 const double obs_log_likelihood = PF_SLAM_computeObservationLikelihoodForParticle(PF_options,i,*sf,partPose);
300 me->m_particles[i].log_w += obs_log_likelihood * PF_options.
powFactor;
319 template <
class PARTICLE_TYPE,
class MY
SELF>
320 template <
class BINTYPE>
328 PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal<BINTYPE>(actions,sf,PF_options,KLD_options,
false );
334 template <
class PARTICLE_TYPE,
class MY
SELF>
335 template <
class BINTYPE>
341 const void *observation )
347 const MYSELF *me =
static_cast<const MYSELF*
>(
obj);
353 double indivLik, maxLik= -1e300;
363 for (
size_t q=0;
q<N;
q++)
365 me->m_movementDrawer.drawSample(drawnSample);
369 indivLik = me->PF_SLAM_computeObservationLikelihoodForParticle(
372 *static_cast<const mrpt::obs::CSensoryFrame*>(observation),
376 vectLiks[
q] = indivLik;
377 if ( indivLik > maxLik )
379 maxLikDraw = drawnSample;
390 me->m_pfAuxiliaryPFOptimal_estimatedProb[
index] = avrgLogLik;
391 me->m_pfAuxiliaryPFOptimal_maxLikelihood[
index] = maxLik;
394 me->m_pfAuxiliaryPFOptimal_maxLikDrawnMovement[
index] = maxLikDraw;
398 return me->m_particles[
index].log_w + me->m_pfAuxiliaryPFOptimal_estimatedProb[
index];
410 template <
class PARTICLE_TYPE,
class MY
SELF>
411 template <
class BINTYPE>
417 const void *observation )
422 const MYSELF *myObj =
static_cast<const MYSELF*
>(
obj);
425 const double cur_logweight = myObj->m_particles[
index].log_w;
435 x_predict.
composeFrom( oldPose, *static_cast<const mrpt::poses::CPose3D*>(action) );
439 myObj->m_pfAuxiliaryPFStandard_estimatedProb[
index] = myObj->PF_SLAM_computeObservationLikelihoodForParticle(
441 *static_cast<const mrpt::obs::CSensoryFrame*>(observation), x_predict );
444 return cur_logweight + myObj->m_pfAuxiliaryPFStandard_estimatedProb[
index];
453 double indivLik, maxLik= -1e300;
460 for (
size_t q=0;
q<N;
q++)
462 myObj->m_movementDrawer.drawSample(drawnSample);
466 indivLik = myObj->PF_SLAM_computeObservationLikelihoodForParticle(
469 *static_cast<const mrpt::obs::CSensoryFrame*>(observation),
473 vectLiks[
q] = indivLik;
474 if ( indivLik > maxLik )
476 maxLikDraw = drawnSample;
487 myObj->m_pfAuxiliaryPFStandard_estimatedProb[
index] = avrgLogLik;
489 myObj->m_pfAuxiliaryPFOptimal_maxLikelihood[
index] = maxLik;
491 myObj->m_pfAuxiliaryPFOptimal_maxLikDrawnMovement[
index] = maxLikDraw;
495 return cur_logweight + myObj->m_pfAuxiliaryPFOptimal_estimatedProb[
index];
503 template <
class PARTICLE_TYPE,
class MY
SELF>
504 template <
class BINTYPE>
510 const bool USE_OPTIMAL_SAMPLING )
513 typedef std::set<BINTYPE,typename BINTYPE::lt_operator> TSetStateSpaceBins;
515 MYSELF *me =
static_cast<MYSELF*
>(
this);
517 const size_t M = me->m_particles.size();
525 if (!PF_SLAM_implementation_gatherActionsCheckBothActObs<BINTYPE>(actions,sf))
539 m_pfAuxiliaryPFOptimal_maxLikDrawnMovement.resize(M);
540 m_pfAuxiliaryPFOptimal_estimatedProb.resize(M);
541 m_pfAuxiliaryPFStandard_estimatedProb.resize(M);
545 m_movementDrawer.getSamplingMean3D(meanRobotMovement);
552 me->prepareFastDrawSample(
554 USE_OPTIMAL_SAMPLING ? funcOpt : funcStd,
560 if (USE_OPTIMAL_SAMPLING && me->isLoggingLevelVisible(mrpt::utils::LVL_DEBUG) )
562 me->logStr(mrpt::utils::LVL_DEBUG,
mrpt::format(
"[prepareFastDrawSample] max (log) = %10.06f\n",
math::maximum(m_pfAuxiliaryPFOptimal_estimatedProb) ) );
563 me->logStr(mrpt::utils::LVL_DEBUG,
mrpt::format(
"[prepareFastDrawSample] max-mean (log) = %10.06f\n", -
math::mean(m_pfAuxiliaryPFOptimal_estimatedProb) +
math::maximum(m_pfAuxiliaryPFOptimal_estimatedProb) ) );
564 me->logStr(mrpt::utils::LVL_DEBUG,
mrpt::format(
"[prepareFastDrawSample] max-min (log) = %10.06f\n", -
math::minimum(m_pfAuxiliaryPFOptimal_estimatedProb) +
math::maximum(m_pfAuxiliaryPFOptimal_estimatedProb) ) );
580 std::vector<mrpt::math::TPose3D> newParticles;
581 std::vector<double> newParticlesWeight;
582 std::vector<size_t> newParticlesDerivedFromIdx;
590 m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed.assign(M,
false);
593 USE_OPTIMAL_SAMPLING ?
594 m_pfAuxiliaryPFOptimal_estimatedProb :
595 m_pfAuxiliaryPFStandard_estimatedProb );
602 newParticles.resize(M);
603 newParticlesWeight.resize(M);
604 newParticlesDerivedFromIdx.resize(M);
606 const bool doResample = me->ESS() < PF_options.
BETA;
608 for (
size_t i=0;i<M;i++)
616 k = me->fastDrawSample(PF_options);
622 double newParticleLogWeight;
623 PF_SLAM_aux_perform_one_rejection_sampling_step<BINTYPE>(
624 USE_OPTIMAL_SAMPLING,doResample,maxMeanLik,
627 newPose, newParticleLogWeight);
630 newParticles[i] = newPose;
631 newParticlesDerivedFromIdx[i] = k;
632 newParticlesWeight[i] = newParticleLogWeight;
645 newParticles.clear();
646 newParticlesWeight.resize(0);
647 newParticlesDerivedFromIdx.clear();
656 TSetStateSpaceBins stateSpaceBinsLastTimestep;
657 std::vector<vector_uint> stateSpaceBinsLastTimestepParticles;
659 unsigned int partIndex;
661 me->logStr(mrpt::utils::LVL_DEBUG,
"[FIXED_SAMPLING] Computing...");
662 for (partIt = me->m_particles.begin(),partIndex=0; partIt!=me->m_particles.end(); ++partIt,++partIndex)
666 KLF_loadBinFromParticle<PARTICLE_TYPE,BINTYPE>(
p, KLD_options,partIt->d.get() );
670 if ( posFound == stateSpaceBinsLastTimestep.end() )
672 stateSpaceBinsLastTimestep.insert(
p );
673 stateSpaceBinsLastTimestepParticles.push_back(
vector_uint(1,partIndex) );
677 const size_t idx =
std::distance(stateSpaceBinsLastTimestep.begin(),posFound);
678 stateSpaceBinsLastTimestepParticles[idx].push_back( partIndex );
681 me->logStr(mrpt::utils::LVL_DEBUG,
mrpt::format(
"[FIXED_SAMPLING] done (%u bins in t-1)\n",(
unsigned int)stateSpaceBinsLastTimestep.size()) );
686 double delta_1 = 1.0 - KLD_options.
KLD_delta;
688 bool doResample = me->ESS() < PF_options.
BETA;
693 size_t Nx = minNumSamples_KLD ;
695 const size_t Np1 = me->m_particles.size();
697 for (
size_t k=0;k<Np1;k++) oldPartIdxsStillNotPropragated[k]=k;
699 const size_t Np = stateSpaceBinsLastTimestepParticles.size();
701 for (
size_t k=0;k<Np;k++) permutationPathsAuxVector[k]=k;
706 permutationPathsAuxVector.begin(),
707 permutationPathsAuxVector.end(),
713 TSetStateSpaceBins stateSpaceBins;
726 k = me->fastDrawSample(PF_options);
732 if (permutationPathsAuxVector.size())
734 const size_t idxBinSpacePath = *permutationPathsAuxVector.rbegin();
735 permutationPathsAuxVector.resize(permutationPathsAuxVector.size()-1);
738 k = stateSpaceBinsLastTimestepParticles[idxBinSpacePath][idx];
739 ASSERT_(k<me->m_particles.size());
742 oldPartIdxsStillNotPropragated.erase(
std::find(oldPartIdxsStillNotPropragated.begin(),oldPartIdxsStillNotPropragated.end(),k));
753 if (oldPartIdxsStillNotPropragated.size())
758 oldPartIdxsStillNotPropragated.erase(it);
771 double newParticleLogWeight;
772 PF_SLAM_aux_perform_one_rejection_sampling_step<BINTYPE>(
773 USE_OPTIMAL_SAMPLING,doResample,maxMeanLik,
776 newPose, newParticleLogWeight);
779 newParticles.push_back( newPose );
780 newParticlesDerivedFromIdx.push_back( k );
781 newParticlesWeight.push_back(newParticleLogWeight);
789 KLF_loadBinFromParticle<PARTICLE_TYPE,BINTYPE>(
p,KLD_options, me->m_particles[k].d.get(), &newPose_s );
797 if ( stateSpaceBins.find(
p)==stateSpaceBins.end() )
800 stateSpaceBins.insert(
p );
803 int K = stateSpaceBins.
size();
812 N = newParticles.size();
815 N < std::max(Nx,minNumSamples_KLD)) ||
816 (permutationPathsAuxVector.size() && !doResample) );
818 me->logStr(mrpt::utils::LVL_DEBUG,
mrpt::format(
"[ADAPTIVE SAMPLE SIZE] #Bins: %u \t #Particles: %u \t Nx=%u\n", static_cast<unsigned>(stateSpaceBins.size()),static_cast<unsigned>(N), (unsigned)Nx) );
827 this->PF_SLAM_implementation_replaceByNewParticleSet(
829 newParticles,newParticlesWeight,newParticlesDerivedFromIdx );
833 me->normalizeWeights();
842 template <
class PARTICLE_TYPE,
class MY
SELF>
843 template <
class BINTYPE>
845 const bool USE_OPTIMAL_SAMPLING,
846 const bool doResample,
847 const double maxMeanLik,
852 double & out_newParticleLogWeight)
854 MYSELF *me =
static_cast<MYSELF*
>(
this);
858 while ( ( (USE_OPTIMAL_SAMPLING ? m_pfAuxiliaryPFOptimal_estimatedProb[k] : m_pfAuxiliaryPFStandard_estimatedProb[k] )
863 me->logStr(mrpt::utils::LVL_DEBUG,
"[PF_SLAM_aux_perform_one_rejection_sampling_step] Warning: Discarding very unlikely particle.");
874 if ( PF_SLAM_implementation_skipRobotMovement() )
878 out_newPose = oldPose;
884 if (!USE_OPTIMAL_SAMPLING)
886 m_movementDrawer.drawSample( movementDraw );
889 poseLogLik = PF_SLAM_computeObservationLikelihoodForParticle(PF_options, k,*sf,out_newPose);
894 double acceptanceProb;
896 const int maxTries=10000;
897 double bestTryByNow_loglik= -std::numeric_limits<double>::max();
904 m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed[k] =
true;
910 m_movementDrawer.drawSample( movementDraw );
916 poseLogLik = PF_SLAM_computeObservationLikelihoodForParticle(PF_options, k,*sf,out_newPose);
918 if (poseLogLik>bestTryByNow_loglik)
920 bestTryByNow_loglik = poseLogLik;
921 bestTryByNow_pose = out_newPose;
924 double ratioLikLik = std::exp( poseLogLik - m_pfAuxiliaryPFOptimal_maxLikelihood[k] );
925 acceptanceProb =
std::min( 1.0, ratioLikLik );
927 if ( ratioLikLik > 1)
929 m_pfAuxiliaryPFOptimal_maxLikelihood[k] = poseLogLik;
934 if (timeout>=maxTries)
937 poseLogLik = bestTryByNow_loglik;
938 me->logStr(mrpt::utils::LVL_WARN,
"[PF_implementation] Warning: timeout in rejection sampling.");
943 if (USE_OPTIMAL_SAMPLING)
946 out_newParticleLogWeight = 0;
949 const double weightFact = m_pfAuxiliaryPFOptimal_estimatedProb[k] * PF_options.
powFactor;
950 out_newParticleLogWeight = me->m_particles[k].log_w + weightFact;
955 const double weightFact = (poseLogLik-m_pfAuxiliaryPFStandard_estimatedProb[k]) * PF_options.
powFactor;
957 out_newParticleLogWeight = weightFact;
958 else out_newParticleLogWeight = 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.
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...
mrpt::poses::CPose3DPDFGaussian poseChange
The 3D pose change probabilistic estimation.
mrpt::utils::poly_ptr_ptr< mrpt::poses::CPosePDFPtr > poseChange
The 2D pose change probabilistic estimation.
std::vector< uint32_t > vector_uint
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)
#define INVALID_LIKELIHOOD_VALUE
unsigned int pfAuxFilterOptimal_MaximumSearchSamples
In the algorithm "CParticleFilter::pfAuxiliaryPFOptimal" (and in "CParticleFilter::pfAuxiliaryPFStand...
#define MRPT_CHECK_NORMAL_NUMBER(val)
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...
#define THROW_EXCEPTION(msg)
GLdouble GLdouble GLdouble GLdouble q
BASE_IMPEXP CRandomGenerator randomGenerator
A static instance of a CRandomGenerator class, for use in single-thread applications.
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...
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...
const_iterator find(const KEY &key) const
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)
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...
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...
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
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 BASE_IMPEXP averageLogLikelihood(const CVectorDouble &logLikelihoods)
A numerically-stable method to compute average likelihood values with strongly different ranges (unwe...
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.
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...
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
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...
ptrdiff_t random_generator_for_STL(ptrdiff_t i)
A random number generator for usage in STL algorithms expecting a function like this (eg...
double(* TParticleProbabilityEvaluator)(const bayes::CParticleFilter::TParticleFilterOptions &PF_options, const CParticleFilterCapable *obj, size_t index, const void *action, const void *observation)
A callback function type for evaluating the probability of m_particles of being selected, used in "fastDrawSample".
unsigned int KLD_maxSampleSize
void getMean(CPose3D &mean_pose) const MRPT_OVERRIDE
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF).
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
double BASE_IMPEXP chi2inv(double P, unsigned int dim=1)
The "quantile" of the Chi-Square distribution, for dimension "dim" and probability 0<P<1 (the inverse...
const T & get_ptr() const
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).
double powFactor
An optional step to "smooth" dramatic changes in the observation model to affect the variance of the ...
The configuration of a particle filter.
CActionRobotMovement2DPtr getBestMovementEstimation() const
Returns the best pose increment estimator in the collection, based on the determinant of its pose cha...
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
double mean(const CONTAINER &v)
Computes the mean value of a vector.
int round(const T value)
Returns the closer integer (int) to x.
std::vector< size_t > vector_size_t
bool pfAuxFilterOptimal_MLE
(Default=false) In the algorithm "CParticleFilter::pfAuxiliaryPFOptimal", if set to true...
T::Ptr getActionByClass(const size_t &ith=0) const
Access to the i'th action of a given class, or a NULL smart pointer if there is no action of that cla...
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...
double BASE_IMPEXP 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...