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::slam::CSimpleMap" objects. The methods are thread-safe.

#include <mrpt/slam/CMetricMapBuilderICP.h>

Inheritance diagram for mrpt::slam::CMetricMapBuilderICP:

[legend]

List of all members.

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".
virtual	~CMetricMapBuilderICP ()
	Destructor:
void	initialize (const CSimpleMap &initialMap=CSimpleMap(), 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.
CPose3DPDFPtr	getCurrentPoseEstimation () const
	Returns a copy of the current best pose estimation as a pose PDF.
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.
void	processActionObservation (CActionCollection &action, 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.
void	processObservation (const CObservationPtr &obs)
	The main method of this class: Process one odometry or sensor observation.
void	getCurrentlyBuiltMap (CSimpleMap &out_map) const
	Fills "out_map" with the set of "poses"-"sensory-frames", thus the so far built map.
void	getCurrentMapPoints (std::vector< float > &x, std::vector< float > &y)
	Returns the 2D points of current local map.
CMultiMetricMap *	getCurrentlyBuiltMetricMap ()
	Returns the map built so far.
unsigned int	getCurrentlyBuiltMapSize ()
	Returns just how many sensory-frames are stored in the currently build map.
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.
void	clear ()
	Clear all elements of the maps, and reset localization to (0,0,0deg).
void	enableMapUpdating (bool enable)
	Enables or disables the map updating (default state is enabled)
void	loadCurrentMapFromFile (const std::string &fileName)
	Load map (mrpt::slam::CSimpleMap) from a ".simplemap" file.
void	saveCurrentMapToFile (const std::string &fileName, bool compressGZ=true) const
	Save map (mrpt::slam::CSimpleMap) to a ".simplemap" file.
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
TConfigParams	ICP_options
	Options for the ICP-SLAM application.
CICP::TConfigParams	ICP_params
	Options for the ICP algorithm itself.
TOptions	options
Protected Member Functions
void	enterCriticalSection ()
	Enter critical section for map updating:
void	leaveCriticalSection ()
	Leave critical section for map updating:
Protected Attributes
synch::CCriticalSection	critZoneChangingMap
	Critical zones.
Private Member Functions
void	accumulateRobotDisplacementCounters (const CPose2D &Apose)
Private Attributes
CSimpleMap	SF_Poses_seq
	The set of observations that leads to current map:
CMultiMetricMap	metricMap
	The metric map representation as a points map:
std::string	currentMapFile
	Current map file.
mrpt::poses::CRobot2DPoseEstimator	m_lastPoseEst
	The pose estimation by the alignment algorithm (ICP).
mrpt::math::CMatrixDouble33	m_lastPoseEst_cov
	Last pose estimation (covariance)
std::deque< mrpt::math::TPose2D >	m_estRobotPath
	The estimated robot path:
mrpt::poses::CPose2D	m_auxAccumOdometry
TDist	m_distSinceLastICP
std::map< std::string, TDist >	m_distSinceLastInsertion
	Indexed by sensor label.
bool	m_there_has_been_an_odometry

Constructor & Destructor Documentation

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

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

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

Destructor:

Member Function Documentation

void mrpt::slam::CMetricMapBuilderICP::accumulateRobotDisplacementCounters ( const CPose2D & Apose ) [private]

void mrpt::slam::CMetricMapBuilder::clear ( ) [inherited]

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

Reimplemented in mrpt::slam::CMetricMapBuilderRBPF.

void mrpt::slam::CMetricMapBuilder::enableMapUpdating ( bool enable ) [inline, inherited]

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

Definition at line 114 of file CMetricMapBuilder.h.

void mrpt::slam::CMetricMapBuilder::enterCriticalSection ( ) [inline, protected, inherited]

Enter critical section for map updating:

Definition at line 63 of file CMetricMapBuilder.h.

void mrpt::slam::CMetricMapBuilderICP::getCurrentlyBuiltMap ( 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.

unsigned int mrpt::slam::CMetricMapBuilderICP::getCurrentlyBuiltMapSize ( ) [virtual]

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

Implements mrpt::slam::CMetricMapBuilder.

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.

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

Returns the 2D points of current local map.

CPose3DPDFPtr mrpt::slam::CMetricMapBuilderICP::getCurrentPoseEstimation ( ) const [virtual]

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

Implements mrpt::slam::CMetricMapBuilder.

void mrpt::slam::CMetricMapBuilderICP::initialize	(	const CSimpleMap &	initialMap = `CSimpleMap()`,
		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.

void mrpt::slam::CMetricMapBuilder::leaveCriticalSection ( ) [inline, protected, inherited]

Leave critical section for map updating:

Definition at line 67 of file CMetricMapBuilder.h.

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

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

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::slam::CMetricMapBuilderICP::processActionObservation	(	CActionCollection &	action,
		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.

void mrpt::slam::CMetricMapBuilderICP::processObservation ( const 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

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.

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

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

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.