mrpt::slam::PF_implementation< PARTICLE_TYPE > Class Template Reference

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

#include <mrpt/slam/PF_implementations_data.h>

Collaboration diagram for mrpt::slam::PF_implementation< PARTICLE_TYPE >:

Public Types
typedef mrpt::bayes::CParticleFilterData < PARTICLE_TYPE >	BASE
typedef mrpt::bayes::CParticleFilterCapable	BASE2
Public Member Functions
	PF_implementation (mrpt::bayes::CParticleFilterData< PARTICLE_TYPE > &pfd, mrpt::bayes::CParticleFilterCapable &pfc)
Protected Member Functions
template<class BINTYPE >
bool	PF_SLAM_implementation_gatherActionsCheckBothActObs (const CActionCollection actions, const 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.
The generic PF implementations for localization & SLAM.

template<class BINTYPE >
void	PF_SLAM_implementation_pfAuxiliaryPFOptimal (const CActionCollection actions, const CSensoryFrame sf, const 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.
template<class BINTYPE >
void	PF_SLAM_implementation_pfAuxiliaryPFStandard (const CActionCollection actions, const CSensoryFrame sf, const 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.
template<class BINTYPE >
void	PF_SLAM_implementation_pfStandardProposal (const CActionCollection actions, const CSensoryFrame sf, const 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.
Virtual methods that the PF_implementations assume exist.

virtual const TPose3D *	getLastPose (const size_t i) const =0
	Return a pointer to the last robot pose in the i'th particle (or NULL if it's a path and it's empty).
virtual void	PF_SLAM_implementation_custom_update_particle_with_new_pose (PARTICLE_TYPE *particleData, const TPose3D &newPose) const =0
virtual void	PF_SLAM_implementation_replaceByNewParticleSet (typename BASE::CParticleList &old_particles, const vector< TPose3D > &newParticles, const vector_double &newParticlesWeight, const vector< size_t > &newParticlesDerivedFromIdx) const
	This is the default algorithm to efficiently replace one old set of samples by another new set.
virtual bool	PF_SLAM_implementation_doWeHaveValidObservations (const typename BASE::CParticleList &particles, const CSensoryFrame *sf) const
virtual bool	PF_SLAM_implementation_skipRobotMovement () const
	Make a specialization if needed, eg.
virtual double	PF_SLAM_computeObservationLikelihoodForParticle (const CParticleFilter::TParticleFilterOptions &PF_options, const size_t particleIndexForMap, const CSensoryFrame &observation, const CPose3D &x) const =0
	Evaluate the observation likelihood for one particle at a given location.
Protected Attributes
BASE &	m_partdata
	the particle filter data object, implementing CParticleFilterData<PARTICLE_TYPE>
BASE2 &	m_pfc
	the particle filter data object, implementing CParticleFilterCapable
Private Member Functions
template<class BINTYPE >
void	PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal (const CActionCollection actions, const CSensoryFrame sf, const 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.
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 CSensoryFrame *sf, const CParticleFilter::TParticleFilterOptions &PF_options, CPose3D &out_newPose, double &out_newParticleLogWeight)
Data members and methods used by generic PF implementations

CActionRobotMovement2D	m_accumRobotMovement2D
bool	m_accumRobotMovement2DIsValid
CPose3DPDFGaussian	m_accumRobotMovement3D
bool	m_accumRobotMovement3DIsValid
CPoseRandomSampler	m_movementDrawer
	Used in al PF implementations.
vector_double	m_pfAuxiliaryPFOptimal_estimatedProb
	Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
vector_double	m_pfAuxiliaryPFStandard_estimatedProb
	Auxiliary variable used in the "pfAuxiliaryPFStandard" algorithm.
vector_double	m_pfAuxiliaryPFOptimal_maxLikelihood
	Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
std::vector< 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 CParticleFilter::TParticleFilterOptions &PF_options, const 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.
template<class BINTYPE >
static double	PF_SLAM_particlesEvaluator_AuxPFOptimal (const CParticleFilter::TParticleFilterOptions &PF_options, const CParticleFilterCapable obj, size_t index, const void action, const void *observation)

Detailed Description

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

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

Definition at line 63 of file PF_implementations_data.h.

Member Typedef Documentation

template<class PARTICLE_TYPE>

typedef mrpt::bayes::CParticleFilterData<PARTICLE_TYPE> mrpt::slam::PF_implementation< PARTICLE_TYPE >::BASE

Definition at line 77 of file PF_implementations_data.h.

template<class PARTICLE_TYPE>

typedef mrpt::bayes::CParticleFilterCapable mrpt::slam::PF_implementation< PARTICLE_TYPE >::BASE2

Definition at line 78 of file PF_implementations_data.h.

Constructor & Destructor Documentation

template<class PARTICLE_TYPE>

mrpt::slam::PF_implementation< PARTICLE_TYPE >::PF_implementation	(	mrpt::bayes::CParticleFilterData< PARTICLE_TYPE > &	pfd,
		mrpt::bayes::CParticleFilterCapable &	pfc
	)			`[inline]`

Definition at line 66 of file PF_implementations_data.h.

Member Function Documentation

template<class PARTICLE_TYPE>

virtual const TPose3D* mrpt::slam::PF_implementation< PARTICLE_TYPE >::getLastPose ( const size_t i ) const [protected, pure virtual]

Return a pointer to the last robot pose in the i'th particle (or NULL if it's a path and it's empty).

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

Referenced by mrpt::slam::PF_implementation< PARTICLE_TYPE >::PF_SLAM_aux_perform_one_rejection_sampling_step(), and mrpt::slam::PF_implementation< PARTICLE_TYPE >::PF_SLAM_implementation_pfStandardProposal().

template<class PARTICLE_TYPE >

template<class BINTYPE >

void mrpt::slam::PF_implementation< PARTICLE_TYPE >::PF_SLAM_aux_perform_one_rejection_sampling_step	(	const bool	USE_OPTIMAL_SAMPLING,
		const bool	doResample,
		const double	maxMeanLik,
		size_t	k,
		const CSensoryFrame *	sf,
		const CParticleFilter::TParticleFilterOptions &	PF_options,
		CPose3D &	out_newPose,
		double &	out_newParticleLogWeight
	)			`[inline, private]`

Definition at line 850 of file PF_implementations.h.

template<class PARTICLE_TYPE>

virtual double mrpt::slam::PF_implementation< PARTICLE_TYPE >::PF_SLAM_computeObservationLikelihoodForParticle	(	const CParticleFilter::TParticleFilterOptions &	PF_options,
		const size_t	particleIndexForMap,
		const CSensoryFrame &	observation,
		const CPose3D &	x
	)			const `[protected, pure virtual]`

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

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

Referenced by mrpt::slam::PF_implementation< PARTICLE_TYPE >::PF_SLAM_aux_perform_one_rejection_sampling_step(), mrpt::slam::PF_implementation< PARTICLE_TYPE >::PF_SLAM_implementation_pfStandardProposal(), and mrpt::slam::PF_implementation< PARTICLE_TYPE >::PF_SLAM_particlesEvaluator_AuxPFOptimal().

template<class PARTICLE_TYPE>

virtual void mrpt::slam::PF_implementation< PARTICLE_TYPE >::PF_SLAM_implementation_custom_update_particle_with_new_pose	(	PARTICLE_TYPE *	particleData,
		const TPose3D &	newPose
	)			const `[protected, pure virtual]`

Referenced by mrpt::slam::PF_implementation< PARTICLE_TYPE >::PF_SLAM_implementation_pfStandardProposal(), and mrpt::slam::PF_implementation< CRBPFParticleData >::PF_SLAM_implementation_replaceByNewParticleSet().

template<class PARTICLE_TYPE>

virtual bool mrpt::slam::PF_implementation< PARTICLE_TYPE >::PF_SLAM_implementation_doWeHaveValidObservations	(	const typename BASE::CParticleList &	particles,
		const CSensoryFrame *	sf
	)			const `[inline, protected, virtual]`

Definition at line 261 of file PF_implementations_data.h.

Referenced by mrpt::slam::PF_implementation< PARTICLE_TYPE >::PF_SLAM_implementation_gatherActionsCheckBothActObs().

template<class PARTICLE_TYPE >

template<class BINTYPE >

bool mrpt::slam::PF_implementation< PARTICLE_TYPE >::PF_SLAM_implementation_gatherActionsCheckBothActObs	(	const CActionCollection *	actions,
		const CSensoryFrame *	sf
	)			`[inline, protected]`

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.

Definition at line 69 of file PF_implementations.h.

References mrpt::slam::CActionRobotMovement2D::computeFromOdometry(), mrpt::slam::CActionCollection::getActionByClass(), mrpt::slam::CActionCollection::getBestMovementEstimation(), mrpt::slam::PF_implementation< PARTICLE_TYPE >::m_accumRobotMovement2D, mrpt::slam::PF_implementation< PARTICLE_TYPE >::m_accumRobotMovement2DIsValid, mrpt::slam::PF_implementation< PARTICLE_TYPE >::m_accumRobotMovement3D, mrpt::slam::PF_implementation< PARTICLE_TYPE >::m_accumRobotMovement3DIsValid, mrpt::slam::PF_implementation< PARTICLE_TYPE >::m_movementDrawer, mrpt::slam::PF_implementation< PARTICLE_TYPE >::m_partdata, mrpt::bayes::CParticleFilterData< T >::m_particles, mrpt::slam::CActionRobotMovement2D::motionModelConfiguration, mrpt::slam::PF_implementation< PARTICLE_TYPE >::PF_SLAM_implementation_doWeHaveValidObservations(), mrpt::slam::CActionRobotMovement2D::poseChange, mrpt::slam::CActionRobotMovement2D::rawOdometryIncrementReading, mrpt::poses::CPoseRandomSampler::setPosePDF(), and THROW_EXCEPTION.

template<class PARTICLE_TYPE >

template<class BINTYPE >

void mrpt::slam::PF_implementation< PARTICLE_TYPE >::PF_SLAM_implementation_pfAuxiliaryPFOptimal	(	const CActionCollection *	actions,
		const CSensoryFrame *	sf,
		const CParticleFilter::TParticleFilterOptions &	PF_options,
		const TKLDParams &	KLD_options
	)			`[inline, 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. González, and J.-A. Fernández-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. GonzÃ¡lez, and J.-A. FernÃ¡ndez-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.

Definition at line 151 of file PF_implementations.h.

template<class PARTICLE_TYPE >

template<class BINTYPE >

void mrpt::slam::PF_implementation< PARTICLE_TYPE >::PF_SLAM_implementation_pfAuxiliaryPFStandard	(	const CActionCollection *	actions,
		const CSensoryFrame *	sf,
		const CParticleFilter::TParticleFilterOptions &	PF_options,
		const TKLDParams &	KLD_options
	)			`[inline, 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.

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 334 of file PF_implementations.h.

template<class PARTICLE_TYPE >

template<class BINTYPE >

void mrpt::slam::PF_implementation< PARTICLE_TYPE >::PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal	(	const CActionCollection *	actions,
		const CSensoryFrame *	sf,
		const CParticleFilter::TParticleFilterOptions &	PF_options,
		const TKLDParams &	KLD_options,
		const bool	USE_OPTIMAL_SAMPLING
	)			`[inline, private]`

The shared implementation body of two PF methods: APF and Optimal-APF, depending on USE_OPTIMAL_SAMPLING.

Definition at line 511 of file PF_implementations.h.

template<class PARTICLE_TYPE >

template<class BINTYPE >

void mrpt::slam::PF_implementation< PARTICLE_TYPE >::PF_SLAM_implementation_pfStandardProposal	(	const CActionCollection *	actions,
		const CSensoryFrame *	sf,
		const CParticleFilter::TParticleFilterOptions &	PF_options,
		const TKLDParams &	KLD_options
	)			`[inline, protected]`

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.

Definition at line 169 of file PF_implementations.h.

References mrpt::bayes::CParticleFilter::TParticleFilterOptions::adaptiveSampleSize, mrpt::math::chi2inv(), mrpt::poses::CPoseRandomSampler::drawSample(), mrpt::bayes::CParticleFilterCapable::fastDrawSample(), mrpt::slam::CActionCollection::getActionByClass(), mrpt::slam::CActionCollection::getBestMovementEstimation(), mrpt::slam::PF_implementation< PARTICLE_TYPE >::getLastPose(), mrpt::slam::TKLDParams::KLD_delta, mrpt::slam::TKLDParams::KLD_epsilon, mrpt::slam::TKLDParams::KLD_maxSampleSize, mrpt::slam::TKLDParams::KLD_minSampleSize, mrpt::slam::PF_implementation< PARTICLE_TYPE >::m_movementDrawer, mrpt::slam::PF_implementation< PARTICLE_TYPE >::m_partdata, mrpt::bayes::CParticleFilterData< T >::m_particles, mrpt::slam::PF_implementation< PARTICLE_TYPE >::m_pfc, MRPT_END, MRPT_START, mrpt::slam::PF_implementation< PARTICLE_TYPE >::PF_SLAM_computeObservationLikelihoodForParticle(), mrpt::slam::PF_implementation< PARTICLE_TYPE >::PF_SLAM_implementation_custom_update_particle_with_new_pose(), mrpt::slam::PF_implementation< PARTICLE_TYPE >::PF_SLAM_implementation_replaceByNewParticleSet(), mrpt::bayes::CParticleFilter::TParticleFilterOptions::powFactor, mrpt::bayes::CParticleFilterCapable::prepareFastDrawSample(), mrpt::utils::round(), mrpt::poses::CPoseRandomSampler::setPosePDF(), and THROW_EXCEPTION.

template<class PARTICLE_TYPE>

virtual void mrpt::slam::PF_implementation< PARTICLE_TYPE >::PF_SLAM_implementation_replaceByNewParticleSet	(	typename BASE::CParticleList &	old_particles,
		const vector< TPose3D > &	newParticles,
		const vector_double &	newParticlesWeight,
		const vector< size_t > &	newParticlesDerivedFromIdx
	)			const `[inline, protected, virtual]`

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.

Reimplemented in mrpt::slam::CMonteCarloLocalization2D, and mrpt::slam::CMonteCarloLocalization3D.

Definition at line 196 of file PF_implementations_data.h.

Referenced by mrpt::slam::PF_implementation< PARTICLE_TYPE >::PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal(), and mrpt::slam::PF_implementation< PARTICLE_TYPE >::PF_SLAM_implementation_pfStandardProposal().

template<class PARTICLE_TYPE>

virtual bool mrpt::slam::PF_implementation< PARTICLE_TYPE >::PF_SLAM_implementation_skipRobotMovement ( ) const [inline, protected, virtual]

Make a specialization if needed, eg.

in the first step in SLAM.

Reimplemented in mrpt::slam::CMultiMetricMapPDF.

Definition at line 269 of file PF_implementations_data.h.

Referenced by mrpt::slam::PF_implementation< PARTICLE_TYPE >::PF_SLAM_aux_perform_one_rejection_sampling_step().

template<class PARTICLE_TYPE >

template<class BINTYPE >

double mrpt::slam::PF_implementation< PARTICLE_TYPE >::PF_SLAM_particlesEvaluator_AuxPFOptimal	(	const CParticleFilter::TParticleFilterOptions &	PF_options,
		const CParticleFilterCapable *	obj,
		size_t	index,
		const void *	action,
		const void *	observation
	)			`[inline, static, protected]`

Definition at line 349 of file PF_implementations.h.

References ASSERT_, mrpt::math::averageLogLikelihood(), mrpt::slam::PF_implementation< PARTICLE_TYPE >::m_pfAuxiliaryPFOptimal_maxLikDrawnMovement, MRPT_CHECK_NORMAL_NUMBER, MRPT_END, MRPT_START, mrpt::slam::PF_implementation< PARTICLE_TYPE >::PF_SLAM_computeObservationLikelihoodForParticle(), mrpt::bayes::CParticleFilter::TParticleFilterOptions::pfAuxFilterOptimal_MaximumSearchSamples, and mrpt::bayes::CParticleFilter::TParticleFilterOptions::pfAuxFilterOptimal_MLE.

template<class PARTICLE_TYPE >

template<class BINTYPE >

double mrpt::slam::PF_implementation< PARTICLE_TYPE >::PF_SLAM_particlesEvaluator_AuxPFStandard	(	const CParticleFilter::TParticleFilterOptions &	PF_options,
		const CParticleFilterCapable *	obj,
		size_t	index,
		const void *	action,
		const void *	observation
	)			`[inline, static, protected]`