Detailed Description

An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).

The main entry points for a user are pushAction() and pushObservations(). Several parameters can be modified through m_options.

The mathematical models of this approach have been reported in:

Blanco J.L., Fernandez-Madrigal J.A., and Gonzalez J., "Towards a Unified Bayesian Approach to Hybrid Metric-Topological SLAM", in IEEE Transactions on Robotics (TRO), Vol. 24, No. 2, April 2008.
...

More information in the wiki page: http://www.mrpt.org/HMT-SLAM . A complete working application can be found in "MRPT/apps/hmt-slam".

The complete state of the SLAM framework is serializable, so it can be saved and restore to/from a binary dump. This class implements mrpt::utils::CSerializable, so it can be saved with "stream << slam_object;" and restored with "stream >> slam_object;". Alternatively, the methods CHMTSLAM::saveState and CHMTSLAM::loadState can be invoked, which in turn call internally to the CSerializable interface.

See also:: CHierarchicalMHMap

#include <mrpt/hmtslam/CHMTSLAM.h>

Inheritance diagram for mrpt::hmtslam::CHMTSLAM:

[legend]

List of all members.

Classes
struct	TMessageLSLAMfromAA
	Message definition: More...
struct	TMessageLSLAMfromTBI
	Message definition: More...
struct	TMessageLSLAMtoTBI
	Message definition: More...
struct	TOptions
	A variety of options and configuration params (private, use loadOptions). More...
Public Member Functions
	CHMTSLAM ()
	Default constructor.
	CHMTSLAM (const CHMTSLAM &o)
const CHMTSLAM &	operator= (const CHMTSLAM &o)
virtual	~CHMTSLAM ()
	Destructor.
bool	abortedDueToErrors ()
	Return true if an exception has been caught in any thread leading to the end of the mapping application: no more actions/observations will be processed from now on.
void	generateLogFiles (unsigned int nIteration)
	Called from LSLAM thread when log files must be created.
void	getAs3DScene (COpenGLScene &outScene)
	Gets a 3D representation of the current state of the whole mapping framework.
mrpt::utils::CObjectPtr	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).
CObject *	clone () const
	Cloning interface for smart pointers.
High-level map management
void	loadOptions (const std::string &configFile)
	Loads the options from a config file.
void	loadOptions (const mrpt::utils::CConfigFileBase &cfgSource)
	Loads the options from a config source.
void	initializeEmptyMap ()
	Initializes the whole HMT-SLAM framework, reseting to an empty map (It also clears the logs directory) - this must be called AFTER loading the options with CHMTSLAM::loadOptions.
bool	saveState (CStream &out) const
	Save the state of the whole HMT-SLAM framework to some binary stream (e.g.
bool	loadState (CStream &in)
	Load the state of the whole HMT-SLAM framework from some binary stream (e.g.
Static Public Member Functions
static void	printf_debug (const char *frmt,...)
	Sends a formated text to "debugOut" if not NULL, or to cout otherwise.
Public Attributes
The important data.
CHierarchicalMHMap	m_map
	The hiearchical, multi-hypothesis graph-based map.
aligned_containers < THypothesisID, CLocalMetricHypothesis > ::map_t	m_LMHs
	The list of LMHs at each instant.
Static Public Attributes
static const mrpt::utils::TRuntimeClassId	classCObject
Protected Member Functions
virtual void	writeToStream (mrpt::utils::CStream &out, int *getVersion) const =0
	Introduces a pure virtual method responsible for writing to a CStream.
virtual void	readFromStream (mrpt::utils::CStream &in, int version)=0
	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.
HMT-SLAM sub-processes.
void	LSLAM_process_message (const CMessage &msg)
	Auxiliary method within thread_LSLAM.
void	LSLAM_process_message_from_AA (const TMessageLSLAMfromAA &myMsg)
	No critical section locks are assumed at the entrance of this method.
void	LSLAM_process_message_from_TBI (const TMessageLSLAMfromTBI &myMsg)
	No critical section locks are assumed at the entrance of this method.
void	perform_TLC (CLocalMetricHypothesis &LMH, const CHMHMapNode::TNodeID areaInLMH, const CHMHMapNode::TNodeID areaLoopClosure, const mrpt::poses::CPose3DPDFGaussian &pose1wrt2)
	Topological Loop Closure: Performs all the required operations to close a loop between two areas which have been determined to be the same.
Static Protected Member Functions
static TMessageLSLAMfromAAPtr	areaAbstraction (CLocalMetricHypothesis *LMH, const TPoseIDList &newPoseIDs)
	The Area Abstraction (AA) method, invoked from LSLAM.
static TMessageLSLAMfromTBIPtr	TBI_main_method (CLocalMetricHypothesis *LMH, const CHMHMapNode::TNodeID &areaID)
	The entry point for Topological Bayesian Inference (TBI) engines, invoked from LSLAM.
static std::string	generateUniqueAreaLabel ()
	Generates a new and unique area textual label (currently this generates "0","1",...)
static TPoseID	generatePoseID ()
	Generates a new and unique pose ID.
static THypothesisID	generateHypothesisID ()
	Generates a new and unique hypothesis ID.
Protected Attributes
bool	m_terminateThreads
	Termination flag for signaling all threads to terminate.
bool	m_terminationFlag_LSLAM
	Threads termination flags:
bool	m_terminationFlag_TBI
bool	m_terminationFlag_3D_viewer
mrpt::hmtslam::CHMTSLAM::TOptions	m_options
The different SLAM algorithms that can be invoked from the LSLAM thread.
CLSLAMAlgorithmBase *	m_LSLAM_method
	An instance of a local SLAM method, to be applied to each LMH - initialized by "initializeEmptyMap" or "loadState".
Static Protected Attributes
static int64_t	m_nextAreaLabel
static TPoseID	m_nextPoseID
static THypothesisID	m_nextHypID
Friends
class HMTSLAM_IMPEXP	CLocalMetricHypothesis
class HMTSLAM_IMPEXP	CLSLAM_RBPF_2DLASER
class HMTSLAM_IMPEXP	CTopLCDetector_GridMatching
class HMTSLAM_IMPEXP	CTopLCDetector_FabMap
RTTI stuff
class	mrpt::utils::CStream
static const mrpt::utils::TRuntimeClassId	classCSerializable
RTTI stuff
typedef CHMTSLAMPtr	SmartPtr
static mrpt::utils::CLASSINIT	_init_CHMTSLAM
static mrpt::utils::TRuntimeClassId	classCHMTSLAM
static const mrpt::utils::TRuntimeClassId *	classinfo
static const mrpt::utils::TRuntimeClassId *	_GetBaseClass ()
virtual const mrpt::utils::TRuntimeClassId *	GetRuntimeClass () const
	Returns information about the class of an object in runtime.
virtual mrpt::utils::CObject *	duplicate () const
	Returns a copy of the object, indepently of its class.
static mrpt::utils::CObject *	CreateObject ()
static CHMTSLAMPtr	Create ()
Inter-thread communication queues:
typedef stlplus::smart_ptr < TMessageLSLAMfromAA >	TMessageLSLAMfromAAPtr
typedef stlplus::smart_ptr < TMessageLSLAMtoTBI >	TMessageLSLAMtoTBIPtr
typedef stlplus::smart_ptr < TMessageLSLAMfromTBI >	TMessageLSLAMfromTBIPtr
utils::CMessageQueue	m_LSLAM_queue
	LSLAM thread input queue, messages of type CHMTSLAM::TMessageLSLAMfromAA.
Related to the input queue:
enum	TLSlamMethod { lsmRBPF_2DLASER = 1 }
std::queue< CSerializablePtr >	m_inputQueue
	The queue of pending actions/observations supplied by the user waiting for being processed.
synch::CCriticalSection	m_inputQueue_cs
	Critical section for accessing m_inputQueue.
synch::CCriticalSection	m_map_cs
	Critical section for accessing m_map.
synch::CCriticalSection	m_LMHs_cs
	Critical section for accessing m_LMHs.
void	clearInputQueue ()
	Empty the input queue.
bool	isInputQueueEmpty ()
	Returns true if the input queue is empty (Note that the queue must not be empty to the user to enqueue more actions/observaitions)
size_t	inputQueueSize ()
	Returns the number of objects waiting for processing in the input queue.
void	pushAction (const CActionCollectionPtr &acts)
	Here the user can enter an action into the system (will go to the SLAM process).
void	pushObservations (const CSensoryFramePtr &sf)
	Here the user can enter observations into the system (will go to the SLAM process).
void	pushObservation (const CObservationPtr &obs)
	Here the user can enter an observation into the system (will go to the SLAM process).
CSerializablePtr	getNextObjectFromInputQueue ()
	Used from the LSLAM thread to retrieve the next object from the queue.
Threads stuff
TThreadHandle	m_hThread_LSLAM
	Threads handles.
TThreadHandle	m_hThread_TBI
TThreadHandle	m_hThread_3D_viewer
void	thread_LSLAM ()
	The function for the "Local SLAM" thread.
void	thread_TBI ()
	The function for the "TBI" thread.
void	thread_3D_viewer ()
	The function for the "3D viewer" thread.
The different Loop-Closure modules that are to be executed in the TBI thread.
typedef CTopLCDetectorBase (	TLopLCDetectorFactory )(CHMTSLAM *)
std::map< std::string, TLopLCDetectorFactory >	m_registeredLCDetectors
std::deque< CTopLCDetectorBase * >	m_topLCdets
	The list of LC modules in operation - initialized by "initializeEmptyMap" or "loadState".
synch::CCriticalSection	m_topLCdets_cs
	The critical section for accessing m_topLCdets.
void	registerLoopClosureDetector (const std::string &name, CTopLCDetectorBase (ptrCreateObject)(CHMTSLAM *))
	Must be invoked before calling initializeEmptyMap, so LC objects can be created.
CTopLCDetectorBase *	loopClosureDetector_factory (const std::string &name)
	The class factory for topological loop closure detectors.

Member Typedef Documentation

typedef CHMTSLAMPtr mrpt::hmtslam::CHMTSLAM::SmartPtr

A typedef for the associated smart pointer

Definition at line 100 of file CHMTSLAM.h.

typedef CTopLCDetectorBase*(* mrpt::hmtslam::CHMTSLAM::TLopLCDetectorFactory)(CHMTSLAM *) [protected]

Definition at line 318 of file CHMTSLAM.h.

typedef stlplus::smart_ptr<TMessageLSLAMfromAA> mrpt::hmtslam::CHMTSLAM::TMessageLSLAMfromAAPtr [protected]

Definition at line 130 of file CHMTSLAM.h.

typedef stlplus::smart_ptr<TMessageLSLAMfromTBI> mrpt::hmtslam::CHMTSLAM::TMessageLSLAMfromTBIPtr [protected]

Definition at line 174 of file CHMTSLAM.h.

typedef stlplus::smart_ptr<TMessageLSLAMtoTBI> mrpt::hmtslam::CHMTSLAM::TMessageLSLAMtoTBIPtr [protected]

Definition at line 142 of file CHMTSLAM.h.

Member Enumeration Documentation

enum mrpt::hmtslam::CHMTSLAM::TLSlamMethod

Enumerator:

lsmRBPF_2DLASER

Definition at line 236 of file CHMTSLAM.h.

Constructor & Destructor Documentation

mrpt::hmtslam::CHMTSLAM::CHMTSLAM ( )

Default constructor.

Parameters:

debug_out_stream If debug output messages should be redirected to any other stream apart from std::cout

mrpt::hmtslam::CHMTSLAM::CHMTSLAM ( const CHMTSLAM & o ) [inline]

Definition at line 376 of file CHMTSLAM.h.

References THROW_EXCEPTION.

virtual mrpt::hmtslam::CHMTSLAM::~CHMTSLAM ( ) [virtual]

Destructor.

Member Function Documentation

static const mrpt::utils::TRuntimeClassId* mrpt::hmtslam::CHMTSLAM::_GetBaseClass ( ) [static, protected]

Reimplemented from mrpt::utils::CSerializable.

bool mrpt::hmtslam::CHMTSLAM::abortedDueToErrors ( )

Return true if an exception has been caught in any thread leading to the end of the mapping application: no more actions/observations will be processed from now on.

static TMessageLSLAMfromAAPtr mrpt::hmtslam::CHMTSLAM::areaAbstraction	(	CLocalMetricHypothesis *	LMH,
		const TPoseIDList &	newPoseIDs
	)		`[static, protected]`

The Area Abstraction (AA) method, invoked from LSLAM.

Parameters:

LMH	(IN) The LMH which to this query applies.
newPoseIDs	(IN) The new poseIDs to be added to the graph partitioner.

Returns:: A structure with all return data. Memory to be freed by user.

Note:: The critical section for LMH must be locked BEFORE calling this method (it does NOT lock any critical section).

void mrpt::hmtslam::CHMTSLAM::clearInputQueue ( )

Empty the input queue.

CObject* mrpt::utils::CObject::clone ( ) const [inline, inherited]

Cloning interface for smart pointers.

Reimplemented in mrpt::opengl::CRenderizable, and mrpt::opengl::CRenderizableDisplayList.

Definition at line 154 of file CObject.h.

static CHMTSLAMPtr mrpt::hmtslam::CHMTSLAM::Create ( ) [static]

static mrpt::utils::CObject* mrpt::hmtslam::CHMTSLAM::CreateObject ( ) [static]

virtual mrpt::utils::CObject* mrpt::hmtslam::CHMTSLAM::duplicate ( ) const [virtual]

Returns a copy of the object, indepently of its class.

Implements mrpt::utils::CObject.

mrpt::utils::CObjectPtr mrpt::utils::CObject::duplicateGetSmartPtr ( ) const [inline, inherited]

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

Definition at line 151 of file CObject.h.

static THypothesisID mrpt::hmtslam::CHMTSLAM::generateHypothesisID ( ) [static, protected]

Generates a new and unique hypothesis ID.

void mrpt::hmtslam::CHMTSLAM::generateLogFiles ( unsigned int nIteration )

Called from LSLAM thread when log files must be created.

static TPoseID mrpt::hmtslam::CHMTSLAM::generatePoseID ( ) [static, protected]

Generates a new and unique pose ID.

static std::string mrpt::hmtslam::CHMTSLAM::generateUniqueAreaLabel ( ) [static, protected]

Generates a new and unique area textual label (currently this generates "0","1",...)

void mrpt::hmtslam::CHMTSLAM::getAs3DScene ( COpenGLScene & outScene )

Gets a 3D representation of the current state of the whole mapping framework.

CSerializablePtr mrpt::hmtslam::CHMTSLAM::getNextObjectFromInputQueue ( ) [protected]

Used from the LSLAM thread to retrieve the next object from the queue.

Returns:: The object, or NULL if empty.

virtual const mrpt::utils::TRuntimeClassId* mrpt::hmtslam::CHMTSLAM::GetRuntimeClass ( ) const [virtual]

Returns information about the class of an object in runtime.

Reimplemented from mrpt::utils::CSerializable.

void mrpt::hmtslam::CHMTSLAM::initializeEmptyMap ( )

Initializes the whole HMT-SLAM framework, reseting to an empty map (It also clears the logs directory) - this must be called AFTER loading the options with CHMTSLAM::loadOptions.

size_t mrpt::hmtslam::CHMTSLAM::inputQueueSize ( )

Returns the number of objects waiting for processing in the input queue.

See also:: pushAction,pushObservations, isInputQueueEmpty

bool mrpt::hmtslam::CHMTSLAM::isInputQueueEmpty ( )

Returns true if the input queue is empty (Note that the queue must not be empty to the user to enqueue more actions/observaitions)

See also:: pushAction,pushObservations, inputQueueSize

void mrpt::hmtslam::CHMTSLAM::loadOptions ( const std::string & configFile )

Loads the options from a config file.

void mrpt::hmtslam::CHMTSLAM::loadOptions ( const mrpt::utils::CConfigFileBase & cfgSource )

Loads the options from a config source.

bool mrpt::hmtslam::CHMTSLAM::loadState ( CStream & in )

Load the state of the whole HMT-SLAM framework from some binary stream (e.g.

a file).

Returns:: true if everything goes OK.

See also:: saveState

CTopLCDetectorBase* mrpt::hmtslam::CHMTSLAM::loopClosureDetector_factory ( const std::string & name )

The class factory for topological loop closure detectors.

Possible values are enumerated in TOptions::TLC_detectors

Exceptions:

std::exception On unknown name.

void mrpt::hmtslam::CHMTSLAM::LSLAM_process_message ( const CMessage & msg ) [protected]

Auxiliary method within thread_LSLAM.

void mrpt::hmtslam::CHMTSLAM::LSLAM_process_message_from_AA ( const TMessageLSLAMfromAA & myMsg ) [protected]

No critical section locks are assumed at the entrance of this method.

void mrpt::hmtslam::CHMTSLAM::LSLAM_process_message_from_TBI ( const TMessageLSLAMfromTBI & myMsg ) [protected]

No critical section locks are assumed at the entrance of this method.

const CHMTSLAM& mrpt::hmtslam::CHMTSLAM::operator= ( const CHMTSLAM & o ) [inline]

Definition at line 377 of file CHMTSLAM.h.

References THROW_EXCEPTION.

void mrpt::hmtslam::CHMTSLAM::perform_TLC	(	CLocalMetricHypothesis &	LMH,
		const CHMHMapNode::TNodeID	areaInLMH,
		const CHMHMapNode::TNodeID	areaLoopClosure,
		const mrpt::poses::CPose3DPDFGaussian &	pose1wrt2
	)		`[protected]`

Topological Loop Closure: Performs all the required operations to close a loop between two areas which have been determined to be the same.

static void mrpt::utils::CDebugOutputCapable::printf_debug	(	const char *	frmt,
			...
	)		`[static, inherited]`

Sends a formated text to "debugOut" if not NULL, or to cout otherwise.

Referenced by mrpt::math::CLevenbergMarquardtTempl::execute().

void mrpt::hmtslam::CHMTSLAM::pushAction ( const CActionCollectionPtr & acts )

Here the user can enter an action into the system (will go to the SLAM process).

This class will delete the passed object when required, so DO NOT DELETE the passed object after calling this.

See also:: pushObservations,pushObservation

void mrpt::hmtslam::CHMTSLAM::pushObservation ( const CObservationPtr & obs )

Here the user can enter an observation into the system (will go to the SLAM process).

This class will delete the passed object when required, so DO NOT DELETE the passed object after calling this.

See also:: pushAction,pushObservation

void mrpt::hmtslam::CHMTSLAM::pushObservations ( const CSensoryFramePtr & sf )

Here the user can enter observations into the system (will go to the SLAM process).

This class will delete the passed object when required, so DO NOT DELETE the passed object after calling this.

See also:: pushAction,pushObservation

virtual void mrpt::utils::CSerializable::readFromStream	(	mrpt::utils::CStream &	in,
		int	version
	)		`[protected, pure virtual, inherited]`

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

Implemented in mrpt::math::CMatrixD, and mrpt::math::CMatrix.

void mrpt::hmtslam::CHMTSLAM::registerLoopClosureDetector	(	const std::string &	name,
		CTopLCDetectorBase ()(CHMTSLAM *)	ptrCreateObject
	)

Must be invoked before calling initializeEmptyMap, so LC objects can be created.

bool mrpt::hmtslam::CHMTSLAM::saveState ( CStream & out ) const

Save the state of the whole HMT-SLAM framework to some binary stream (e.g.

a file).

Returns:: true if everything goes OK.

See also:: loadState

static TMessageLSLAMfromTBIPtr mrpt::hmtslam::CHMTSLAM::TBI_main_method	(	CLocalMetricHypothesis *	LMH,
		const CHMHMapNode::TNodeID &	areaID
	)		`[static, protected]`

The entry point for Topological Bayesian Inference (TBI) engines, invoked from LSLAM.

Parameters:

LMH	(IN) The LMH which to this query applies.
areaID	(IN) The area ID to consider for potential loop-closures.

Note:: The critical section for LMH must be locked BEFORE calling this method (it does NOT lock any critical section).

void mrpt::hmtslam::CHMTSLAM::thread_3D_viewer ( ) [protected]

The function for the "3D viewer" thread.

void mrpt::hmtslam::CHMTSLAM::thread_LSLAM ( ) [protected]

The function for the "Local SLAM" thread.

void mrpt::hmtslam::CHMTSLAM::thread_TBI ( ) [protected]

The function for the "TBI" thread.

virtual void mrpt::utils::CSerializable::writeToStream	(	mrpt::utils::CStream &	out,
		int *	getVersion
	)		const `[protected, pure virtual, inherited]`

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 NULL, 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