Detailed Description

A class for very simple 2D SLAM based on ICP.

This is a non-probabilistic pose tracking algorithm. Map are stored as in files as binary dumps of "mrpt::maps::CSimpleMap" objects. The methods are thread-safe.

Definition at line 28 of file CMetricMapBuilderICP.h.

#include <mrpt/slam/CMetricMapBuilderICP.h>

Inheritance diagram for mrpt::slam::CMetricMapBuilderICP:

Classes
struct	TConfigParams
	Algorithm configuration params. More...

struct	TDist
	Traveled distances from last map update / ICP-based localization. More...

Public Member Functions
	CMetricMapBuilderICP ()
	Default constructor - Upon construction, you can set the parameters in ICP_options, then call "initialize". More...

virtual	~CMetricMapBuilderICP ()
	Destructor: More...

void	initialize (const mrpt::maps::CSimpleMap &initialMap=mrpt::maps::CSimpleMap(), mrpt::poses::CPosePDF *x0=NULL)
	Initialize the method, starting with a known location PDF "x0"(if supplied, set to NULL to left unmodified) and a given fixed, past map. More...

mrpt::poses::CPose3DPDFPtr	getCurrentPoseEstimation () const
	Returns a copy of the current best pose estimation as a pose PDF. More...

void	setCurrentMapFile (const char *mapFile)
	Sets the "current map file", thus that map will be loaded if it exists or a new one will be created if it does not, and the updated map will be save to that file when destroying the object. More...

void	processActionObservation (mrpt::obs::CActionCollection &action, mrpt::obs::CSensoryFrame &in_SF)
	Appends a new action and observations to update this map: See the description of the class at the top of this page to see a more complete description. More...

void	processObservation (const mrpt::obs::CObservationPtr &obs)
	The main method of this class: Process one odometry or sensor observation. More...

void	getCurrentlyBuiltMap (mrpt::maps::CSimpleMap &out_map) const
	Fills "out_map" with the set of "poses"-"sensory-frames", thus the so far built map. More...

void	getCurrentMapPoints (std::vector< float > &x, std::vector< float > &y)
	Returns the 2D points of current local map. More...

mrpt::maps::CMultiMetricMap *	getCurrentlyBuiltMetricMap ()
	Returns the map built so far. More...

unsigned int	getCurrentlyBuiltMapSize ()
	Returns just how many sensory-frames are stored in the currently build map. More...

void	saveCurrentEstimationToImage (const std::string &file, bool formatEMF_BMP=true)
	A useful method for debugging: the current map (and/or poses) estimation is dumped to an image file. More...

void	clear ()
	Clear all elements of the maps, and reset localization to (0,0,0deg). More...

void	enableMapUpdating (bool enable)
	Enables or disables the map updating (default state is enabled) More...

void	loadCurrentMapFromFile (const std::string &fileName)
	Load map (mrpt::maps::CSimpleMap) from a ".simplemap" file. More...

void	saveCurrentMapToFile (const std::string &fileName, bool compressGZ=true) const
	Save map (mrpt::maps::CSimpleMap) to a ".simplemap" file. More...

Static Public Member Functions
static void	printf_debug (const char *frmt,...)
	Sends a formated text to "debugOut" if not NULL, or to cout otherwise. More...

Public Attributes
TConfigParams	ICP_options
	Options for the ICP-SLAM application. More...

CICP::TConfigParams	ICP_params
	Options for the ICP algorithm itself. More...

TOptions	options

Protected Member Functions
void	enterCriticalSection ()
	Enter critical section for map updating: More...

void	leaveCriticalSection ()
	Leave critical section for map updating: More...

Protected Attributes
synch::CCriticalSection	critZoneChangingMap
	Critical zones. More...

Private Member Functions
void	accumulateRobotDisplacementCounters (const mrpt::poses::CPose2D &new_pose)

void	resetRobotDisplacementCounters (const mrpt::poses::CPose2D &new_pose)

Private Attributes
mrpt::maps::CSimpleMap	SF_Poses_seq
	The set of observations that leads to current map: More...

mrpt::maps::CMultiMetricMap	metricMap
	The metric map representation as a points map: More...

std::string	currentMapFile
	Current map file. More...

mrpt::poses::CRobot2DPoseEstimator	m_lastPoseEst
	The pose estimation by the alignment algorithm (ICP). More...

mrpt::math::CMatrixDouble33	m_lastPoseEst_cov
	Last pose estimation (covariance) More...

std::deque< mrpt::math::TPose2D >	m_estRobotPath
	The estimated robot path: More...

mrpt::poses::CPose2D	m_auxAccumOdometry

TDist	m_distSinceLastICP

mrpt::aligned_containers< std::string, TDist >::map_t	m_distSinceLastInsertion
	Indexed by sensor label. More...

bool	m_there_has_been_an_odometry

Constructor & Destructor Documentation

◆ CMetricMapBuilderICP()

mrpt::slam::CMetricMapBuilderICP::CMetricMapBuilderICP ( )

Default constructor - Upon construction, you can set the parameters in ICP_options, then call "initialize".

◆ ~CMetricMapBuilderICP()

virtual mrpt::slam::CMetricMapBuilderICP::~CMetricMapBuilderICP ( )

virtual

Destructor:

Member Function Documentation

◆ accumulateRobotDisplacementCounters()

void mrpt::slam::CMetricMapBuilderICP::accumulateRobotDisplacementCounters ( const mrpt::poses::CPose2D & new_pose )

private

◆ clear()

void mrpt::slam::CMetricMapBuilder::clear ( )

inherited

Clear all elements of the maps, and reset localization to (0,0,0deg).

◆ enableMapUpdating()

void mrpt::slam::CMetricMapBuilder::enableMapUpdating ( bool enable )

inlineinherited

Enables or disables the map updating (default state is enabled)

Definition at line 95 of file CMetricMapBuilder.h.

◆ enterCriticalSection()

void mrpt::slam::CMetricMapBuilder::enterCriticalSection ( )

inlineprotectedinherited

Enter critical section for map updating:

Definition at line 44 of file CMetricMapBuilder.h.

References mrpt::synch::CCriticalSection::enter().

◆ getCurrentlyBuiltMap()

void mrpt::slam::CMetricMapBuilderICP::getCurrentlyBuiltMap ( mrpt::maps::CSimpleMap & out_map ) const

virtual

Fills "out_map" with the set of "poses"-"sensory-frames", thus the so far built map.

Implements mrpt::slam::CMetricMapBuilder.

◆ getCurrentlyBuiltMapSize()

unsigned int mrpt::slam::CMetricMapBuilderICP::getCurrentlyBuiltMapSize ( )

virtual

Returns just how many sensory-frames are stored in the currently build map.

Implements mrpt::slam::CMetricMapBuilder.

◆ getCurrentlyBuiltMetricMap()

mrpt::maps::CMultiMetricMap* mrpt::slam::CMetricMapBuilderICP::getCurrentlyBuiltMetricMap ( )

virtual

Returns the map built so far.

NOTE that for efficiency a pointer to the internal object is passed, DO NOT delete nor modify the object in any way, if desired, make a copy of ir with "duplicate()".

Implements mrpt::slam::CMetricMapBuilder.

◆ getCurrentMapPoints()

void mrpt::slam::CMetricMapBuilderICP::getCurrentMapPoints	(	std::vector< float > &	x,
		std::vector< float > &	y
	)

Returns the 2D points of current local map.

◆ getCurrentPoseEstimation()

mrpt::poses::CPose3DPDFPtr mrpt::slam::CMetricMapBuilderICP::getCurrentPoseEstimation ( ) const

virtual

Returns a copy of the current best pose estimation as a pose PDF.

Implements mrpt::slam::CMetricMapBuilder.

◆ initialize()

void mrpt::slam::CMetricMapBuilderICP::initialize	(	const mrpt::maps::CSimpleMap &	initialMap = `mrpt::maps::CSimpleMap()`,
		mrpt::poses::CPosePDF *	x0 = `NULL`
	)

virtual

Initialize the method, starting with a known location PDF "x0"(if supplied, set to NULL to left unmodified) and a given fixed, past map.

This method MUST be called if using the default constructor, after loading the configuration into ICP_options. In particular, TConfigParams::mapInitializers

Implements mrpt::slam::CMetricMapBuilder.

◆ leaveCriticalSection()

void mrpt::slam::CMetricMapBuilder::leaveCriticalSection ( )

inlineprotectedinherited

Leave critical section for map updating:

Definition at line 48 of file CMetricMapBuilder.h.

References mrpt::synch::CCriticalSection::leave().

◆ loadCurrentMapFromFile()

void mrpt::slam::CMetricMapBuilder::loadCurrentMapFromFile ( const std::string & fileName )

inherited

Load map (mrpt::maps::CSimpleMap) from a ".simplemap" file.

◆ printf_debug()

static void mrpt::utils::CDebugOutputCapable::printf_debug	(	const char *	frmt,
			...
	)

staticinherited

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

Referenced by mrpt::math::CLevenbergMarquardtTempl< VECTORTYPE, USERPARAM >::execute().

◆ processActionObservation()

void mrpt::slam::CMetricMapBuilderICP::processActionObservation	(	mrpt::obs::CActionCollection &	action,
		mrpt::obs::CSensoryFrame &	in_SF
	)

virtual

Appends a new action and observations to update this map: See the description of the class at the top of this page to see a more complete description.

Parameters

action	The estimation of the incremental pose change in the robot pose.
in_SF	The set of observations that robot senses at the new pose. See params in CMetricMapBuilder::options and CMetricMapBuilderICP::ICP_options

See also: processObservation

Implements mrpt::slam::CMetricMapBuilder.

◆ processObservation()

void mrpt::slam::CMetricMapBuilderICP::processObservation ( const mrpt::obs::CObservationPtr & obs )

The main method of this class: Process one odometry or sensor observation.

The new entry point of the algorithm (the old one was processActionObservation, which now is a wrapper to this method). See params in CMetricMapBuilder::options and CMetricMapBuilderICP::ICP_options

◆ resetRobotDisplacementCounters()

void mrpt::slam::CMetricMapBuilderICP::resetRobotDisplacementCounters ( const mrpt::poses::CPose2D & new_pose )

private

◆ saveCurrentEstimationToImage()

void mrpt::slam::CMetricMapBuilderICP::saveCurrentEstimationToImage	(	const std::string &	file,
		bool	formatEMF_BMP = `true`
	)

virtual

A useful method for debugging: the current map (and/or poses) estimation is dumped to an image file.

Parameters

file	The output file name
formatEMF_BMP	Output format = true:EMF, false:BMP

Implements mrpt::slam::CMetricMapBuilder.

◆ saveCurrentMapToFile()

void mrpt::slam::CMetricMapBuilder::saveCurrentMapToFile	(	const std::string &	fileName,
		bool	compressGZ = `true`
	)		const

inherited

Save map (mrpt::maps::CSimpleMap) to a ".simplemap" file.

◆ setCurrentMapFile()

void mrpt::slam::CMetricMapBuilderICP::setCurrentMapFile ( const char * mapFile )

Sets the "current map file", thus that map will be loaded if it exists or a new one will be created if it does not, and the updated map will be save to that file when destroying the object.

Member Data Documentation

◆ critZoneChangingMap

synch::CCriticalSection mrpt::slam::CMetricMapBuilder::critZoneChangingMap

protectedinherited

Critical zones.

Definition at line 40 of file CMetricMapBuilder.h.

◆ currentMapFile

std::string mrpt::slam::CMetricMapBuilderICP::currentMapFile

private

Current map file.

Definition at line 133 of file CMetricMapBuilderICP.h.

◆ ICP_options

TConfigParams mrpt::slam::CMetricMapBuilderICP::ICP_options

Options for the ICP-SLAM application.

See also: ICP_params

Definition at line 65 of file CMetricMapBuilderICP.h.

◆ ICP_params

CICP::TConfigParams mrpt::slam::CMetricMapBuilderICP::ICP_params

Options for the ICP algorithm itself.

See also: ICP_options

Definition at line 66 of file CMetricMapBuilderICP.h.

◆ m_auxAccumOdometry

mrpt::poses::CPose2D mrpt::slam::CMetricMapBuilderICP::m_auxAccumOdometry

private

Definition at line 142 of file CMetricMapBuilderICP.h.

◆ m_distSinceLastICP

TDist mrpt::slam::CMetricMapBuilderICP::m_distSinceLastICP

private

Definition at line 155 of file CMetricMapBuilderICP.h.

◆ m_distSinceLastInsertion

mrpt::aligned_containers<std::string,TDist>::map_t mrpt::slam::CMetricMapBuilderICP::m_distSinceLastInsertion

private

Indexed by sensor label.

Definition at line 156 of file CMetricMapBuilderICP.h.

◆ m_estRobotPath

std::deque<mrpt::math::TPose2D> mrpt::slam::CMetricMapBuilderICP::m_estRobotPath

private

The estimated robot path:

Definition at line 141 of file CMetricMapBuilderICP.h.

◆ m_lastPoseEst

mrpt::poses::CRobot2DPoseEstimator mrpt::slam::CMetricMapBuilderICP::m_lastPoseEst

private

The pose estimation by the alignment algorithm (ICP).

Last pose estimation (Mean)

Definition at line 136 of file CMetricMapBuilderICP.h.

◆ m_lastPoseEst_cov

mrpt::math::CMatrixDouble33 mrpt::slam::CMetricMapBuilderICP::m_lastPoseEst_cov

private

Last pose estimation (covariance)

Definition at line 137 of file CMetricMapBuilderICP.h.

◆ m_there_has_been_an_odometry

bool mrpt::slam::CMetricMapBuilderICP::m_there_has_been_an_odometry

private

Definition at line 157 of file CMetricMapBuilderICP.h.

◆ metricMap

mrpt::maps::CMultiMetricMap mrpt::slam::CMetricMapBuilderICP::metricMap

private

The metric map representation as a points map:

Definition at line 130 of file CMetricMapBuilderICP.h.

◆ options

TOptions mrpt::slam::CMetricMapBuilder::options

inherited

Definition at line 131 of file CMetricMapBuilder.h.

◆ SF_Poses_seq

mrpt::maps::CSimpleMap mrpt::slam::CMetricMapBuilderICP::SF_Poses_seq

private

The set of observations that leads to current map:

Definition at line 127 of file CMetricMapBuilderICP.h.

Detailed Description

Classes

Public Member Functions

Static Public Member Functions

Public Attributes

Protected Member Functions

Protected Attributes

Private Member Functions

Private Attributes

Constructor & Destructor Documentation

◆ CMetricMapBuilderICP()

◆ ~CMetricMapBuilderICP()

Member Function Documentation

◆ accumulateRobotDisplacementCounters()

◆ clear()

◆ enableMapUpdating()

◆ enterCriticalSection()

◆ getCurrentlyBuiltMap()

◆ getCurrentlyBuiltMapSize()

◆ getCurrentlyBuiltMetricMap()

◆ getCurrentMapPoints()

◆ getCurrentPoseEstimation()

◆ initialize()

◆ leaveCriticalSection()

◆ loadCurrentMapFromFile()

◆ printf_debug()

◆ processActionObservation()

◆ processObservation()

◆ resetRobotDisplacementCounters()

◆ saveCurrentEstimationToImage()

◆ saveCurrentMapToFile()

◆ setCurrentMapFile()

Member Data Documentation

◆ critZoneChangingMap

◆ currentMapFile

◆ ICP_options

◆ ICP_params

◆ m_auxAccumOdometry

◆ m_distSinceLastICP

◆ m_distSinceLastInsertion

◆ m_estRobotPath

◆ m_lastPoseEst

◆ m_lastPoseEst_cov

◆ m_there_has_been_an_odometry

◆ metricMap

◆ options

◆ SF_Poses_seq