Detailed Description

Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x,y,phi), using a set of weighted samples.

This class also implements particle filtering for robot localization. See the MRPT application "app/pf-localization" for an example of usage.

See also: CMonteCarloLocalization3D, CPose2D, CPosePDF, CPoseGaussianPDF,

Definition at line 39 of file CMonteCarloLocalization2D.h.

#include <mrpt/slam/CMonteCarloLocalization2D.h>

Inheritance diagram for mrpt::slam::CMonteCarloLocalization2D:

Public Types
using	CParticleDataContent = mrpt::poses::CPosePDFParticles::CParticleDataContent

using	CParticleList = mrpt::poses::CPosePDFParticles::CParticleList

Public Member Functions
	CMonteCarloLocalization2D (size_t M=1)
	Constructor. More...

virtual	~CMonteCarloLocalization2D ()
	Destructor. More...

void	resetUniformFreeSpace (mrpt::maps::COccupancyGridMap2D *theMap, const double freeCellsThreshold=0.7, const int particlesCount=-1, const double x_min=-1e10f, const double x_max=1e10f, const double y_min=-1e10f, const double y_max=1e10f, const double phi_min=-M_PI, const double phi_max=M_PI)
	Reset the PDF to an uniformly distributed one, but only in the free-space of a given 2D occupancy-grid-map. More...

template<class T >
void	prediction_and_update (const mrpt::obs::CActionCollection actions, const mrpt::obs::CSensoryFrame observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options)
	Update the m_particles, predicting the posterior of robot pose and map after a movement command. More...

void	executeOn (mrpt::bayes::CParticleFilter &pf, const mrpt::obs::CActionCollection action, const mrpt::obs::CSensoryFrame observation, mrpt::bayes::CParticleFilter::TParticleFilterStats *stats, mrpt::bayes::CParticleFilter::TParticleFilterAlgorithm PF_algorithm)

Virtual methods that the PF_implementations assume exist.
mrpt::math::TPose3D	getLastPose (const size_t i, bool &is_valid_pose) const override
	Return the robot pose for the i'th particle. More...

void	PF_SLAM_implementation_custom_update_particle_with_new_pose (CParticleDataContent *particleData, const mrpt::math::TPose3D &newPose) const override

void	PF_SLAM_implementation_replaceByNewParticleSet (CParticleList &old_particles, const std::vector< mrpt::math::TPose3D > &newParticles, const std::vector< double > &newParticlesWeight, const std::vector< size_t > &newParticlesDerivedFromIdx) const override

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 override
	Evaluate the observation likelihood for one particle at a given location. More...

Virtual methods that the PF_implementations assume exist.
virtual void	PF_SLAM_implementation_custom_update_particle_with_new_pose (mrpt::poses::CPose2D *particleData, const mrpt::math::TPose3D &newPose) const=0

virtual void	PF_SLAM_implementation_replaceByNewParticleSet (typename mrpt::poses::CPosePDFParticles ::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< mrpt::poses::CPose2D >::typename mrpt::poses::CPosePDFParticles ::CParticleList &particles, const mrpt::obs::CSensoryFrame *sf) const

virtual bool	PF_SLAM_implementation_skipRobotMovement () const
	Make a specialization if needed, eg. More...

Public Attributes
mrpt::poses::CPosePDFParticles	m_poseParticles

TMonteCarloLocalizationParams	options
	MCL parameters. More...

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::TPose3D >	m_pfAuxiliaryPFOptimal_maxLikDrawnMovement
	Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. More...

std::vector< bool >	m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed

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

Member Typedef Documentation

◆ CParticleDataContent

using mrpt::slam::CMonteCarloLocalization2D::CParticleDataContent = mrpt::poses::CPosePDFParticles::CParticleDataContent

Definition at line 45 of file CMonteCarloLocalization2D.h.

◆ CParticleList

using mrpt::slam::CMonteCarloLocalization2D::CParticleList = mrpt::poses::CPosePDFParticles::CParticleList

Definition at line 46 of file CMonteCarloLocalization2D.h.

Constructor & Destructor Documentation

◆ CMonteCarloLocalization2D()

mrpt::slam::CMonteCarloLocalization2D::CMonteCarloLocalization2D ( size_t M = 1 )

Constructor.

Parameters

M	The number of m_particles.

Definition at line 72 of file CMonteCarloLocalization2D.cpp.

Referenced by executeOn(), and prediction_and_update().

Here is the caller graph for this function:

◆ ~CMonteCarloLocalization2D()

mrpt::slam::CMonteCarloLocalization2D::~CMonteCarloLocalization2D ( )

virtual

Destructor.

Definition at line 81 of file CMonteCarloLocalization2D.cpp.

Member Function Documentation

◆ executeOn()

void mrpt::slam::CMonteCarloLocalization2D::executeOn	(	mrpt::bayes::CParticleFilter &	pf,
		const mrpt::obs::CActionCollection *	action,
		const mrpt::obs::CSensoryFrame *	observation,
		mrpt::bayes::CParticleFilter::TParticleFilterStats *	stats,
		mrpt::bayes::CParticleFilter::TParticleFilterAlgorithm	PF_algorithm
	)

Definition at line 236 of file CMonteCarloLocalization2D.cpp.

References CMonteCarloLocalization2D(), mrpt::bayes::CParticleFilter::executeOn(), and THROW_EXCEPTION.

Referenced by run_test_pf_localization().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getLastPose()

TPose3D mrpt::slam::CMonteCarloLocalization2D::getLastPose	(	const size_t	i,
		bool &	is_valid_pose
	)		const

overridevirtual

Return the robot pose for the i'th particle.

is_valid is always true in this class.

Implements mrpt::slam::PF_implementation< mrpt::poses::CPose2D, mrpt::poses::CPosePDFParticles >.

Definition at line 82 of file CMonteCarloLocalization2D.cpp.

References ASSERTDEB_, mrpt::bayes::CParticleFilterData< T >::m_particles, m_poseParticles, and THROW_EXCEPTION.

◆ PF_SLAM_computeObservationLikelihoodForParticle()

double mrpt::slam::CMonteCarloLocalization2D::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

overridevirtual

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

Implements mrpt::slam::PF_implementation< mrpt::poses::CPose2D, mrpt::poses::CPosePDFParticles >.

Definition at line 96 of file CMonteCarloLocalization2D.cpp.

References ASSERT_, mrpt::obs::CSensoryFrame::begin(), mrpt::obs::CSensoryFrame::end(), mrpt::slam::TMonteCarloLocalizationParams::metricMap, mrpt::slam::TMonteCarloLocalizationParams::metricMaps, MRPT_UNUSED_PARAM, and options.

Here is the call graph for this function:

◆ PF_SLAM_implementation_custom_update_particle_with_new_pose() [1/2]

virtual void mrpt::slam::PF_implementation< mrpt::poses::CPose2D , mrpt::poses::CPosePDFParticles , typename mrpt::poses::CPosePDFParticles ::CParticleList >::PF_SLAM_implementation_custom_update_particle_with_new_pose	(	mrpt::poses::CPose2D *	particleData,
		const mrpt::math::TPose3D &	newPose
	)		const

pure virtualinherited

◆ PF_SLAM_implementation_custom_update_particle_with_new_pose() [2/2]

void mrpt::slam::CMonteCarloLocalization2D::PF_SLAM_implementation_custom_update_particle_with_new_pose	(	CParticleDataContent *	particleData,
		const mrpt::math::TPose3D &	newPose
	)		const

override

Definition at line 122 of file CMonteCarloLocalization2D.cpp.

◆ PF_SLAM_implementation_doWeHaveValidObservations()

virtual bool mrpt::slam::PF_implementation< mrpt::poses::CPose2D , mrpt::poses::CPosePDFParticles , typename mrpt::poses::CPosePDFParticles ::CParticleList >::PF_SLAM_implementation_doWeHaveValidObservations	(	const typename mrpt::bayes::CParticleFilterData< mrpt::poses::CPose2D >::typename mrpt::poses::CPosePDFParticles ::CParticleList &	particles,
		const mrpt::obs::CSensoryFrame *	sf
	)		const

inlinevirtualinherited

Definition at line 179 of file PF_implementations_data.h.

References MRPT_UNUSED_PARAM.

◆ PF_SLAM_implementation_replaceByNewParticleSet() [1/2]

virtual void mrpt::slam::PF_implementation< mrpt::poses::CPose2D , mrpt::poses::CPosePDFParticles , typename mrpt::poses::CPosePDFParticles ::CParticleList >::PF_SLAM_implementation_replaceByNewParticleSet	(	typename mrpt::poses::CPosePDFParticles ::CParticleList &	old_particles,
		const std::vector< mrpt::math::TPose3D > &	newParticles,
		const std::vector< double > &	newParticlesWeight,
		const std::vector< size_t > &	newParticlesDerivedFromIdx
	)		const

inlinevirtualinherited

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 105 of file PF_implementations_data.h.

References ASSERT_, and mrpt::slam::PF_implementation< PARTICLE_TYPE, PDF_TYPE, CParticleList >::PF_SLAM_implementation_custom_update_particle_with_new_pose().

◆ PF_SLAM_implementation_replaceByNewParticleSet() [2/2]

void mrpt::slam::CMonteCarloLocalization2D::PF_SLAM_implementation_replaceByNewParticleSet	(	CParticleList &	old_particles,
		const std::vector< mrpt::math::TPose3D > &	newParticles,
		const std::vector< double > &	newParticlesWeight,
		const std::vector< size_t > &	newParticlesDerivedFromIdx
	)		const

override

Definition at line 128 of file CMonteCarloLocalization2D.cpp.

References ASSERT_EQUAL_, and MRPT_UNUSED_PARAM.

◆ PF_SLAM_implementation_skipRobotMovement()

virtual bool mrpt::slam::PF_implementation< mrpt::poses::CPose2D , mrpt::poses::CPosePDFParticles , typename mrpt::poses::CPosePDFParticles ::CParticleList >::PF_SLAM_implementation_skipRobotMovement ( ) const

inlinevirtualinherited

Make a specialization if needed, eg.

in the first step in SLAM.

Definition at line 190 of file PF_implementations_data.h.

◆ PF_SLAM_particlesEvaluator_AuxPFStandard()

static double mrpt::slam::PF_implementation< mrpt::poses::CPose2D , mrpt::poses::CPosePDFParticles , typename mrpt::poses::CPosePDFParticles ::CParticleList >::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
	)

staticinherited

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*"

◆ prediction_and_update()

template<class T >

void mrpt::slam::CMonteCarloLocalization2D::prediction_and_update	(	const mrpt::obs::CActionCollection *	actions,
		const mrpt::obs::CSensoryFrame *	observation,
		const bayes::CParticleFilter::TParticleFilterOptions &	PF_options
	)

inline

Update the m_particles, predicting the posterior of robot pose and map after a movement command.

This method has additional configuration parameters in "options". Performs the update stage of the RBPF, using the sensed CSensoryFrame:

Parameters

action	This is a pointer to CActionCollection, containing the pose change the robot has been commanded.
observation	This must be a pointer to a CSensoryFrame object, with robot sensed observations.

See also: options

Definition at line 100 of file CMonteCarloLocalization2D.h.

References ASSERT_, CMonteCarloLocalization2D(), mrpt::slam::TMonteCarloLocalizationParams::KLD_params, mrpt::bayes::CParticleFilterData< T >::m_particles, m_poseParticles, mrpt::slam::TMonteCarloLocalizationParams::metricMap, mrpt::slam::TMonteCarloLocalizationParams::metricMaps, MRPT_END, MRPT_START, and options.

Here is the call graph for this function:

◆ resetUniformFreeSpace()

void mrpt::slam::CMonteCarloLocalization2D::resetUniformFreeSpace	(	mrpt::maps::COccupancyGridMap2D *	theMap,
		const double	freeCellsThreshold = `0.7`,
		const int	particlesCount = `-1`,
		const double	x_min = `-1e10f`,
		const double	x_max = `1e10f`,
		const double	y_min = `-1e10f`,
		const double	y_max = `1e10f`,
		const double	phi_min = `-M_PI`,
		const double	phi_max = `M_PI`
	)

Reset the PDF to an uniformly distributed one, but only in the free-space of a given 2D occupancy-grid-map.

Orientation is randomly generated in the whole 2*PI range.

Parameters

theMap	The occupancy grid map
freeCellsThreshold	The minimum free-probability to consider a cell as empty (default is 0.7)
particlesCount	If set to -1 the number of m_particles remains unchanged.
x_min	The limits of the area to look for free cells.
x_max	The limits of the area to look for free cells.
y_min	The limits of the area to look for free cells.
y_max	The limits of the area to look for free cells.
phi_min	The limits of the area to look for free cells.
phi_max	The limits of the area to look for free cells.