Main MRPT website > C++ reference for MRPT 1.5.6
List of all members | Public Member Functions | Private Member Functions
mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF > Class Template Referenceabstract

Detailed Description

template<class PARTICLE_TYPE, class MYSELF>
class mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >

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

Definition at line 41 of file PF_implementations_data.h.

#include <mrpt/slam/PF_implementations_data.h>

Inheritance diagram for mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >:
Inheritance graph

Public Member Functions

 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. More...
 
Virtual methods that the PF_implementations assume exist.
virtual mrpt::math::TPose3D getLastPose (const size_t i, bool &pose_is_valid) const =0
 Return the last robot pose in the i'th particle; pose_is_valid will be false if particle is a path and it's empty. More...
 
virtual void PF_SLAM_implementation_custom_update_particle_with_new_pose (PARTICLE_TYPE *particleData, const mrpt::math::TPose3D &newPose) const =0
 
virtual void PF_SLAM_implementation_replaceByNewParticleSet (typename mrpt::bayes::CParticleFilterData< PARTICLE_TYPE >::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. More...
 
virtual bool PF_SLAM_implementation_doWeHaveValidObservations (const typename mrpt::bayes::CParticleFilterData< PARTICLE_TYPE >::CParticleList &particles, const mrpt::obs::CSensoryFrame *sf) const
 
virtual bool PF_SLAM_implementation_skipRobotMovement () const
 Make a specialization if needed, eg. More...
 
virtual double PF_SLAM_computeObservationLikelihoodForParticle (const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options, const 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. More...
 

Protected Member Functions

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. More...
 
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. More...
 
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. More...
 

Private Member Functions

template<class BINTYPE >
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_SAMPLING. More...
 
template<class BINTYPE >
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)
 

Data members and methods used by generic PF implementations

mrpt::obs::CActionRobotMovement2D m_accumRobotMovement2D
 
bool m_accumRobotMovement2DIsValid
 
mrpt::poses::CPose3DPDFGaussian m_accumRobotMovement3D
 
bool m_accumRobotMovement3DIsValid
 
mrpt::poses::CPoseRandomSampler m_movementDrawer
 Used in al PF implementations. More...
 
mrpt::math::CVectorDouble m_pfAuxiliaryPFOptimal_estimatedProb
 Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. More...
 
mrpt::math::CVectorDouble m_pfAuxiliaryPFStandard_estimatedProb
 Auxiliary variable used in the "pfAuxiliaryPFStandard" algorithm. More...
 
mrpt::math::CVectorDouble m_pfAuxiliaryPFOptimal_maxLikelihood
 Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. More...
 
std::vector< mrpt::math::TPose3Dm_pfAuxiliaryPFOptimal_maxLikDrawnMovement
 Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. More...
 
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. More...
 
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)
 

Constructor & Destructor Documentation

template<class PARTICLE_TYPE, class MYSELF>
mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::PF_implementation ( )
inline

Definition at line 45 of file PF_implementations_data.h.

Member Function Documentation

template<class PARTICLE_TYPE, class MYSELF>
virtual mrpt::math::TPose3D mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::getLastPose ( const size_t  i,
bool &  pose_is_valid 
) const
pure virtual

Return the last robot pose in the i'th particle; pose_is_valid will be false if particle is a path and it's empty.

Implemented in mrpt::maps::CMultiMetricMapPDF, mrpt::slam::CMonteCarloLocalization2D, and mrpt::slam::CMonteCarloLocalization3D.

template<class PARTICLE_TYPE , class MYSELF >
template<class BINTYPE >
void mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::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 
)
private
template<class PARTICLE_TYPE, class MYSELF>
virtual double mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::PF_SLAM_computeObservationLikelihoodForParticle ( const mrpt::bayes::CParticleFilter::TParticleFilterOptions PF_options,
const size_t  particleIndexForMap,
const mrpt::obs::CSensoryFrame observation,
const mrpt::poses::CPose3D x 
) const
pure virtual

Evaluate the observation likelihood for one particle at a given location.

Implemented in mrpt::maps::CMultiMetricMapPDF, mrpt::slam::CMonteCarloLocalization2D, and mrpt::slam::CMonteCarloLocalization3D.

template<class PARTICLE_TYPE, class MYSELF>
virtual void mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::PF_SLAM_implementation_custom_update_particle_with_new_pose ( PARTICLE_TYPE *  particleData,
const mrpt::math::TPose3D newPose 
) const
pure virtual
template<class PARTICLE_TYPE, class MYSELF>
virtual bool mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::PF_SLAM_implementation_doWeHaveValidObservations ( const typename mrpt::bayes::CParticleFilterData< PARTICLE_TYPE >::CParticleList &  particles,
const mrpt::obs::CSensoryFrame sf 
) const
inlinevirtual

Definition at line 230 of file PF_implementations_data.h.

template<class PARTICLE_TYPE , class MYSELF >
template<class BINTYPE >
void mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::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 
)
protected

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.

  • 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.

Definition at line 130 of file PF_implementations.h.

template<class PARTICLE_TYPE , class MYSELF >
template<class BINTYPE >
void mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::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 
)
protected

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.

Definition at line 321 of file PF_implementations.h.

template<class PARTICLE_TYPE , class MYSELF >
template<class BINTYPE >
void mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::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 
)
private
template<class PARTICLE_TYPE , class MYSELF >
template<class BINTYPE >
void mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::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 
)
protected
template<class PARTICLE_TYPE, class MYSELF>
virtual void mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::PF_SLAM_implementation_replaceByNewParticleSet ( typename mrpt::bayes::CParticleFilterData< PARTICLE_TYPE >::CParticleList &  old_particles,
const std::vector< mrpt::math::TPose3D > &  newParticles,
const std::vector< double > &  newParticlesWeight,
const std::vector< size_t > &  newParticlesDerivedFromIdx 
) const
inlinevirtual

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.

Definition at line 164 of file PF_implementations_data.h.

template<class PARTICLE_TYPE, class MYSELF>
virtual bool mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::PF_SLAM_implementation_skipRobotMovement ( ) const
inlinevirtual

Make a specialization if needed, eg.

in the first step in SLAM.

Reimplemented in mrpt::maps::CMultiMetricMapPDF.

Definition at line 239 of file PF_implementations_data.h.

template<class PARTICLE_TYPE , class MYSELF >
template<class BINTYPE >
double mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::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 
)
staticprotected
template<class PARTICLE_TYPE , class MYSELF >
template<class BINTYPE >
double mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::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 
)
staticprotected

Compute w[i]*p(z_t | mu_t^i), with mu_t^i being the mean of the new robot pose.

Parameters
actionMUST be a "const CPose3D*"
observationMUST be a "const CSensoryFrame*"
actionMUST be a "const mrpt::poses::CPose3D*"
observationMUST be a "const CSensoryFrame*"

Definition at line 412 of file PF_implementations.h.

References ASSERT_, mrpt::math::averageLogLikelihood(), mrpt::poses::CPose3D::composeFrom(), MRPT_CHECK_NORMAL_NUMBER, MRPT_END, MRPT_START, mrpt::bayes::CParticleFilter::TParticleFilterOptions::pfAuxFilterOptimal_MaximumSearchSamples, mrpt::bayes::CParticleFilter::TParticleFilterOptions::pfAuxFilterOptimal_MLE, and mrpt::bayes::CParticleFilter::TParticleFilterOptions::pfAuxFilterStandard_FirstStageWeightsMonteCarlo.

Member Data Documentation

template<class PARTICLE_TYPE, class MYSELF>
mrpt::obs::CActionRobotMovement2D mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::m_accumRobotMovement2D
protected

Definition at line 55 of file PF_implementations_data.h.

template<class PARTICLE_TYPE, class MYSELF>
bool mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::m_accumRobotMovement2DIsValid
protected

Definition at line 56 of file PF_implementations_data.h.

template<class PARTICLE_TYPE, class MYSELF>
mrpt::poses::CPose3DPDFGaussian mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::m_accumRobotMovement3D
protected

Definition at line 57 of file PF_implementations_data.h.

template<class PARTICLE_TYPE, class MYSELF>
bool mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::m_accumRobotMovement3DIsValid
protected

Definition at line 58 of file PF_implementations_data.h.

template<class PARTICLE_TYPE, class MYSELF>
mrpt::poses::CPoseRandomSampler mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::m_movementDrawer
protected

Used in al PF implementations.

See Also
PF_SLAM_implementation_gatherActionsCheckBothActObs

Definition at line 60 of file PF_implementations_data.h.

template<class PARTICLE_TYPE, class MYSELF>
mrpt::math::CVectorDouble mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::m_pfAuxiliaryPFOptimal_estimatedProb
mutableprotected

Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.

Definition at line 61 of file PF_implementations_data.h.

template<class PARTICLE_TYPE, class MYSELF>
std::vector<mrpt::math::TPose3D> mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::m_pfAuxiliaryPFOptimal_maxLikDrawnMovement
mutableprotected

Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.

Definition at line 64 of file PF_implementations_data.h.

template<class PARTICLE_TYPE, class MYSELF>
mrpt::math::CVectorDouble mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::m_pfAuxiliaryPFOptimal_maxLikelihood
mutableprotected

Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.

Definition at line 63 of file PF_implementations_data.h.

template<class PARTICLE_TYPE, class MYSELF>
std::vector<bool> mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed
protected

Definition at line 65 of file PF_implementations_data.h.

template<class PARTICLE_TYPE, class MYSELF>
mrpt::math::CVectorDouble mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::m_pfAuxiliaryPFStandard_estimatedProb
mutableprotected

Auxiliary variable used in the "pfAuxiliaryPFStandard" algorithm.

Definition at line 62 of file PF_implementations_data.h.




Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018