Detailed Description

Declares a class that represents a Rao-Blackwellized set of particles for solving the SLAM problem (This class is the base of RBPF-SLAM applications).

This class is used internally by the map building algorithm in "mrpt::slam::CMetricMapBuilderRBPF"

See also: mrpt::slam::CMetricMapBuilderRBPF

Definition at line 67 of file CMultiMetricMapPDF.h.

#include <mrpt/maps/CMultiMetricMapPDF.h>

Inheritance diagram for mrpt::maps::CMultiMetricMapPDF:

Classes
struct	TPredictionParams
	The struct for passing extra simulation parameters to the prediction/update stage when running a particle filter. More...

Public Types
using	ParticleData = CMultiMetricMapPDFParticles

using	CParticleList = ParticleData::CParticleList

using	CParticleDataContent = ParticleData::CParticleDataContent

Public Member Functions
void *	operator new (size_t size)

void *	operator new[] (size_t size)

void	operator delete (void *ptr) noexcept

void	operator delete[] (void *ptr) noexcept

void	operator delete (void memory, void ptr) noexcept

void *	operator new (size_t size, const std::nothrow_t &) noexcept

void	operator delete (void *ptr, const std::nothrow_t &) noexcept

template<class PF_ALGORITHM >
void	prediction_and_update (const mrpt::obs::CActionCollection actions, const mrpt::obs::CSensoryFrame observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options)

	CMultiMetricMapPDF (const bayes::CParticleFilter::TParticleFilterOptions &opts=bayes::CParticleFilter::TParticleFilterOptions(), const mrpt::maps::TSetOfMetricMapInitializers mapsInitializers=nullptr, const TPredictionParams predictionOptions=nullptr)
	Constructor. More...

void	clear (const mrpt::poses::CPose2D &initialPose)
	Clear all elements of the maps, and restore all paths to a single starting pose. More...

void	clear (const mrpt::poses::CPose3D &initialPose)

void	clear (const mrpt::maps::CSimpleMap &prevMap, const mrpt::poses::CPose3D &currentPose)
	Resets the map by loading an already-mapped map for past poses. More...

void	getEstimatedPosePDFAtTime (size_t timeStep, mrpt::poses::CPose3DPDFParticles &out_estimation) const
	Returns the estimate of the robot pose as a particles PDF for the instant of time "timeStep", from 0 to N-1. More...

void	getEstimatedPosePDF (mrpt::poses::CPose3DPDFParticles &out_estimation) const
	Returns the current estimate of the robot pose, as a particles PDF. More...

const CMultiMetricMap *	getAveragedMetricMapEstimation ()
	Returns the weighted averaged map based on the current best estimation. More...

const CMultiMetricMap *	getCurrentMostLikelyMetricMap () const
	Returns a pointer to the current most likely map (associated to the most likely particle) More...

size_t	getNumberOfObservationsInSimplemap () const
	Get the number of CSensoryFrame inserted into the internal member SFs. More...

bool	insertObservation (mrpt::obs::CSensoryFrame &sf)
	Insert an observation to the map, at each particle's pose and to each particle's metric map. More...

void	getPath (size_t i, std::deque< math::TPose3D > &out_path) const
	Return the path (in absolute coordinate poses) for the i'th particle. More...

double	getCurrentEntropyOfPaths ()
	Returns the current entropy of paths, computed as the average entropy of poses along the path, where entropy of each pose estimation is computed as the entropy of the gaussian approximation covariance. More...

double	getCurrentJointEntropy ()
	Returns the joint entropy estimation over paths and maps, acording to "Information Gain-based Exploration Using" by C. More...

void	updateSensoryFrameSequence ()
	Update the poses estimation of the member "SFs" according to the current path belief. More...

void	saveCurrentPathEstimationToTextFile (const std::string &fil)
	A logging utility: saves the current path estimation for each particle in a text file (a row per particle, each 3-column-entry is a set [x,y,phi], respectively). 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 mxArray *	writeToMatlab () const
	Introduces a pure virtual method responsible for writing to a `mxArray` Matlab object, typically a MATLAB `struct` whose contents are documented in each derived class. More...

Virtual methods that the PF_implementations assume exist.
mrpt::math::TPose3D	getLastPose (const size_t i, bool &pose_is_valid) const override
	Return the last robot pose in the i'th particle; is_valid_pose will be false if there is no such last pose. More...

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

bool	PF_SLAM_implementation_doWeHaveValidObservations (const CParticleList &particles, const mrpt::obs::CSensoryFrame *sf) const override

bool	PF_SLAM_implementation_skipRobotMovement () const override
	Do not move the particles until the map is populated. More...

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

RTTI classes and functions
mrpt::utils::CObject::Ptr	duplicateGetSmartPtr () const
	Returns a copy of the object, indepently of its class, as a smart pointer (the newly created object will exist as long as any copy of this smart pointer). More...

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

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

Static Public Member Functions
static void *	operator new (size_t size, void *ptr)

Public Attributes
ParticleData	m_poseParticles

mrpt::maps::CMultiMetricMapPDF::TPredictionParams	options

Protected Member Functions
CSerializable virtual methods
void	writeToStream (mrpt::utils::CStream &out, int *getVersion) const override
	Introduces a pure virtual method responsible for writing to a CStream. More...

void	readFromStream (mrpt::utils::CStream &in, int version) override
	Introduces a pure virtual method responsible for loading from a CStream This can not be used directly be users, instead use "stream >> object;" for reading it from a stream or "stream >> object_ptr;" if the class is unknown apriori. More...

Private Member Functions
void	rebuildAverageMap ()
	Rebuild the "expected" grid map. More...

Private Attributes
mrpt::maps::CMultiMetricMap	averageMap
	Internal buffer for the averaged map. More...

bool	averageMapIsUpdated

mrpt::maps::CSimpleMap	SFs
	The SFs and their corresponding pose estimations. More...

std::vector< uint32_t >	SF2robotPath
	A mapping between indexes in the SFs to indexes in the robot paths from particles. More...

float	newInfoIndex
	An index [0,1] measuring how much information an observation aports to the map (Typ. More...

Friends
class	mrpt::slam::CMetricMapBuilderRBPF

RTTI stuff
using	Ptr = std::shared_ptr< CMultiMetricMapPDF >

using	ConstPtr = std::shared_ptr< const CMultiMetricMapPDF >

static mrpt::utils::CLASSINIT	_init_CMultiMetricMapPDF

static const mrpt::utils::TRuntimeClassId	runtimeClassId

static constexpr const char *	className = "CMultiMetricMapPDF"

static const mrpt::utils::TRuntimeClassId *	_GetBaseClass ()

static const mrpt::utils::TRuntimeClassId &	GetRuntimeClassIdStatic ()

static mrpt::utils::CObject *	CreateObject ()

template<typename... Args>
static Ptr	Create (Args &&... args)

virtual const mrpt::utils::TRuntimeClassId *	GetRuntimeClass () const override
	Returns information about the class of an object in runtime. More...

virtual mrpt::utils::CObject *	clone () const override
	Returns a deep copy (clone) of the object, indepently of its class. 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

◆ ConstPtr

using mrpt::maps::CMultiMetricMapPDF::ConstPtr = std::shared_ptr<const CMultiMetricMapPDF >

Definition at line 80 of file CMultiMetricMapPDF.h.

◆ CParticleDataContent

using mrpt::maps::CMultiMetricMapPDF::CParticleDataContent = ParticleData::CParticleDataContent

Definition at line 74 of file CMultiMetricMapPDF.h.

◆ CParticleList

using mrpt::maps::CMultiMetricMapPDF::CParticleList = ParticleData::CParticleList

Definition at line 73 of file CMultiMetricMapPDF.h.

◆ ParticleData

using mrpt::maps::CMultiMetricMapPDF::ParticleData = CMultiMetricMapPDFParticles

Definition at line 72 of file CMultiMetricMapPDF.h.

◆ Ptr

using mrpt::maps::CMultiMetricMapPDF::Ptr = std::shared_ptr< CMultiMetricMapPDF >

A typedef for the associated smart pointer

Definition at line 80 of file CMultiMetricMapPDF.h.

Constructor & Destructor Documentation

◆ CMultiMetricMapPDF()

CMultiMetricMapPDF::CMultiMetricMapPDF	(	const bayes::CParticleFilter::TParticleFilterOptions &	opts = `bayes::CParticleFilter::TParticleFilterOptions()`,
		const mrpt::maps::TSetOfMetricMapInitializers *	mapsInitializers = `nullptr`,
		const TPredictionParams *	predictionOptions = `nullptr`
	)

Constructor.

Definition at line 46 of file CMultiMetricMapPDF.cpp.

References mrpt::utils::clear().

Referenced by prediction_and_update().

Here is the call graph for this function:

Here is the caller graph for this function:

Member Function Documentation

◆ _GetBaseClass()

static const mrpt::utils::TRuntimeClassId* mrpt::maps::CMultiMetricMapPDF::_GetBaseClass ( )

staticprotected

◆ clear() [1/3]

void CMultiMetricMapPDF::clear ( const mrpt::poses::CPose2D & initialPose )

Clear all elements of the maps, and restore all paths to a single starting pose.

Definition at line 76 of file CMultiMetricMapPDF.cpp.

References mrpt::utils::clear().

Referenced by mrpt::slam::CMetricMapBuilderRBPF::clear(), and mrpt::slam::CMetricMapBuilderRBPF::initialize().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ clear() [2/3]

void CMultiMetricMapPDF::clear ( const mrpt::poses::CPose3D & initialPose )

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

Definition at line 85 of file CMultiMetricMapPDF.cpp.

◆ clear() [3/3]

void CMultiMetricMapPDF::clear	(	const mrpt::maps::CSimpleMap &	prevMap,
		const mrpt::poses::CPose3D &	currentPose
	)

Resets the map by loading an already-mapped map for past poses.

Current robot pose should be normally set to the last keyframe in the simplemap.

Definition at line 104 of file CMultiMetricMapPDF.cpp.

References ASSERT_, mrpt::maps::CSimpleMap::clear(), mrpt::utils::clear(), mrpt::maps::CSimpleMap::get(), IS_CLASS, and mrpt::maps::CSimpleMap::size().

Here is the call graph for this function:

◆ clone()

virtual mrpt::utils::CObject* mrpt::maps::CMultiMetricMapPDF::clone ( ) const

overridevirtual

Returns a deep copy (clone) of the object, indepently of its class.

Implements mrpt::utils::CObject.

◆ Create()

template<typename... Args>

static Ptr mrpt::maps::CMultiMetricMapPDF::Create ( Args &&... args )

inlinestatic

Definition at line 80 of file CMultiMetricMapPDF.h.

◆ CreateObject()

static mrpt::utils::CObject* mrpt::maps::CMultiMetricMapPDF::CreateObject ( )

static

◆ executeOn()

void mrpt::maps::CMultiMetricMapPDF::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 1022 of file CMultiMetricMapPDF_RBPF.cpp.

References mrpt::bayes::CParticleFilter::executeOn(), mrpt::bayes::CParticleFilter::pfAuxiliaryPFOptimal, mrpt::bayes::CParticleFilter::pfAuxiliaryPFStandard, mrpt::bayes::CParticleFilter::pfOptimalProposal, mrpt::bayes::CParticleFilter::pfStandardProposal, and THROW_EXCEPTION.

Referenced by mrpt::slam::CMetricMapBuilderRBPF::processActionObservation().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getAveragedMetricMapEstimation()

const CMultiMetricMap * CMultiMetricMapPDF::getAveragedMetricMapEstimation ( )

Returns the weighted averaged map based on the current best estimation.

If you need a persistent copy of this object, please use "CSerializable::duplicate" and use the copy.

See also: Almost 100% sure you would prefer the best current map, given by getCurrentMostLikelyMetricMap()

Definition at line 310 of file CMultiMetricMapPDF.cpp.

◆ getCurrentEntropyOfPaths()

double CMultiMetricMapPDF::getCurrentEntropyOfPaths ( )

Returns the current entropy of paths, computed as the average entropy of poses along the path, where entropy of each pose estimation is computed as the entropy of the gaussian approximation covariance.

Definition at line 479 of file CMultiMetricMapPDF.cpp.

References mrpt::utils::CProbabilityDensityFunction< TDATA, STATE_LEN >::getCovarianceEntropy().

Here is the call graph for this function:

◆ getCurrentJointEntropy()

double CMultiMetricMapPDF::getCurrentJointEntropy ( )

Returns the joint entropy estimation over paths and maps, acording to "Information Gain-based Exploration Using" by C.

Stachniss, G. Grissetti and W.Burgard.

Definition at line 509 of file CMultiMetricMapPDF.cpp.

References ASSERT_, mrpt::maps::COccupancyGridMap2D::TEntropyInfo::H, and min.

Referenced by mrpt::slam::CMetricMapBuilderRBPF::getCurrentJointEntropy().

Here is the caller graph for this function:

◆ getCurrentMostLikelyMetricMap()

const CMultiMetricMap * CMultiMetricMapPDF::getCurrentMostLikelyMetricMap ( ) const

Returns a pointer to the current most likely map (associated to the most likely particle)

Definition at line 561 of file CMultiMetricMapPDF.cpp.

Referenced by mrpt::slam::CMetricMapBuilderRBPF::drawCurrentEstimationToImage(), and mrpt::slam::CMetricMapBuilderRBPF::getCurrentlyBuiltMetricMap().

Here is the caller graph for this function:

◆ getEstimatedPosePDF()

void CMultiMetricMapPDF::getEstimatedPosePDF ( mrpt::poses::CPose3DPDFParticles & out_estimation ) const

Returns the current estimate of the robot pose, as a particles PDF.

See also: getEstimatedPosePDFAtTime

Definition at line 172 of file CMultiMetricMapPDF.cpp.

References ASSERT_.

Referenced by mrpt::slam::CMetricMapBuilderRBPF::getCurrentPoseEstimation(), and mrpt::slam::CMetricMapBuilderRBPF::processActionObservation().

Here is the caller graph for this function:

◆ getEstimatedPosePDFAtTime()

void CMultiMetricMapPDF::getEstimatedPosePDFAtTime	(	size_t	timeStep,
		mrpt::poses::CPose3DPDFParticles &	out_estimation
	)		const

Returns the estimate of the robot pose as a particles PDF for the instant of time "timeStep", from 0 to N-1.

See also: getEstimatedPosePDF

Definition at line 183 of file CMultiMetricMapPDF.cpp.

References mrpt::bayes::CParticleFilterData< T >::clearParticles(), and mrpt::bayes::CParticleFilterData< T >::m_particles.

Here is the call graph for this function:

◆ getLastPose()

TPose3D CMultiMetricMapPDF::getLastPose	(	const size_t	i,
		bool &	is_valid_pose
	)		const

overridevirtual

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

Exceptions

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

Implements mrpt::slam::PF_implementation< CRBPFParticleData, CMultiMetricMapPDFParticles >.

Definition at line 293 of file CMultiMetricMapPDF.cpp.

References THROW_EXCEPTION.

◆ getNumberOfObservationsInSimplemap()

size_t mrpt::maps::CMultiMetricMapPDF::getNumberOfObservationsInSimplemap ( ) const

inline

Get the number of CSensoryFrame inserted into the internal member SFs.

Definition at line 211 of file CMultiMetricMapPDF.h.

References SFs, and mrpt::maps::CSimpleMap::size().

Here is the call graph for this function:

◆ getPath()

void CMultiMetricMapPDF::getPath	(	size_t	i,
		std::deque< math::TPose3D > &	out_path
	)		const

Return the path (in absolute coordinate poses) for the i'th particle.

Exceptions

On	index out of bounds

Definition at line 469 of file CMultiMetricMapPDF.cpp.

References THROW_EXCEPTION.

Referenced by mrpt::slam::CMetricMapBuilderRBPF::drawCurrentEstimationToImage(), and mrpt::slam::CMetricMapBuilderRBPF::getCurrentMostLikelyPath().

Here is the caller graph for this function:

◆ GetRuntimeClass()

virtual const mrpt::utils::TRuntimeClassId* mrpt::maps::CMultiMetricMapPDF::GetRuntimeClass ( ) const

overridevirtual

Returns information about the class of an object in runtime.

Reimplemented from mrpt::utils::CSerializable.

◆ GetRuntimeClassIdStatic()

static const mrpt::utils::TRuntimeClassId& mrpt::maps::CMultiMetricMapPDF::GetRuntimeClassIdStatic ( )

static

◆ insertObservation()

bool CMultiMetricMapPDF::insertObservation ( mrpt::obs::CSensoryFrame & sf )

Insert an observation to the map, at each particle's pose and to each particle's metric map.

Parameters

sf	The SF to be inserted

Returns: true if any may was updated, false otherwise

Definition at line 436 of file CMultiMetricMapPDF.cpp.

References mrpt::obs::CSensoryFrame::insertObservationsInto().

Referenced by mrpt::slam::CMetricMapBuilderRBPF::processActionObservation().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ operator delete() [1/3]

void mrpt::maps::CMultiMetricMapPDF::operator delete ( void * ptr )

inlinenoexcept

Definition at line 80 of file CMultiMetricMapPDF.h.

◆ operator delete() [2/3]

void mrpt::maps::CMultiMetricMapPDF::operator delete	(	void *	memory,
		void *	ptr
	)

inlinenoexcept

Definition at line 80 of file CMultiMetricMapPDF.h.

◆ operator delete() [3/3]

void mrpt::maps::CMultiMetricMapPDF::operator delete	(	void *	ptr,
		const std::nothrow_t &
	)

inlinenoexcept

Definition at line 80 of file CMultiMetricMapPDF.h.

◆ operator delete[]()

void mrpt::maps::CMultiMetricMapPDF::operator delete[] ( void * ptr )

inlinenoexcept

Definition at line 80 of file CMultiMetricMapPDF.h.

◆ operator new() [1/3]

static void* mrpt::maps::CMultiMetricMapPDF::operator new	(	size_t	size,
		void *	ptr
	)

inlinestatic

Definition at line 80 of file CMultiMetricMapPDF.h.

◆ operator new() [2/3]

void* mrpt::maps::CMultiMetricMapPDF::operator new	(	size_t	size,
		const std::nothrow_t &
	)

inlinenoexcept

Definition at line 80 of file CMultiMetricMapPDF.h.

◆ operator new() [3/3]

void* mrpt::maps::CMultiMetricMapPDF::operator new ( size_t size )

inline

Definition at line 80 of file CMultiMetricMapPDF.h.

◆ operator new[]()

void* mrpt::maps::CMultiMetricMapPDF::operator new[] ( size_t size )

inline

Definition at line 80 of file CMultiMetricMapPDF.h.

◆ PF_SLAM_computeObservationLikelihoodForParticle()

double mrpt::maps::CMultiMetricMapPDF::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< CRBPFParticleData, CMultiMetricMapPDFParticles >.

Definition at line 1007 of file CMultiMetricMapPDF_RBPF.cpp.

References mrpt::obs::CSensoryFrame::begin(), mrpt::maps::CMetricMap::computeObservationLikelihood(), mrpt::obs::CSensoryFrame::end(), and MRPT_UNUSED_PARAM.

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< CRBPFParticleData , CMultiMetricMapPDFParticles , typename CMultiMetricMapPDFParticles ::CParticleList >::PF_SLAM_implementation_custom_update_particle_with_new_pose	(	CRBPFParticleData *	particleData,
		const mrpt::math::TPose3D &	newPose
	)		const

pure virtualinherited

◆ PF_SLAM_implementation_custom_update_particle_with_new_pose() [2/2]

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

override

Definition at line 979 of file CMultiMetricMapPDF_RBPF.cpp.

References mrpt::maps::CRBPFParticleData::robotPath.

◆ PF_SLAM_implementation_doWeHaveValidObservations() [1/2]

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

inlinevirtualinherited

Definition at line 179 of file PF_implementations_data.h.

◆ PF_SLAM_implementation_doWeHaveValidObservations() [2/2]

bool mrpt::maps::CMultiMetricMapPDF::PF_SLAM_implementation_doWeHaveValidObservations	(	const CParticleList &	particles,
		const mrpt::obs::CSensoryFrame *	sf
	)		const

override

Definition at line 986 of file CMultiMetricMapPDF_RBPF.cpp.

References ASSERT_.

◆ PF_SLAM_implementation_replaceByNewParticleSet()

virtual void mrpt::slam::PF_implementation< CRBPFParticleData , CMultiMetricMapPDFParticles , typename CMultiMetricMapPDFParticles ::CParticleList >::PF_SLAM_implementation_replaceByNewParticleSet	(	typename CMultiMetricMapPDFParticles ::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.

◆ PF_SLAM_implementation_skipRobotMovement()

bool mrpt::maps::CMultiMetricMapPDF::PF_SLAM_implementation_skipRobotMovement ( ) const

overridevirtual

Do not move the particles until the map is populated.

Reimplemented from mrpt::slam::PF_implementation< CRBPFParticleData, CMultiMetricMapPDFParticles >.

Definition at line 998 of file CMultiMetricMapPDF_RBPF.cpp.

◆ PF_SLAM_particlesEvaluator_AuxPFStandard()

static double mrpt::slam::PF_implementation< CRBPFParticleData , CMultiMetricMapPDFParticles , typename CMultiMetricMapPDFParticles ::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 PF_ALGORITHM >

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

inline

Definition at line 85 of file CMultiMetricMapPDF.h.

References CMultiMetricMapPDF(), mrpt::maps::CMultiMetricMapPDF::TPredictionParams::KLD_params, MRPT_END, MRPT_START, and options.

Here is the call graph for this function:

◆ readFromStream()

void CMultiMetricMapPDF::readFromStream	(	mrpt::utils::CStream &	in,
		int	version
	)

overrideprotectedvirtual

Introduces a pure virtual method responsible for loading from a CStream This can not be used directly be users, instead use "stream >> object;" for reading it from a stream or "stream >> object_ptr;" if the class is unknown apriori.

Parameters

in	The input binary stream where the object data must read from.
version	The version of the object stored in the stream: use this version number in your code to know how to read the incoming data.

Exceptions

std::exception On any error, see CStream::ReadBuffer

See also: CStream

Implements mrpt::utils::CSerializable.

Definition at line 252 of file CMultiMetricMapPDF.cpp.

References MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION.

◆ rebuildAverageMap()

void CMultiMetricMapPDF::rebuildAverageMap ( )

private

Rebuild the "expected" grid map.

Used internally, do not call

Definition at line 319 of file CMultiMetricMapPDF.cpp.

References ASSERT_, min, MRPT_END, and MRPT_START.

◆ saveCurrentPathEstimationToTextFile()

void CMultiMetricMapPDF::saveCurrentPathEstimationToTextFile ( const std::string & fil )

A logging utility: saves the current path estimation for each particle in a text file (a row per particle, each 3-column-entry is a set [x,y,phi], respectively).

Definition at line 607 of file CMultiMetricMapPDF.cpp.

References mrpt::system::os::fclose(), mrpt::system::os::fopen(), and mrpt::system::os::fprintf().

Referenced by mrpt::slam::CMetricMapBuilderRBPF::saveCurrentPathEstimationToTextFile().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ updateSensoryFrameSequence()

void CMultiMetricMapPDF::updateSensoryFrameSequence ( )

Update the poses estimation of the member "SFs" according to the current path belief.

Definition at line 582 of file CMultiMetricMapPDF.cpp.

References MRPT_END, and MRPT_START.

Referenced by mrpt::slam::CMetricMapBuilderRBPF::getCurrentlyBuiltMap().

Here is the caller graph for this function:

◆ writeToMatlab()

virtual mxArray* mrpt::utils::CSerializable::writeToMatlab ( ) const

inlinevirtualinherited

Introduces a pure virtual method responsible for writing to a mxArray Matlab object, typically a MATLAB struct whose contents are documented in each derived class.

Returns: A new mxArray (caller is responsible of memory freeing) or nullptr is class does not support conversion to MATLAB.

Definition at line 89 of file CSerializable.h.

◆ writeToStream()

void CMultiMetricMapPDF::writeToStream	(	mrpt::utils::CStream &	out,
		int *	getVersion
	)		const

overrideprotectedvirtual

Introduces a pure virtual method responsible for writing to a CStream.

This can not be used directly be users, instead use "stream << object;" for writing it to a stream.

Parameters

out	The output binary stream where object must be dumped.
getVersion	If nullptr, the object must be dumped. If not, only the version of the object dump must be returned in this pointer. This enables the versioning of objects dumping and backward compatibility with previously stored data.

Exceptions

std::exception On any error, see CStream::WriteBuffer

See also: CStream

Implements mrpt::utils::CSerializable.

Definition at line 226 of file CMultiMetricMapPDF.cpp.

Friends And Related Function Documentation

◆ mrpt::slam::CMetricMapBuilderRBPF

friend class mrpt::slam::CMetricMapBuilderRBPF

friend

Definition at line 78 of file CMultiMetricMapPDF.h.

Member Data Documentation

◆ _init_CMultiMetricMapPDF

mrpt::utils::CLASSINIT mrpt::maps::CMultiMetricMapPDF::_init_CMultiMetricMapPDF

staticprotected

Definition at line 80 of file CMultiMetricMapPDF.h.

◆ averageMap

mrpt::maps::CMultiMetricMap mrpt::maps::CMultiMetricMapPDF::averageMap

private

Internal buffer for the averaged map.

Definition at line 102 of file CMultiMetricMapPDF.h.

◆ averageMapIsUpdated

bool mrpt::maps::CMultiMetricMapPDF::averageMapIsUpdated

private

Definition at line 103 of file CMultiMetricMapPDF.h.

◆ className

constexpr const char* mrpt::maps::CMultiMetricMapPDF::className = "CMultiMetricMapPDF"

static

Definition at line 80 of file CMultiMetricMapPDF.h.

◆ m_accumRobotMovement2D

mrpt::obs::CActionRobotMovement2D mrpt::slam::PF_implementation< CRBPFParticleData , CMultiMetricMapPDFParticles , typename CMultiMetricMapPDFParticles ::CParticleList >::m_accumRobotMovement2D

inherited

Definition at line 48 of file PF_implementations_data.h.

◆ m_accumRobotMovement2DIsValid

bool mrpt::slam::PF_implementation< CRBPFParticleData , CMultiMetricMapPDFParticles , typename CMultiMetricMapPDFParticles ::CParticleList >::m_accumRobotMovement2DIsValid

inherited

Definition at line 49 of file PF_implementations_data.h.

◆ m_accumRobotMovement3D

mrpt::poses::CPose3DPDFGaussian mrpt::slam::PF_implementation< CRBPFParticleData , CMultiMetricMapPDFParticles , typename CMultiMetricMapPDFParticles ::CParticleList >::m_accumRobotMovement3D

inherited

Definition at line 50 of file PF_implementations_data.h.

◆ m_accumRobotMovement3DIsValid

bool mrpt::slam::PF_implementation< CRBPFParticleData , CMultiMetricMapPDFParticles , typename CMultiMetricMapPDFParticles ::CParticleList >::m_accumRobotMovement3DIsValid

inherited

Definition at line 51 of file PF_implementations_data.h.

◆ m_movementDrawer

mrpt::poses::CPoseRandomSampler mrpt::slam::PF_implementation< CRBPFParticleData , CMultiMetricMapPDFParticles , typename CMultiMetricMapPDFParticles ::CParticleList >::m_movementDrawer

inherited

Used in al PF implementations.

See also: PF_SLAM_implementation_gatherActionsCheckBothActObs

Definition at line 55 of file PF_implementations_data.h.

◆ m_pfAuxiliaryPFOptimal_estimatedProb

mrpt::math::CVectorDouble mrpt::slam::PF_implementation< CRBPFParticleData , CMultiMetricMapPDFParticles , typename CMultiMetricMapPDFParticles ::CParticleList >::m_pfAuxiliaryPFOptimal_estimatedProb

mutableinherited

Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.

Definition at line 57 of file PF_implementations_data.h.

◆ m_pfAuxiliaryPFOptimal_maxLikDrawnMovement

std::vector<mrpt::math::TPose3D> mrpt::slam::PF_implementation< CRBPFParticleData , CMultiMetricMapPDFParticles , typename CMultiMetricMapPDFParticles ::CParticleList >::m_pfAuxiliaryPFOptimal_maxLikDrawnMovement

mutableinherited

Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.

Definition at line 64 of file PF_implementations_data.h.

◆ m_pfAuxiliaryPFOptimal_maxLikelihood

mrpt::math::CVectorDouble mrpt::slam::PF_implementation< CRBPFParticleData , CMultiMetricMapPDFParticles , typename CMultiMetricMapPDFParticles ::CParticleList >::m_pfAuxiliaryPFOptimal_maxLikelihood

mutableinherited

Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.

Definition at line 61 of file PF_implementations_data.h.

◆ m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed

std::vector<bool> mrpt::slam::PF_implementation< CRBPFParticleData , CMultiMetricMapPDFParticles , typename CMultiMetricMapPDFParticles ::CParticleList >::m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed

inherited

Definition at line 65 of file PF_implementations_data.h.

◆ m_pfAuxiliaryPFStandard_estimatedProb

mrpt::math::CVectorDouble mrpt::slam::PF_implementation< CRBPFParticleData , CMultiMetricMapPDFParticles , typename CMultiMetricMapPDFParticles ::CParticleList >::m_pfAuxiliaryPFStandard_estimatedProb

mutableinherited

Auxiliary variable used in the "pfAuxiliaryPFStandard" algorithm.

Definition at line 59 of file PF_implementations_data.h.

◆ m_poseParticles

ParticleData mrpt::maps::CMultiMetricMapPDF::m_poseParticles

Definition at line 75 of file CMultiMetricMapPDF.h.

Referenced by mrpt::slam::CMetricMapBuilderRBPF::drawCurrentEstimationToImage(), mrpt::slam::CMetricMapBuilderRBPF::getCurrentMostLikelyPath(), and mrpt::slam::CMetricMapBuilderRBPF::processActionObservation().

◆ newInfoIndex

float mrpt::maps::CMultiMetricMapPDF::newInfoIndex

private

An index [0,1] measuring how much information an observation aports to the map (Typ.

threshold=0.07)

Definition at line 253 of file CMultiMetricMapPDF.h.

◆ options

mrpt::maps::CMultiMetricMapPDF::TPredictionParams mrpt::maps::CMultiMetricMapPDF::options

Referenced by prediction_and_update().

◆ runtimeClassId

const mrpt::utils::TRuntimeClassId mrpt::maps::CMultiMetricMapPDF::runtimeClassId

staticprotected

Definition at line 80 of file CMultiMetricMapPDF.h.

◆ SF2robotPath

std::vector<uint32_t> mrpt::maps::CMultiMetricMapPDF::SF2robotPath

private

A mapping between indexes in the SFs to indexes in the robot paths from particles.

Definition at line 109 of file CMultiMetricMapPDF.h.

◆ SFs

mrpt::maps::CSimpleMap mrpt::maps::CMultiMetricMapPDF::SFs

private

The SFs and their corresponding pose estimations.

Definition at line 106 of file CMultiMetricMapPDF.h.

Referenced by mrpt::slam::CMetricMapBuilderRBPF::getCurrentlyBuiltMap(), mrpt::slam::CMetricMapBuilderRBPF::getCurrentlyBuiltMapSize(), getNumberOfObservationsInSimplemap(), and mrpt::slam::CMetricMapBuilderRBPF::processActionObservation().

Detailed Description

Classes

Public Types

Public Member Functions

Static Public Member Functions

Public Attributes

Protected Member Functions

Private Member Functions

Private Attributes

Friends

RTTI stuff

Data members and methods used by generic PF implementations

Member Typedef Documentation

◆ ConstPtr

◆ CParticleDataContent

◆ CParticleList

◆ ParticleData

◆ Ptr

Constructor & Destructor Documentation

◆ CMultiMetricMapPDF()

Member Function Documentation

◆ _GetBaseClass()

◆ clear() [1/3]

◆ clear() [2/3]

◆ clear() [3/3]

◆ clone()

◆ Create()

◆ CreateObject()

◆ executeOn()

◆ getAveragedMetricMapEstimation()

◆ getCurrentEntropyOfPaths()

◆ getCurrentJointEntropy()

◆ getCurrentMostLikelyMetricMap()

◆ getEstimatedPosePDF()

◆ getEstimatedPosePDFAtTime()

◆ getLastPose()

◆ getNumberOfObservationsInSimplemap()

◆ getPath()

◆ GetRuntimeClass()

◆ GetRuntimeClassIdStatic()

◆ insertObservation()

◆ operator delete() [1/3]

◆ operator delete() [2/3]

◆ operator delete() [3/3]

◆ operator delete[]()

◆ operator new() [1/3]

◆ operator new() [2/3]

◆ operator new() [3/3]

◆ operator new[]()

◆ PF_SLAM_computeObservationLikelihoodForParticle()

◆ PF_SLAM_implementation_custom_update_particle_with_new_pose() [1/2]

◆ PF_SLAM_implementation_custom_update_particle_with_new_pose() [2/2]

◆ PF_SLAM_implementation_doWeHaveValidObservations() [1/2]

◆ PF_SLAM_implementation_doWeHaveValidObservations() [2/2]

◆ PF_SLAM_implementation_replaceByNewParticleSet()

◆ PF_SLAM_implementation_skipRobotMovement()

◆ PF_SLAM_particlesEvaluator_AuxPFStandard()

◆ prediction_and_update()

◆ readFromStream()

◆ rebuildAverageMap()

◆ saveCurrentPathEstimationToTextFile()

◆ updateSensoryFrameSequence()

◆ writeToMatlab()

◆ writeToStream()

Friends And Related Function Documentation

◆ mrpt::slam::CMetricMapBuilderRBPF

Member Data Documentation

◆ _init_CMultiMetricMapPDF

◆ averageMap

◆ averageMapIsUpdated

◆ className

◆ m_accumRobotMovement2D

◆ m_accumRobotMovement2DIsValid

◆ m_accumRobotMovement3D

◆ m_accumRobotMovement3DIsValid

◆ m_movementDrawer

◆ m_pfAuxiliaryPFOptimal_estimatedProb

◆ m_pfAuxiliaryPFOptimal_maxLikDrawnMovement

◆ m_pfAuxiliaryPFOptimal_maxLikelihood

◆ m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed