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

#include <mrpt/poses/CPosePDFParticles.h>

Inheritance diagram for mrpt::poses::CPosePDFParticles:

[legend]

Collaboration diagram for mrpt::poses::CPosePDFParticles:

[legend]

List of all members.

Classes
struct	TPredictionParams
	The struct for passing extra simulation parameters to the prediction stage when running a particle filter. More...
Public Member Functions
void	clear ()
	Free all the memory associated to m_particles, and set the number of parts = 0.
	CPosePDFParticles (size_t M=1)
	Constructor.
	CPosePDFParticles (const CPosePDFParticles &obj)
	Copy constructor:
virtual	~CPosePDFParticles ()
	Destructor.
void	copyFrom (const CPosePDF &o)
	Copy operator, translating if necesary (for example, between m_particles and gaussian representations).
void	resetDeterministic (const CPose2D &location, size_t particlesCount=0)
	Reset the PDF to a single point: All m_particles will be set exactly to the supplied pose.
void	resetUniform (const double &x_min, const double &x_max, const double &y_min, const double &y_max, const double &phi_min=-M_PI, const double &phi_max=M_PI, const int &particlesCount=-1)
	Reset the PDF to an uniformly distributed one, inside of the defined cube.
void	resetUniformFreeSpace (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.
void	getMean (CPose2D &mean_pose) const
	Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF).
void	getCovarianceAndMean (CMatrixDouble33 &cov, CPose2D &mean_point) const
	Returns an estimate of the pose covariance matrix (3x3 cov matrix) and the mean, both at once.
CPose2D	getParticlePose (size_t i) const
	Returns the pose of the i'th particle.
void	prediction_and_update_pfStandardProposal (const mrpt::slam::CActionCollection action, const mrpt::slam::CSensoryFrame observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options)
	Update the m_particles, predicting the posterior of robot pose and map after a movement command.
void	prediction_and_update_pfAuxiliaryPFStandard (const mrpt::slam::CActionCollection action, const mrpt::slam::CSensoryFrame observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options)
	Update the m_particles, predicting the posterior of robot pose and map after a movement command.
void	prediction_and_update_pfAuxiliaryPFOptimal (const mrpt::slam::CActionCollection action, const mrpt::slam::CSensoryFrame observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options)
	Update the m_particles, predicting the posterior of robot pose and map after a movement command.
void	saveToTextFile (const std::string &file) const
	Save PDF's m_particles to a text file.
size_t	size () const
	Get the m_particles count (equivalent to "particlesCount").
void	performSubstitution (std::vector< int > &indx)
	Performs the substitution for internal use of resample in particle filter algorithm, don't call it directly.
void	changeCoordinatesReference (const CPose3D &newReferenceBase)
	This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which "to project" the current pdf.
void	drawSingleSample (CPose2D &outPart) const
	Draws a single sample from the distribution (WARNING: weights are assumed to be normalized!).
void	drawManySamples (size_t N, std::vector< vector_double > &outSamples) const
	Draws a number of samples from the distribution, and saves as a list of 1x3 vectors, where each row contains a (x,y,phi) datum.
void	operator+= (CPose2D Ap)
	Appends (pose-composition) a given pose "p" to each particle.
void	append (CPosePDFParticles &o)
	Appends (add to the list) a set of m_particles to the existing ones, and then normalize weights.
void	inverse (CPosePDF &o) const
	Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
CPose2D	getMostLikelyParticle () const
	Returns the particle with the highest weight.
void	bayesianFusion (CPosePDF &p1, CPosePDF &p2, const double &minMahalanobisDistToDrop=0)
	Bayesian fusion.
double	evaluatePDF_parzen (const double &x, const double &y, const double &phi, const double &stdXY, const double &stdPhi) const
	Evaluates the PDF at a given arbitrary point as reconstructed by a Parzen window.
void	saveParzenPDFToTextFile (const char *fileName, const double &x_min, const double &x_max, const double &y_min, const double &y_max, const double &phi, const double &stepSizeXY, const double &stdXY, const double &stdPhi) const
	Save a text file (compatible with matlab) representing the 2D evaluation of the PDF as reconstructed by a Parzen window.
Public Attributes
struct MRPTDLLIMPEXP mrpt::poses::CPosePDFParticles::TPredictionParams	options
	The struct for passing extra simulation parameters to the prediction stage when running a particle filter.
Protected Member Functions
Virtual methods that the PF_implementations assume exist.

const TPose3D *	getLastPose (const size_t i) const
	Return a pointer to the last robot pose in the i'th particle (or NULL if it's a path and it's empty).
void	PF_SLAM_implementation_custom_update_particle_with_new_pose (CParticleDataContent *particleData, const TPose3D &newPose) const
void	PF_SLAM_implementation_replaceByNewParticleSet (CParticleList &old_particles, const vector< TPose3D > &newParticles, const vector_double &newParticlesWeight, const vector< size_t > &newParticlesDerivedFromIdx) const
double	PF_SLAM_computeObservationLikelihoodForParticle (const CParticleFilter::TParticleFilterOptions &PF_options, const size_t particleIndexForMap, const CSensoryFrame &observation, const CPose3D &x) const
	Evaluate the observation likelihood for one particle at a given location.
Private Member Functions
	IMPLEMENT_PARTICLE_FILTER_CAPABLE (CPose2D)

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:: CPose2D, CPosePDF, CPoseGaussianPDF, CParticleFilterCapable

Definition at line 62 of file CPosePDFParticles.h.

Constructor & Destructor Documentation

mrpt::poses::CPosePDFParticles::CPosePDFParticles ( size_t M = 1 )

Constructor.

Parameters:

M

The number of m_particles.

mrpt::poses::CPosePDFParticles::CPosePDFParticles ( const CPosePDFParticles & obj ) [inline]

Copy constructor:

Definition at line 124 of file CPosePDFParticles.h.

virtual mrpt::poses::CPosePDFParticles::~CPosePDFParticles ( ) [virtual]

Destructor.

Member Function Documentation

void mrpt::poses::CPosePDFParticles::append ( CPosePDFParticles & o )

Appends (add to the list) a set of m_particles to the existing ones, and then normalize weights.

void mrpt::poses::CPosePDFParticles::bayesianFusion	(	CPosePDF &	p1,
		CPosePDF &	p2,
		const double &	minMahalanobisDistToDrop = `0`
	)

Bayesian fusion.

void mrpt::poses::CPosePDFParticles::changeCoordinatesReference ( const CPose3D & newReferenceBase )

This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which "to project" the current pdf.

Result PDF substituted the currently stored one in the object.

void mrpt::poses::CPosePDFParticles::clear ( )

Free all the memory associated to m_particles, and set the number of parts = 0.

void mrpt::poses::CPosePDFParticles::copyFrom ( const CPosePDF & o )

Copy operator, translating if necesary (for example, between m_particles and gaussian representations).

void mrpt::poses::CPosePDFParticles::drawManySamples	(	size_t	N,
		std::vector< vector_double > &	outSamples
	)			const

Draws a number of samples from the distribution, and saves as a list of 1x3 vectors, where each row contains a (x,y,phi) datum.

void mrpt::poses::CPosePDFParticles::drawSingleSample ( CPose2D & outPart ) const

Draws a single sample from the distribution (WARNING: weights are assumed to be normalized!).

double mrpt::poses::CPosePDFParticles::evaluatePDF_parzen	(	const double &	x,
		const double &	y,
		const double &	phi,
		const double &	stdXY,
		const double &	stdPhi
	)			const

Evaluates the PDF at a given arbitrary point as reconstructed by a Parzen window.

See also:: saveParzenPDFToTextFile

void mrpt::poses::CPosePDFParticles::getCovarianceAndMean	(	CMatrixDouble33 &	cov,
		CPose2D &	mean_point
	)			const

Returns an estimate of the pose covariance matrix (3x3 cov matrix) and the mean, both at once.

See also:: getMean

const TPose3D* mrpt::poses::CPosePDFParticles::getLastPose ( const size_t i ) const [protected, 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).

Implements mrpt::slam::PF_implementation< CPose2D >.

void mrpt::poses::CPosePDFParticles::getMean ( CPose2D & mean_pose ) const

Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF).

See also:: getCovariance

CPose2D mrpt::poses::CPosePDFParticles::getMostLikelyParticle ( ) const

Returns the particle with the highest weight.

CPose2D mrpt::poses::CPosePDFParticles::getParticlePose ( size_t i ) const

Returns the pose of the i'th particle.

mrpt::poses::CPosePDFParticles::IMPLEMENT_PARTICLE_FILTER_CAPABLE ( CPose2D ) [private]

void mrpt::poses::CPosePDFParticles::inverse ( CPosePDF & o ) const

Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.

void mrpt::poses::CPosePDFParticles::operator+= ( CPose2D Ap )

Appends (pose-composition) a given pose "p" to each particle.

void mrpt::poses::CPosePDFParticles::performSubstitution ( std::vector< int > & indx )

Performs the substitution for internal use of resample in particle filter algorithm, don't call it directly.

double mrpt::poses::CPosePDFParticles::PF_SLAM_computeObservationLikelihoodForParticle	(	const CParticleFilter::TParticleFilterOptions &	PF_options,
		const size_t	particleIndexForMap,
		const CSensoryFrame &	observation,
		const CPose3D &	x
	)			const `[protected, virtual]`

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

Implements mrpt::slam::PF_implementation< CPose2D >.

void mrpt::poses::CPosePDFParticles::PF_SLAM_implementation_custom_update_particle_with_new_pose	(	CParticleDataContent *	particleData,
		const TPose3D &	newPose
	)			const `[protected]`

void mrpt::poses::CPosePDFParticles::PF_SLAM_implementation_replaceByNewParticleSet	(	CParticleList &	old_particles,
		const vector< TPose3D > &	newParticles,
		const vector_double &	newParticlesWeight,
		const vector< size_t > &	newParticlesDerivedFromIdx
	)			const `[protected]`

void mrpt::poses::CPosePDFParticles::prediction_and_update_pfAuxiliaryPFOptimal	(	const mrpt::slam::CActionCollection *	action,
		const mrpt::slam::CSensoryFrame *	observation,
		const bayes::CParticleFilter::TParticleFilterOptions &	PF_options
	)			`[virtual]`

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 Sensorial Frame:

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

Reimplemented from mrpt::bayes::CParticleFilterCapable.

void mrpt::poses::CPosePDFParticles::prediction_and_update_pfAuxiliaryPFStandard	(	const mrpt::slam::CActionCollection *	action,
		const mrpt::slam::CSensoryFrame *	observation,
		const bayes::CParticleFilter::TParticleFilterOptions &	PF_options
	)			`[virtual]`

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 Sensorial Frame:

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

Reimplemented from mrpt::bayes::CParticleFilterCapable.

void mrpt::poses::CPosePDFParticles::prediction_and_update_pfStandardProposal	(	const mrpt::slam::CActionCollection *	action,
		const mrpt::slam::CSensoryFrame *	observation,
		const bayes::CParticleFilter::TParticleFilterOptions &	PF_options
	)			`[virtual]`

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 Sensorial Frame:

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

Reimplemented from mrpt::bayes::CParticleFilterCapable.

void mrpt::poses::CPosePDFParticles::resetDeterministic	(	const CPose2D &	location,
		size_t	particlesCount = `0`
	)

Reset the PDF to a single point: All m_particles will be set exactly to the supplied pose.

Parameters:

	location	The location to set all the m_particles.
	particlesCount	If this is set to 0 the number of m_particles remains unchanged.

See also:: resetUniform, resetUniformFreeSpace

void mrpt::poses::CPosePDFParticles::resetUniform	(	const double &	x_min,
		const double &	x_max,
		const double &	y_min,
		const double &	y_max,
		const double &	phi_min = `-M_PI`,
		const double &	phi_max = `M_PI`,
		const int &	particlesCount = `-1`
	)

Reset the PDF to an uniformly distributed one, inside of the defined cube.

If particlesCount is set to -1 the number of m_particles remains unchanged.

See also:: resetDeterministic, resetUniformFreeSpace

void mrpt::poses::CPosePDFParticles::resetUniformFreeSpace	(	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.

See also:: resetDeterm32inistic

Exceptions:

std::exception

On any error (no free cell found in map, map=NULL, etc...)

void mrpt::poses::CPosePDFParticles::saveParzenPDFToTextFile	(	const char *	fileName,
		const double &	x_min,
		const double &	x_max,
		const double &	y_min,
		const double &	y_max,
		const double &	phi,
		const double &	stepSizeXY,
		const double &	stdXY,
		const double &	stdPhi
	)			const

Save a text file (compatible with matlab) representing the 2D evaluation of the PDF as reconstructed by a Parzen window.

See also:: evaluatePDF_parzen

void mrpt::poses::CPosePDFParticles::saveToTextFile ( const std::string & file ) const

Save PDF's m_particles to a text file.

In each line it will go: "x y phi weight"

size_t mrpt::poses::CPosePDFParticles::size ( ) const [inline]

Get the m_particles count (equivalent to "particlesCount").

Definition at line 247 of file CPosePDFParticles.h.

Member Data Documentation

struct MRPTDLLIMPEXP mrpt::poses::CPosePDFParticles::TPredictionParams mrpt::poses::CPosePDFParticles::options

The struct for passing extra simulation parameters to the prediction stage when running a particle filter.

See also:: prediction

mrpt::poses::CPosePDFParticles Class Reference

Classes

Public Member Functions

Public Attributes

Protected Member Functions

Private Member Functions

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation