Template Class PF_implementation

Defined in File PF_implementations_data.h

Inheritance Relationships

Base Type

public mrpt::system::COutputLogger

Class Documentation

template<class PARTICLE_TYPE, class MYSELF, mrpt::bayes::particle_storage_mode STORAGE> class PF_implementation : public mrpt::system::COutputLogger

A set of common data shared by PF implementations for both SLAM and localization

Data members and methods used by generic PF implementations

mrpt::obs::CActionRobotMovement2D m_accumRobotMovement2D

bool m_accumRobotMovement2DIsValid = {false}

mrpt::poses::CPose3DPDFGaussian m_accumRobotMovement3D

bool m_accumRobotMovement3DIsValid = {false}

mrpt::poses::CPoseRandomSampler m_movementDrawer

Used in al PF implementations.

mutable mrpt::math::CVectorDouble m_pfAuxiliaryPFOptimal_estimatedProb: Auxiliary variable used in the “pfAuxiliaryPFOptimal” algorithm.

mutable mrpt::math::CVectorDouble m_pfAuxiliaryPFStandard_estimatedProb: Auxiliary variable used in the “pfAuxiliaryPFStandard” algorithm.

mutable mrpt::math::CVectorDouble m_pfAuxiliaryPFOptimal_maxLikelihood: Auxiliary variable used in the “pfAuxiliaryPFOptimal” algorithm.

mutable std::vector<mrpt::math::TPose3D> m_pfAuxiliaryPFOptimal_maxLikDrawnMovement: Auxiliary variable used in the “pfAuxiliaryPFOptimal” algorithm.

std::vector<bool> m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed

template<class BINTYPE> 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

Parameters:

action – MUST be a “const CPose3D*”
observation – MUST be a “const CSensoryFrame*”
action – MUST be a “const mrpt::poses::CPose3D*”
observation – MUST be a “const CSensoryFrame*”

template<class BINTYPE> 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)

Virtual methods that the PF_implementations assume exist.

virtual mrpt::math::TPose3D getLastPose(size_t i, bool &is_valid_pose) const = 0

Return the last robot pose in the i’th particle; is_valid_pose will be false if there is no such last pose.

Throws:: std::exception – on out-of-range particle index

virtual void PF_SLAM_implementation_custom_update_particle_with_new_pose(PARTICLE_TYPE *particleData, const mrpt::math::TPose3D &newPose) const = 0

inline virtual void PF_SLAM_implementation_replaceByNewParticleSet(typename mrpt::bayes::CParticleFilterData<PARTICLE_TYPE, STORAGE>::CParticleList &old_particles, const std::vector<mrpt::math::TPose3D> &newParticles, const std::vector<double> &newParticlesWeight, const std::vector<size_t> &newParticlesDerivedFromIdx) const

This is the default algorithm to efficiently replace one old set of samples by another new set. The method uses pointers to make fast copies the first time each particle is duplicated, then makes real copies for the next ones.

Note that more efficient specializations might exist for specific particle data structs.

inline virtual bool PF_SLAM_implementation_doWeHaveValidObservations([[maybe_unused]] const typename mrpt::bayes::CParticleFilterData<PARTICLE_TYPE, STORAGE>::CParticleList &particles, [[maybe_unused]] const mrpt::obs::CSensoryFrame *sf) const

inline virtual bool PF_SLAM_implementation_skipRobotMovement() const: Make a specialization if needed, eg. in the first step in SLAM.

virtual double PF_SLAM_computeObservationLikelihoodForParticle(const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options, size_t particleIndexForMap, const mrpt::obs::CSensoryFrame &observation, const mrpt::poses::CPose3D &x) const = 0: Evaluate the observation likelihood for one particle at a given location

The generic PF implementations for localization & SLAM.

template<class BINTYPE> 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 sampling with rejection sampling approximation), common to both localization and mapping.

BINTYPE: TPoseBin or whatever to discretize the sample space for KLD-sampling.

This method implements optimal sampling with a rejection sampling-based approximation of the true posterior. For details, see the papers:

J.L. Blanco, J. Gonzalez, and J.-A. Fernandez-Madrigal, “An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization,” in Proc. IEEE International Conference on Robotics and Automation (ICRA’08), 2008, pp. 461-466.

A generic implementation of the PF method “prediction_and_update_pfAuxiliaryPFOptimal” (optimal sampling with rejection sampling approximation), common to both localization and mapping.

BINTYPE: TPoseBin or whatever to discretize the sample space for KLD-sampling.

This method implements optimal sampling with a rejection sampling-based approximation of the true posterior. For details, see the papers:

J.-L. Blanco, J. Gonzalez, and J.-A. Fernandez-Madrigal, “An Optimal Filtering Algorithm for Non-Parametric Observation Models in

Robot Localization,” in Proc. IEEE International Conference on Robotics and Automation (ICRA’08), 2008, pp. 461466.

template<class BINTYPE> 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 particle filter with the standard proposal), common to both localization and mapping.

BINTYPE: TPoseBin or whatever to discretize the sample space for KLD-sampling.

This method is described in the paper: Pitt, M.K.; Shephard, N. (1999). “Filtering Via Simulation: Auxiliary

Particle Filters”. Journal of the American Statistical Association 94 (446): 590-591. doi:10.2307/2670179.

template<class BINTYPE> 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, that is, a simple SIS particle filter), common to both localization and mapping.

BINTYPE: TPoseBin or whatever to discretize the sample space for KLD-sampling.

Public Functions

inline PF_implementation()

template<class BINTYPE> 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, otherwise, return false AND accumulate the odometry so when we have an observation we didn’t lose a thing. On return=true, the “m_movementDrawer” member is loaded and ready to draw samples of the increment of pose since last step. This method is smart enough to accumulate CActionRobotMovement2D or CActionRobotMovement3D, whatever comes in.

template<class BINTYPE> double PF_SLAM_particlesEvaluator_AuxPFOptimal(const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options, const mrpt::bayes::CParticleFilterCapable *obj, size_t index, [[maybe_unused]] const void *action, const void *observation)