10 #ifndef PF_implementations_H 11 #define PF_implementations_H 44 template <
class PARTICLE_TYPE,
class MY
SELF>
45 template <
class BINTYPE>
50 MYSELF *me =
static_cast<MYSELF*
>(
this);
56 if (robotMovement2D.present())
58 if (m_accumRobotMovement3DIsValid)
THROW_EXCEPTION(
"Mixing 2D and 3D actions is not allowed.")
60 if (!m_accumRobotMovement2DIsValid)
62 robotMovement2D->poseChange->getMean( m_accumRobotMovement2D.rawOdometryIncrementReading );
63 m_accumRobotMovement2D.motionModelConfiguration = robotMovement2D->motionModelConfiguration;
65 else m_accumRobotMovement2D.rawOdometryIncrementReading += robotMovement2D->poseChange->getMeanVal();
67 m_accumRobotMovement2DIsValid =
true;
74 if (m_accumRobotMovement2DIsValid)
THROW_EXCEPTION(
"Mixing 2D and 3D actions is not allowed.")
76 if (!m_accumRobotMovement3DIsValid)
77 m_accumRobotMovement3D = robotMovement3D->poseChange;
78 else m_accumRobotMovement3D += robotMovement3D->poseChange;
81 m_accumRobotMovement3DIsValid =
true;
89 const bool SFhasValidObservations = (sf==NULL) ?
false : PF_SLAM_implementation_doWeHaveValidObservations(me->m_particles,sf);
92 if (! ((m_accumRobotMovement2DIsValid || m_accumRobotMovement3DIsValid) && SFhasValidObservations))
97 if (m_accumRobotMovement3DIsValid)
99 m_movementDrawer.setPosePDF( m_accumRobotMovement3D );
100 m_accumRobotMovement3DIsValid =
false;
106 m_accumRobotMovement2D.rawOdometryIncrementReading,
107 m_accumRobotMovement2D.motionModelConfiguration );
109 m_movementDrawer.setPosePDF( theResultingRobotMov.
poseChange );
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);
168 CPose3D motionModelMeanIncr;
172 if (robotMovement2D.present())
174 m_movementDrawer.setPosePDF( robotMovement2D->poseChange );
175 motionModelMeanIncr = robotMovement2D->poseChange->getMeanVal();
182 m_movementDrawer.setPosePDF( robotMovement3D->poseChange );
185 else {
THROW_EXCEPTION(
"Action list does not contain any CActionRobotMovement2D or CActionRobotMovement3D object!"); }
192 const size_t M = me->m_particles.size();
197 for (
size_t i=0;i<M;i++)
200 m_movementDrawer.drawSample( incrPose );
201 CPose3D finalPose = CPose3D(*getLastPose(i)) + incrPose;
204 PF_SLAM_implementation_custom_update_particle_with_new_pose( me->m_particles[i].d, TPose3D(finalPose) );
215 TSetStateSpaceBins stateSpaceBins;
218 const double delta_1 = 1.0 - KLD_options.
KLD_delta;
219 const double epsilon_1 = 0.5 / KLD_options.
KLD_epsilon;
222 me->prepareFastDrawSample(PF_options);
225 std::vector<TPose3D> newParticles;
226 std::vector<double> newParticlesWeight;
227 std::vector<size_t> newParticlesDerivedFromIdx;
235 m_movementDrawer.drawSample( increment_i );
238 const size_t drawn_idx = me->fastDrawSample(PF_options);
240 const TPose3D newPose_s = newPose;
243 newParticles.push_back( newPose_s );
244 newParticlesWeight.push_back(0);
245 newParticlesDerivedFromIdx.push_back(drawn_idx);
250 KLF_loadBinFromParticle<PARTICLE_TYPE,BINTYPE>(p,KLD_options, me->m_particles[drawn_idx].d, &newPose_s);
252 if (stateSpaceBins.find( p )==stateSpaceBins.end())
256 stateSpaceBins.insert( p );
259 size_t K = stateSpaceBins.size();
267 N = newParticles.size();
276 this->PF_SLAM_implementation_replaceByNewParticleSet(
278 newParticles,newParticlesWeight,newParticlesDerivedFromIdx );
285 const size_t M = me->m_particles.size();
289 for (
size_t i=0;i<M;i++)
291 const TPose3D *partPose= getLastPose(i);
292 CPose3D partPose2 = CPose3D(*partPose);
293 const double obs_log_likelihood = PF_SLAM_computeObservationLikelihoodForParticle(PF_options,i,*sf,partPose2);
294 me->m_particles[i].log_w += obs_log_likelihood * PF_options.
powFactor;
313 template <
class PARTICLE_TYPE,
class MY
SELF>
314 template <
class BINTYPE>
322 PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal<BINTYPE>(actions,sf,PF_options,KLD_options,
false );
328 template <
class PARTICLE_TYPE,
class MY
SELF>
329 template <
class BINTYPE>
335 const void *observation )
341 const MYSELF *me =
static_cast<const MYSELF*
>(obj);
347 double indivLik, maxLik= -1e300;
355 for (
size_t q=0;q<N;q++)
357 me->m_movementDrawer.drawSample(drawnSample);
358 CPose3D x_predict = oldPose + drawnSample;
361 indivLik = me->PF_SLAM_computeObservationLikelihoodForParticle(
364 *static_cast<const mrpt::obs::CSensoryFrame*>(observation),
368 vectLiks[q] = indivLik;
369 if ( indivLik > maxLik )
371 maxLikDraw = drawnSample;
382 me->m_pfAuxiliaryPFOptimal_estimatedProb[index] = avrgLogLik;
383 me->m_pfAuxiliaryPFOptimal_maxLikelihood[index] = maxLik;
386 me->m_pfAuxiliaryPFOptimal_maxLikDrawnMovement[index] = maxLikDraw;
390 return me->m_particles[index].log_w + me->m_pfAuxiliaryPFOptimal_estimatedProb[index];
402 template <
class PARTICLE_TYPE,
class MY
SELF>
403 template <
class BINTYPE>
409 const void *observation )
414 const MYSELF *myObj =
static_cast<const MYSELF*
>(obj);
417 const double cur_logweight = myObj->m_particles[index].log_w;
425 x_predict.
composeFrom( oldPose, *static_cast<const CPose3D*>(action) );
429 myObj->m_pfAuxiliaryPFStandard_estimatedProb[index] = myObj->PF_SLAM_computeObservationLikelihoodForParticle(
431 *static_cast<const mrpt::obs::CSensoryFrame*>(observation), x_predict );
434 return cur_logweight + myObj->m_pfAuxiliaryPFStandard_estimatedProb[index];
443 double indivLik, maxLik= -1e300;
450 for (
size_t q=0;q<N;q++)
452 myObj->m_movementDrawer.drawSample(drawnSample);
453 CPose3D x_predict = oldPose + drawnSample;
456 indivLik = myObj->PF_SLAM_computeObservationLikelihoodForParticle(
459 *static_cast<const mrpt::obs::CSensoryFrame*>(observation),
463 vectLiks[q] = indivLik;
464 if ( indivLik > maxLik )
466 maxLikDraw = drawnSample;
477 myObj->m_pfAuxiliaryPFStandard_estimatedProb[index] = avrgLogLik;
479 myObj->m_pfAuxiliaryPFOptimal_maxLikelihood[index] = maxLik;
481 myObj->m_pfAuxiliaryPFOptimal_maxLikDrawnMovement[index] = maxLikDraw;
485 return cur_logweight + myObj->m_pfAuxiliaryPFOptimal_estimatedProb[index];
493 template <
class PARTICLE_TYPE,
class MY
SELF>
494 template <
class BINTYPE>
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;
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 );
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())
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();
832 template <
class PARTICLE_TYPE,
class MY
SELF>
833 template <
class BINTYPE>
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;
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...
CActionRobotMovement2DPtr getBestMovementEstimation() const
Returns the best pose increment estimator in the collection, based on the determinant of its pose cha...
BASE_IMPEXP CRandomGenerator randomGenerator
A static instance of a CRandomGenerator class, for use in single-thread applications.
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)
Option set for KLD algorithm.
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...
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...
uint32_t drawUniform32bit()
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm, in the whole range of 32-bit integers.
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
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...
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...
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...
bool verbose
Enable extra messages for each PF iteration (Default=false)
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).
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...
double powFactor
An optional step to "smooth" dramatic changes in the observation model to affect the variance of the ...
T::SmartPtr 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...
The configuration of a particle filter.
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...
mrpt::poses::CPosePDFPtr poseChange
The 2D pose change probabilistic estimation.
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...
dynamic_vector< double > CVectorDouble
Column vector, like Eigen::MatrixXd, but automatically initialized to zeros since construction...
poses::CPose3DPDFGaussian poseChange
The 3D pose change probabilistic estimation.
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...
bool adaptiveSampleSize
A flag that indicates whether the CParticleFilterCapable object should perform adative sample size (d...
double BASE_IMPEXP distance(const TPoint2D &p1, const TPoint2D &p2)
Gets the distance between two points in a 2D space.