Detailed Description

This class is used in HMT-SLAM to represent each of the Local Metric Hypotheses (LMHs).

It has a set of particles representing the robot path in nearby poses.

See also: CHMTSLAM, CLSLAM_RBPF_2DLASER

Definition at line 75 of file CLocalMetricHypothesis.h.

#include <mrpt/hmtslam/CLocalMetricHypothesis.h>

Inheritance diagram for mrpt::hmtslam::CLocalMetricHypothesis:

Classes
struct	TRobotPosesPartitioning
	Used by AA thread. More...

Public Types
using	CParticleData = CLSLAMParticleDataParticles::CParticleData

using	CParticleList = mrpt::bayes::CParticleFilterData< CLSLAMParticleData >::CParticleList

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

	CLocalMetricHypothesis (CHMTSLAM *parent=nullptr)
	Constructor (Default param only used from STL classes) More...

	~CLocalMetricHypothesis ()
	Destructor. More...

	MRPT_TODO ("Separate the serializable class from this code, so we don't have to " "worry about copying locks") struct ThreadLocks

void	getAs3DScene (mrpt::opengl::CSetOfObjects::Ptr &objs) const
	Returns a 3D representation of the the current robot pose, all the poses in the auxiliary graph, and each of the areas they belong to. More...

void	getMeans (TMapPoseID2Pose3D &outList) const
	Returns the mean of each robot pose in this LMH, as computed from the set of particles. More...

void	getPathParticles (std::map< TPoseID, mrpt::poses::CPose3DPDFParticles > &outList) const
	Returns the mean and covariance of each robot pose in this LMH, as computed from the set of particles. More...

void	getPoseParticles (const TPoseID &poseID, mrpt::poses::CPose3DPDFParticles &outPDF) const
	Returns the mean and covariance of each robot pose in this LMH, as computed from the set of particles. More...

void	getRelativePose (const TPoseID &reference, const TPoseID &pose, mrpt::poses::CPose3DPDFParticles &outPDF) const
	Returns the pose PDF of some pose relative to some other pose ID (both must be part of the the LMH). More...

void	dumpAsText (utils::CStringList &st) const
	Describes the LMH in text. More...

void	changeCoordinateOrigin (const TPoseID &newOrigin)
	Change all coordinates to set a given robot pose as the new coordinate origin, and rebuild metric maps and change coords in the partitioning subsystem as well. More...

void	rebuildMetricMaps ()
	Rebuild the metric maps of all particles from the observations and their estimated poses. More...

void	clearRobotPoses ()
	Rebuild the auxiliary metric maps in "m_robotPosesGraph" from the observations "m_SFs" and their estimated poses. More...

const mrpt::poses::CPose3D *	getCurrentPose (const size_t &particleIdx) const
	Returns the i'th particle hypothesis for the current robot pose. More...

mrpt::poses::CPose3D *	getCurrentPose (const size_t &particleIdx)
	Returns the i'th particle hypothesis for the current robot pose. More...

void	removeAreaFromLMH (const CHMHMapNode::TNodeID areaID)
	Removes a given area from the LMH: More...

void	updateAreaFromLMH (const CHMHMapNode::TNodeID areaID, bool eraseSFsFromLMH=false)
	The corresponding node in the HMT map is updated with the robot poses & SFs in the LMH: the poses are referenced to the area's reference poseID, such as that reference is at the origin. More...

template<>
void	prediction_and_update (const mrpt::obs::CActionCollection actions, const mrpt::obs::CSensoryFrame sf, const bayes::CParticleFilter::TParticleFilterOptions &PF_options)
	The PF algorithm implementation for "optimal sampling for non-parametric observation models". More...

template<>
void	prediction_and_update (const mrpt::obs::CActionCollection actions, const mrpt::obs::CSensoryFrame sf, const bayes::CParticleFilter::TParticleFilterOptions &PF_options)
	The PF algorithm implementation for "optimal sampling" approximated with scan matching (Stachniss method) More...

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 for Particle Filter implementation (just a wrapper
interface, actually implemented in CHMTSLAM::m_LSLAM_method)
template<class PF_ALGORITHM >
void	prediction_and_update (const mrpt::obs::CActionCollection action, const mrpt::obs::CSensoryFrame observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options)
	The PF algorithm implementation. More...

double	particlesEvaluator_AuxPFOptimal (const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options, size_t index, const mrpt::obs::CSensoryFrame *observation)
	Auxiliary function used in "prediction_and_update_pfAuxiliaryPFOptimal". More...

double	auxiliarComputeObservationLikelihood (const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options, size_t particleIndexForMap, const mrpt::obs::CSensoryFrame observation, const mrpt::poses::CPose2D x)
	Auxiliary function that evaluates the likelihood of an observation, given a robot pose, and according to the options in "CPosePDFParticles::options". 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)

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

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

Public Attributes
CLSLAMParticleDataParticles	m_poseParticles

	threadLocks

THypothesisID	m_ID
	The unique ID of the hypothesis (Used for accessing mrpt::slam::CHierarchicalMHMap). More...

mrpt::utils::safe_ptr< CHMTSLAM >	m_parent
	For quick access to our parent object. More...

TPoseID	m_currentRobotPose
	The current robot pose (its global unique ID) for this hypothesis. More...

TNodeIDSet	m_neighbors
	The list of all areas sourronding the current one (this includes the current area itself). More...

std::map< TPoseID, CHMHMapNode::TNodeID >	m_nodeIDmemberships
	The hybrid map node membership for each robot pose. More...

std::map< TPoseID, mrpt::obs::CSensoryFrame >	m_SFs
	The SF gathered at each robot pose. More...

TPoseIDList	m_posesPendingAddPartitioner
	The list of poseIDs waiting to be added to the graph partitioner, what happens in the LSLAM thread main loop. More...

TNodeIDList	m_areasPendingTBI
	The list of area IDs waiting to be processed by the TBI (topological bayesian inference) engines to search for potential loop-closures. More...

double	m_log_w
	Log-weight of this hypothesis. More...

std::vector< std::map< TPoseID, double > >	m_log_w_metric_history
	The historic log-weights of the metric observations inserted in this LMH, for each particle. More...

mrpt::obs::CActionRobotMovement2D	m_accumRobotMovement
	Used in CLSLAM_RBPF_2DLASER. More...

bool	m_accumRobotMovementIsValid
	Used in CLSLAM_RBPF_2DLASER. More...

struct mrpt::hmtslam::CLocalMetricHypothesis::TRobotPosesPartitioning	m_robotPosesGraph

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

Protected Attributes
mrpt::math::CVectorDouble	m_pfAuxiliaryPFOptimal_estimatedProb
	Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. More...

std::vector< double >	m_maxLikelihood
	Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. More...

unsigned int	m_movementDrawsIdx
	Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. More...

mrpt::poses::StdVector_CPose2D	m_movementDraws
	Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. More...

mrpt::poses::StdVector_CPose2D	m_movementDrawMaximumLikelihood
	Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. More...

Friends
class	CLSLAM_RBPF_2DLASER

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

using	ConstPtr = std::shared_ptr< const CLocalMetricHypothesis >

static mrpt::utils::CLASSINIT	_init_CLocalMetricHypothesis

static const mrpt::utils::TRuntimeClassId	runtimeClassId

static constexpr const char *	className = "CLocalMetricHypothesis"

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

Member Typedef Documentation

◆ ConstPtr

using mrpt::hmtslam::CLocalMetricHypothesis::ConstPtr = std::shared_ptr<const CLocalMetricHypothesis >

Definition at line 79 of file CLocalMetricHypothesis.h.

◆ CParticleData

using mrpt::hmtslam::CLocalMetricHypothesis::CParticleData = CLSLAMParticleDataParticles::CParticleData

Definition at line 82 of file CLocalMetricHypothesis.h.

◆ CParticleList

using mrpt::hmtslam::CLocalMetricHypothesis::CParticleList = mrpt::bayes::CParticleFilterData<CLSLAMParticleData>::CParticleList

Definition at line 84 of file CLocalMetricHypothesis.h.

◆ Ptr

using mrpt::hmtslam::CLocalMetricHypothesis::Ptr = std::shared_ptr< CLocalMetricHypothesis >

A typedef for the associated smart pointer

Definition at line 79 of file CLocalMetricHypothesis.h.

Constructor & Destructor Documentation

◆ CLocalMetricHypothesis()

CLocalMetricHypothesis::CLocalMetricHypothesis ( CHMTSLAM * parent = nullptr )

Constructor (Default param only used from STL classes)

Definition at line 42 of file CLocalMetricHypothesis.cpp.

◆ ~CLocalMetricHypothesis()

CLocalMetricHypothesis::~CLocalMetricHypothesis ( )

Destructor.

Definition at line 53 of file CLocalMetricHypothesis.cpp.

Member Function Documentation

◆ _GetBaseClass()

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

staticprotected

◆ auxiliarComputeObservationLikelihood()

double mrpt::hmtslam::CLocalMetricHypothesis::auxiliarComputeObservationLikelihood	(	const mrpt::bayes::CParticleFilter::TParticleFilterOptions &	PF_options,
		size_t	particleIndexForMap,
		const mrpt::obs::CSensoryFrame *	observation,
		const mrpt::poses::CPose2D *	x
	)

Auxiliary function that evaluates the likelihood of an observation, given a robot pose, and according to the options in "CPosePDFParticles::options".

Definition at line 718 of file CHMTSLAM_LSLAM_RBPF_2DLASER.cpp.

References mrpt::maps::CMetricMap::computeObservationsLikelihood(), mrpt::bayes::CParticleFilterData< T >::m_particles, m_poseParticles, and MRPT_UNUSED_PARAM.

Referenced by particlesEvaluator_AuxPFOptimal().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ changeCoordinateOrigin()

void CLocalMetricHypothesis::changeCoordinateOrigin ( const TPoseID & newOrigin )

Change all coordinates to set a given robot pose as the new coordinate origin, and rebuild metric maps and change coords in the partitioning subsystem as well.

Definition at line 557 of file CLocalMetricHypothesis.cpp.

References ASSERT_, CLASS_ID, and mrpt::maps::CSimpleMap::get().

Here is the call graph for this function:

◆ clearRobotPoses()

void CLocalMetricHypothesis::clearRobotPoses ( )

Rebuild the auxiliary metric maps in "m_robotPosesGraph" from the observations "m_SFs" and their estimated poses.

Sets the number of particles to the initial number according to the PF options, and initialize them with no robot poses & empty metric maps.

Definition at line 474 of file CLocalMetricHypothesis.cpp.

Referenced by mrpt::hmtslam::CHMTSLAM::initializeEmptyMap().

Here is the caller graph for this function:

◆ clone()

virtual mrpt::utils::CObject* mrpt::hmtslam::CLocalMetricHypothesis::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::hmtslam::CLocalMetricHypothesis::Create ( Args &&... args )

inlinestatic

Definition at line 79 of file CLocalMetricHypothesis.h.

◆ CreateObject()

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

static

◆ dumpAsText()

void CLocalMetricHypothesis::dumpAsText ( utils::CStringList & st ) const

Describes the LMH in text.

Definition at line 900 of file CLocalMetricHypothesis.cpp.

References mrpt::utils::CStringList::clear(), mrpt::format(), and RAD2DEG.

Here is the call graph for this function:

◆ executeOn()

void mrpt::hmtslam::CLocalMetricHypothesis::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 58 of file CHMTSLAM_LSLAM_RBPF_2DLASER.cpp.

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

Referenced by mrpt::hmtslam::CLSLAM_RBPF_2DLASER::processOneLMH().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getAs3DScene()

void CLocalMetricHypothesis::getAs3DScene ( mrpt::opengl::CSetOfObjects::Ptr & objs ) const

Returns a 3D representation of the the current robot pose, all the poses in the auxiliary graph, and each of the areas they belong to.

The metric maps are not included here for convenience, call m_metricMaps.getAs3DScene(). The previous contents of "objs" will be discarded

Definition at line 61 of file CLocalMetricHypothesis.cpp.

References ASSERT_, mrpt::poses::CPose3DPDFGaussian::copyFrom(), mrpt::opengl::stock_objects::CornerXYZ(), mrpt::poses::CPose3DPDFGaussian::cov, mrpt::opengl::CArrow::Create(), mrpt::format(), mrpt::poses::CPose3DPDFGaussian::mean, NODE_ANNOTATION_REF_POSEID, mrpt::poses::CPose3D::pitch(), RAD2DEG, mrpt::opengl::stock_objects::RobotPioneer(), mrpt::poses::CPose3D::roll(), mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::x(), mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::y(), and mrpt::poses::CPose3D::yaw().

Referenced by mrpt::hmtslam::CHMTSLAM::generateLogFiles().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getCurrentPose() [1/2]

const CPose3D * CLocalMetricHypothesis::getCurrentPose ( const size_t & particleIdx ) const

Returns the i'th particle hypothesis for the current robot pose.

Definition at line 495 of file CLocalMetricHypothesis.cpp.

References ASSERT_, and THROW_EXCEPTION.

Referenced by particlesEvaluator_AuxPFOptimal().

Here is the caller graph for this function:

◆ getCurrentPose() [2/2]

CPose3D * CLocalMetricHypothesis::getCurrentPose ( const size_t & particleIdx )

Returns the i'th particle hypothesis for the current robot pose.

Definition at line 510 of file CLocalMetricHypothesis.cpp.

References ASSERT_, and THROW_EXCEPTION.

◆ getMeans()

void CLocalMetricHypothesis::getMeans ( TMapPoseID2Pose3D & outList ) const

Returns the mean of each robot pose in this LMH, as computed from the set of particles.

See also: getPathParticles, getRelativePose

Definition at line 393 of file CLocalMetricHypothesis.cpp.

References MRPT_END, and MRPT_START.

Referenced by mrpt::hmtslam::CLSLAM_RBPF_2DLASER::processOneLMH().

Here is the caller graph for this function:

◆ getPathParticles()

void CLocalMetricHypothesis::getPathParticles ( std::map< TPoseID, mrpt::poses::CPose3DPDFParticles > & outList ) const

Returns the mean and covariance of each robot pose in this LMH, as computed from the set of particles.

See also: getMeans, getPoseParticles

Definition at line 414 of file CLocalMetricHypothesis.cpp.

References MRPT_END, and MRPT_START.

◆ getPoseParticles()

void CLocalMetricHypothesis::getPoseParticles	(	const TPoseID &	poseID,
		mrpt::poses::CPose3DPDFParticles &	outPDF
	)		const

Returns the mean and covariance of each robot pose in this LMH, as computed from the set of particles.

See also: getMeans, getPathParticles

Definition at line 448 of file CLocalMetricHypothesis.cpp.

References ASSERT_, mrpt::bayes::CParticleFilterData< T >::m_particles, MRPT_END, MRPT_START, mrpt::poses::CPose3DPDFParticles::resetDeterministic(), and mrpt::poses::CPose3D::size().

Referenced by mrpt::hmtslam::CHMTSLAM::areaAbstraction(), and mrpt::hmtslam::CHMTSLAM::perform_TLC().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getRelativePose()

void CLocalMetricHypothesis::getRelativePose	(	const TPoseID &	reference,
		const TPoseID &	pose,
		mrpt::poses::CPose3DPDFParticles &	outPDF
	)		const

Returns the pose PDF of some pose relative to some other pose ID (both must be part of the the LMH).

See also: getMeans, getPoseParticles

Definition at line 524 of file CLocalMetricHypothesis.cpp.

References ASSERT_, mrpt::bayes::CParticleFilterData< T >::m_particles, MRPT_END, MRPT_START, mrpt::poses::CPose3DPDFParticles::resetDeterministic(), and mrpt::poses::CPose3D::size().

Here is the call graph for this function:

◆ GetRuntimeClass()

virtual const mrpt::utils::TRuntimeClassId* mrpt::hmtslam::CLocalMetricHypothesis::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::hmtslam::CLocalMetricHypothesis::GetRuntimeClassIdStatic ( )

static

◆ MRPT_TODO()

mrpt::hmtslam::CLocalMetricHypothesis::MRPT_TODO	(	"Separate the serializable class from this	code,
		so we don 't have to " "worry about copying locks"
	)

inline

Definition at line 94 of file CLocalMetricHypothesis.h.

◆ operator delete() [1/3]

void mrpt::hmtslam::CLocalMetricHypothesis::operator delete ( void * ptr )

inlinenoexcept

Definition at line 79 of file CLocalMetricHypothesis.h.

◆ operator delete() [2/3]

void mrpt::hmtslam::CLocalMetricHypothesis::operator delete	(	void *	memory,
		void *	ptr
	)

inlinenoexcept

Definition at line 79 of file CLocalMetricHypothesis.h.

◆ operator delete() [3/3]

void mrpt::hmtslam::CLocalMetricHypothesis::operator delete	(	void *	ptr,
		const std::nothrow_t &
	)

inlinenoexcept

Definition at line 79 of file CLocalMetricHypothesis.h.

◆ operator delete[]()

void mrpt::hmtslam::CLocalMetricHypothesis::operator delete[] ( void * ptr )

inlinenoexcept

Definition at line 79 of file CLocalMetricHypothesis.h.

◆ operator new() [1/3]

static void* mrpt::hmtslam::CLocalMetricHypothesis::operator new	(	size_t	size,
		void *	ptr
	)

inlinestatic

Definition at line 79 of file CLocalMetricHypothesis.h.

◆ operator new() [2/3]

void* mrpt::hmtslam::CLocalMetricHypothesis::operator new	(	size_t	size,
		const std::nothrow_t &
	)

inlinenoexcept

Definition at line 79 of file CLocalMetricHypothesis.h.

◆ operator new() [3/3]

void* mrpt::hmtslam::CLocalMetricHypothesis::operator new ( size_t size )

inline

Definition at line 79 of file CLocalMetricHypothesis.h.

◆ operator new[]()

void* mrpt::hmtslam::CLocalMetricHypothesis::operator new[] ( size_t size )

inline

Definition at line 79 of file CLocalMetricHypothesis.h.

◆ particlesEvaluator_AuxPFOptimal()

double mrpt::hmtslam::CLocalMetricHypothesis::particlesEvaluator_AuxPFOptimal	(	const mrpt::bayes::CParticleFilter::TParticleFilterOptions &	PF_options,
		size_t	index,
		const mrpt::obs::CSensoryFrame *	observation
	)

Auxiliary function used in "prediction_and_update_pfAuxiliaryPFOptimal".

Definition at line 630 of file CHMTSLAM_LSLAM_RBPF_2DLASER.cpp.

References ASSERT_, auxiliarComputeObservationLikelihood(), getCurrentPose(), m_maxLikelihood, m_movementDrawMaximumLikelihood, m_movementDraws, m_movementDrawsIdx, mrpt::bayes::CParticleFilterData< T >::m_particles, m_pfAuxiliaryPFOptimal_estimatedProb, m_poseParticles, mrpt::math::maximum(), MRPT_CHECK_NORMAL_NUMBER, MRPT_END, MRPT_START, and mrpt::bayes::CParticleFilter::TParticleFilterOptions::pfAuxFilterOptimal_MaximumSearchSamples.

Here is the call graph for this function:

◆ prediction_and_update() [1/3]

template<class PF_ALGORITHM >

void mrpt::hmtslam::CLocalMetricHypothesis::prediction_and_update	(	const mrpt::obs::CActionCollection *	action,
		const mrpt::obs::CSensoryFrame *	observation,
		const bayes::CParticleFilter::TParticleFilterOptions &	PF_options
	)

The PF algorithm implementation.

Definition at line 19 of file CLocalMetricHypothesis_impl.h.

References ASSERT_, and m_parent.

◆ prediction_and_update() [2/3]

template<>

void mrpt::hmtslam::CLocalMetricHypothesis::prediction_and_update	(	const mrpt::obs::CActionCollection *	actions,
		const mrpt::obs::CSensoryFrame *	sf,
		const bayes::CParticleFilter::TParticleFilterOptions &	PF_options
	)

The PF algorithm implementation for "optimal sampling for non-parametric observation models".

Definition at line 279 of file CHMTSLAM_LSLAM_RBPF_2DLASER.cpp.

References ASSERT_, mrpt::poses::CPose2D::composeFrom(), mrpt::obs::CActionRobotMovement2D::computeFromOdometry(), mrpt::random::CRandomGenerator::drawUniform(), mrpt::obs::CActionRobotMovement2D::fastDrawSingleSample(), mrpt::random::getRandomGenerator(), mrpt::utils::keep_max(), mrpt::math::maximum(), mrpt::math::mean(), min, mrpt::math::minimum(), MRPT_END, MRPT_START, mrpt::obs::CActionRobotMovement2D::prepareFastDrawSingleSamples(), mrpt::poses::CPose2D::resize(), mrpt::poses::CPose3D::resize(), mrpt::utils::CTicTac::Tac(), THROW_EXCEPTION, and mrpt::utils::CTicTac::Tic().

Here is the call graph for this function:

◆ prediction_and_update() [3/3]

template<>

void mrpt::hmtslam::CLocalMetricHypothesis::prediction_and_update	(	const mrpt::obs::CActionCollection *	actions,
		const mrpt::obs::CSensoryFrame *	sf,
		const bayes::CParticleFilter::TParticleFilterOptions &	PF_options
	)

The PF algorithm implementation for "optimal sampling" approximated with scan matching (Stachniss method)

Definition at line 830 of file CHMTSLAM_LSLAM_RBPF_2DLASER.cpp.

References mrpt::slam::CICP::TConfigParams::ALFA, mrpt::slam::CMetricMapsAlignmentAlgorithm::Align(), ASSERT_, mrpt::maps::CMetricMap::clear(), mrpt::poses::CPose3D::composeFrom(), mrpt::poses::CPosePDFGaussian::copyFrom(), mrpt::bayes::CProbabilityParticle< T >::d, DEG2RAD, mrpt::slam::CICP::TConfigParams::doRANSAC, mrpt::poses::CPose3DPDFGaussian::drawSingleSample(), mrpt::maps::CPointsMap::insertionOptions, mrpt::bayes::CProbabilityParticle< T >::log_w, mrpt::slam::CICP::TConfigParams::maxIterations, mrpt::maps::CPointsMap::TInsertionOptions::minDistBetweenLaserPoints, MRPT_END, MRPT_START, mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::norm(), mrpt::slam::CICP::TConfigParams::onlyClosestCorrespondences, mrpt::slam::CICP::options, mrpt::slam::CICP::TConfigParams::smallestThresholdDist, mrpt::math::square(), mrpt::slam::CICP::TConfigParams::thresholdAng, mrpt::slam::CICP::TConfigParams::thresholdDist, THROW_EXCEPTION, and mrpt::poses::CPose3D::yaw().

Here is the call graph for this function:

◆ readFromStream()

void CLocalMetricHypothesis::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 933 of file CLocalMetricHypothesis.cpp.

References MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION.

◆ rebuildMetricMaps()

void CLocalMetricHypothesis::rebuildMetricMaps ( )

Rebuild the metric maps of all particles from the observations and their estimated poses.

Definition at line 614 of file CLocalMetricHypothesis.cpp.

References ASSERT_.

Referenced by mrpt::hmtslam::CHMTSLAM::perform_TLC().

Here is the caller graph for this function:

◆ removeAreaFromLMH()

void CLocalMetricHypothesis::removeAreaFromLMH ( const CHMHMapNode::TNodeID areaID )

Removes a given area from the LMH:

The corresponding node in the HMT map is updated with the robot poses & SFs in the LMH.
Robot poses belonging to that area are removed from:
- the particles.
- the graph partitioner.
- the list of SFs.
- the list m_nodeIDmemberships.
m_neighbors is updated.

The weights of all particles are changed to remove the effects of the removed metric observations.

After calling this the metric maps should be updated.

This method internally calls updateAreaFromLMH

The corresponding node in the HMT map is updated with the robot poses & SFs in the LMH.
Robot poses belonging to that area are removed from:
- the particles.
- the graph partitioner.
- the list of SFs.
- the list m_nodeIDmemberships.
- The weights of all particles are changed to remove the effects of the removed metric observations.
After calling this the metric maps should be updated.

Definition at line 651 of file CLocalMetricHypothesis.cpp.

References ASSERT_, mrpt::utils::list_searchable< T >::find(), mrpt::utils::list_searchable< T >::insert(), MRPT_CHECK_NORMAL_NUMBER, MRPT_END, and MRPT_START.

Here is the call graph for this function:

◆ updateAreaFromLMH()

void CLocalMetricHypothesis::updateAreaFromLMH	(	const CHMHMapNode::TNodeID	areaID,
		bool	eraseSFsFromLMH = `false`
	)

The corresponding node in the HMT map is updated with the robot poses & SFs in the LMH: the poses are referenced to the area's reference poseID, such as that reference is at the origin.

If eraseSFsFromLMH=true, the sensoryframes are moved rather than copied to the area, and removed from the LMH.

Note: The critical section m_map_cs is locked internally, unlock it before calling this.

Definition at line 790 of file CLocalMetricHypothesis.cpp.

References ASSERT_, mrpt::maps::CMetricMap::clear(), mrpt::poses::CPose3DPDFParticles::copyFrom(), mrpt::utils::list_searchable< T >::insert(), mrpt::poses::CPose3DPDFParticles::inverse(), mrpt::bayes::CParticleFilterData< T >::m_particles, mrpt::obs::CSensoryFrame::moveFrom(), NODE_ANNOTATION_METRIC_MAPS, NODE_ANNOTATION_POSES_GRAPH, NODE_ANNOTATION_REF_POSEID, mrpt::hmtslam::TPoseInfo::pdf, mrpt::hmtslam::TPoseInfo::sf, and mrpt::poses::CPose3DPDFParticles::size().

Referenced by mrpt::hmtslam::CHMTSLAM::generateLogFiles(), mrpt::hmtslam::CHMTSLAM::perform_TLC(), and mrpt::hmtslam::CHMTSLAM::TBI_main_method().

Here is the call graph for this function:

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 CLocalMetricHypothesis::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 959 of file CLocalMetricHypothesis.cpp.

Friends And Related Function Documentation

◆ CLSLAM_RBPF_2DLASER

friend class CLSLAM_RBPF_2DLASER

friend

Definition at line 77 of file CLocalMetricHypothesis.h.

Member Data Documentation

◆ _init_CLocalMetricHypothesis

mrpt::utils::CLASSINIT mrpt::hmtslam::CLocalMetricHypothesis::_init_CLocalMetricHypothesis

staticprotected

Definition at line 79 of file CLocalMetricHypothesis.h.

◆ className

constexpr const char* mrpt::hmtslam::CLocalMetricHypothesis::className = "CLocalMetricHypothesis"

static

Definition at line 79 of file CLocalMetricHypothesis.h.

◆ m_accumRobotMovement

mrpt::obs::CActionRobotMovement2D mrpt::hmtslam::CLocalMetricHypothesis::m_accumRobotMovement

Used in CLSLAM_RBPF_2DLASER.

Definition at line 138 of file CLocalMetricHypothesis.h.

◆ m_accumRobotMovementIsValid

bool mrpt::hmtslam::CLocalMetricHypothesis::m_accumRobotMovementIsValid

Used in CLSLAM_RBPF_2DLASER.

Definition at line 140 of file CLocalMetricHypothesis.h.

◆ m_areasPendingTBI

TNodeIDList mrpt::hmtslam::CLocalMetricHypothesis::m_areasPendingTBI

The list of area IDs waiting to be processed by the TBI (topological bayesian inference) engines to search for potential loop-closures.

Set in CHMTSLAM::LSLAM_process_message_from_AA, read in

Definition at line 127 of file CLocalMetricHypothesis.h.

◆ m_currentRobotPose

TPoseID mrpt::hmtslam::CLocalMetricHypothesis::m_currentRobotPose

The current robot pose (its global unique ID) for this hypothesis.

Definition at line 110 of file CLocalMetricHypothesis.h.

Referenced by mrpt::hmtslam::CHMTSLAM::initializeEmptyMap(), and mrpt::hmtslam::CLSLAM_RBPF_2DLASER::processOneLMH().

◆ m_ID

THypothesisID mrpt::hmtslam::CLocalMetricHypothesis::m_ID

The unique ID of the hypothesis (Used for accessing mrpt::slam::CHierarchicalMHMap).

Definition at line 106 of file CLocalMetricHypothesis.h.

Referenced by mrpt::hmtslam::CHMTSLAM::areaAbstraction(), mrpt::hmtslam::CHMTSLAM::generateLogFiles(), mrpt::hmtslam::CHMTSLAM::initializeEmptyMap(), mrpt::hmtslam::CHMTSLAM::perform_TLC(), mrpt::hmtslam::CLSLAM_RBPF_2DLASER::processOneLMH(), and mrpt::hmtslam::CHMTSLAM::TBI_main_method().

◆ m_log_w

double mrpt::hmtslam::CLocalMetricHypothesis::m_log_w

Log-weight of this hypothesis.

Definition at line 130 of file CLocalMetricHypothesis.h.

Referenced by mrpt::hmtslam::CHMTSLAM::generateLogFiles(), and mrpt::hmtslam::CHMTSLAM::initializeEmptyMap().

◆ m_log_w_metric_history

std::vector<std::map<TPoseID, double> > mrpt::hmtslam::CLocalMetricHypothesis::m_log_w_metric_history

The historic log-weights of the metric observations inserted in this LMH, for each particle.

Definition at line 133 of file CLocalMetricHypothesis.h.

◆ m_maxLikelihood

std::vector<double> mrpt::hmtslam::CLocalMetricHypothesis::m_maxLikelihood

protected

Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.

Definition at line 297 of file CLocalMetricHypothesis.h.

Referenced by particlesEvaluator_AuxPFOptimal().

◆ m_movementDrawMaximumLikelihood

mrpt::poses::StdVector_CPose2D mrpt::hmtslam::CLocalMetricHypothesis::m_movementDrawMaximumLikelihood

protected

Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.

Definition at line 306 of file CLocalMetricHypothesis.h.

Referenced by particlesEvaluator_AuxPFOptimal().

◆ m_movementDraws

mrpt::poses::StdVector_CPose2D mrpt::hmtslam::CLocalMetricHypothesis::m_movementDraws

protected

Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.

Definition at line 303 of file CLocalMetricHypothesis.h.

Referenced by particlesEvaluator_AuxPFOptimal().

◆ m_movementDrawsIdx

unsigned int mrpt::hmtslam::CLocalMetricHypothesis::m_movementDrawsIdx

protected

Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.

Definition at line 300 of file CLocalMetricHypothesis.h.

Referenced by particlesEvaluator_AuxPFOptimal().

◆ m_neighbors

TNodeIDSet mrpt::hmtslam::CLocalMetricHypothesis::m_neighbors

The list of all areas sourronding the current one (this includes the current area itself).

Definition at line 116 of file CLocalMetricHypothesis.h.

Referenced by mrpt::hmtslam::CHMTSLAM::generateLogFiles(), mrpt::hmtslam::CHMTSLAM::initializeEmptyMap(), mrpt::hmtslam::CHMTSLAM::perform_TLC(), and mrpt::hmtslam::CHMTSLAM::TBI_main_method().

◆ m_nodeIDmemberships

std::map<TPoseID, CHMHMapNode::TNodeID> mrpt::hmtslam::CLocalMetricHypothesis::m_nodeIDmemberships

The hybrid map node membership for each robot pose.

Definition at line 118 of file CLocalMetricHypothesis.h.

Referenced by mrpt::hmtslam::CHMTSLAM::perform_TLC(), and mrpt::hmtslam::CLSLAM_RBPF_2DLASER::processOneLMH().

◆ m_parent

mrpt::utils::safe_ptr<CHMTSLAM> mrpt::hmtslam::CLocalMetricHypothesis::m_parent

For quick access to our parent object.

Definition at line 108 of file CLocalMetricHypothesis.h.

Referenced by mrpt::hmtslam::CHMTSLAM::areaAbstraction(), mrpt::hmtslam::CHMTSLAM::initializeEmptyMap(), prediction_and_update(), and mrpt::hmtslam::CHMTSLAM::TBI_main_method().

◆ m_pfAuxiliaryPFOptimal_estimatedProb

mrpt::math::CVectorDouble mrpt::hmtslam::CLocalMetricHypothesis::m_pfAuxiliaryPFOptimal_estimatedProb

protected

Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.

Definition at line 293 of file CLocalMetricHypothesis.h.

Referenced by particlesEvaluator_AuxPFOptimal().

◆ m_poseParticles

CLSLAMParticleDataParticles mrpt::hmtslam::CLocalMetricHypothesis::m_poseParticles

Definition at line 85 of file CLocalMetricHypothesis.h.

Referenced by auxiliarComputeObservationLikelihood(), mrpt::hmtslam::CHMTSLAM::generateLogFiles(), particlesEvaluator_AuxPFOptimal(), mrpt::hmtslam::CHMTSLAM::perform_TLC(), and mrpt::hmtslam::CLSLAM_RBPF_2DLASER::processOneLMH().

◆ m_posesPendingAddPartitioner

TPoseIDList mrpt::hmtslam::CLocalMetricHypothesis::m_posesPendingAddPartitioner

The list of poseIDs waiting to be added to the graph partitioner, what happens in the LSLAM thread main loop.

Definition at line 123 of file CLocalMetricHypothesis.h.

Referenced by mrpt::hmtslam::CHMTSLAM::perform_TLC(), and mrpt::hmtslam::CLSLAM_RBPF_2DLASER::processOneLMH().

◆ m_robotPosesGraph

struct mrpt::hmtslam::CLocalMetricHypothesis::TRobotPosesPartitioning mrpt::hmtslam::CLocalMetricHypothesis::m_robotPosesGraph

Referenced by mrpt::hmtslam::CHMTSLAM::areaAbstraction(), mrpt::hmtslam::CHMTSLAM::generateLogFiles(), and mrpt::hmtslam::CHMTSLAM::perform_TLC().

◆ m_SFs

std::map<TPoseID, mrpt::obs::CSensoryFrame> mrpt::hmtslam::CLocalMetricHypothesis::m_SFs

The SF gathered at each robot pose.

Definition at line 120 of file CLocalMetricHypothesis.h.

Referenced by mrpt::hmtslam::CHMTSLAM::areaAbstraction(), mrpt::hmtslam::CHMTSLAM::perform_TLC(), and mrpt::hmtslam::CLSLAM_RBPF_2DLASER::processOneLMH().

◆ runtimeClassId

const mrpt::utils::TRuntimeClassId mrpt::hmtslam::CLocalMetricHypothesis::runtimeClassId

staticprotected

Definition at line 79 of file CLocalMetricHypothesis.h.

◆ threadLocks

mrpt::hmtslam::CLocalMetricHypothesis::threadLocks

Definition at line 103 of file CLocalMetricHypothesis.h.

Referenced by mrpt::hmtslam::CHMTSLAM::generateLogFiles().

Detailed Description

Classes

Public Types

Public Member Functions

Static Public Member Functions

Public Attributes

Protected Member Functions

Protected Attributes

Friends

RTTI stuff

Member Typedef Documentation

◆ ConstPtr

◆ CParticleData

◆ CParticleList

◆ Ptr

Constructor & Destructor Documentation

◆ CLocalMetricHypothesis()

◆ ~CLocalMetricHypothesis()

Member Function Documentation

◆ _GetBaseClass()

◆ auxiliarComputeObservationLikelihood()

◆ changeCoordinateOrigin()

◆ clearRobotPoses()

◆ clone()

◆ Create()

◆ CreateObject()

◆ dumpAsText()

◆ executeOn()

◆ getAs3DScene()

◆ getCurrentPose() [1/2]

◆ getCurrentPose() [2/2]

◆ getMeans()

◆ getPathParticles()

◆ getPoseParticles()

◆ getRelativePose()

◆ GetRuntimeClass()

◆ GetRuntimeClassIdStatic()

◆ MRPT_TODO()

◆ 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[]()

◆ particlesEvaluator_AuxPFOptimal()

◆ prediction_and_update() [1/3]

◆ prediction_and_update() [2/3]

◆ prediction_and_update() [3/3]

◆ readFromStream()

◆ rebuildMetricMaps()

◆ removeAreaFromLMH()

◆ updateAreaFromLMH()

◆ writeToMatlab()

◆ writeToStream()

Friends And Related Function Documentation

◆ CLSLAM_RBPF_2DLASER

Member Data Documentation

◆ _init_CLocalMetricHypothesis

◆ className

◆ m_accumRobotMovement

◆ m_accumRobotMovementIsValid

◆ m_areasPendingTBI

◆ m_currentRobotPose

◆ m_ID

◆ m_log_w

◆ m_log_w_metric_history

◆ m_maxLikelihood

◆ m_movementDrawMaximumLikelihood

◆ m_movementDraws

◆ m_movementDrawsIdx

◆ m_neighbors

◆ m_nodeIDmemberships

◆ m_parent

◆ m_pfAuxiliaryPFOptimal_estimatedProb

◆ m_poseParticles

◆ m_posesPendingAddPartitioner

◆ m_robotPosesGraph

◆ m_SFs