Detailed Description

Several implementations of ICP (Iterative closest point) algorithms for aligning two point maps or a point map wrt a grid map.

CICP::AlignPDF() or CICP::Align() are the two main entry points of the algorithm.

To choose among existing ICP algorithms or customizing their parameters, see CICP::TConfigParams and the member options.

There exists an extension of the original ICP algorithm that provides multihypotheses-support for the correspondences, and which generates a Sum-of-Gaussians (SOG) PDF as output. See scanmatching::robustRigidTransformation.

For further details on the implemented methods, check the web: http://www.mrpt.org/Iterative_Closest_Point_(ICP)_and_other_matching_algorithms

For a paper explaining some of the basic equations, see for example: J. Martinez, J. Gonzalez, J. Morales, A. Mandow, A. Garcia-Cerezo, "Genetic and ICP Laser Point Matching for 2D Mobile Robot Motion Estimation", Journal of Field Robotics, vol. 23, no. 1, 2006. ( http://babel.isa.uma.es/~jlblanco/papers/martinez2006gil.pdf )

See also:: CMetricMapsAlignmentAlgorithm

#include <mrpt/slam/CICP.h>

Inheritance diagram for mrpt::slam::CICP:

[legend]

List of all members.

Classes
class	TConfigParams
	The ICP algorithm configuration data. More...
struct	TReturnInfo
	The ICP algorithm return information. More...
Public Member Functions
	CICP ()
	Constructor with the default options.
	CICP (const TConfigParams &icpParams)
	Constructor that directly set the ICP params from a given struct.
virtual	~CICP ()
	Destructor.
CPosePDFPtr	AlignPDF (const CMetricMap m1, const CMetricMap m2, const CPosePDFGaussian &initialEstimationPDF, float runningTime=NULL, void info=NULL)
	An implementation of CMetricMapsAlignmentAlgorithm for the case of a point maps and a occupancy grid/point map.
CPose3DPDFPtr	Align3DPDF (const CMetricMap m1, const CMetricMap m2, const CPose3DPDFGaussian &initialEstimationPDF, float runningTime=NULL, void info=NULL)
	Align a pair of metric maps, aligning the full 6D pose.
CPosePDFPtr	Align (const CMetricMap m1, const CMetricMap m2, const CPose2D &grossEst, float runningTime=NULL, void info=NULL)
	The method for aligning a pair of metric maps, aligning only 2D + orientation.
CPose3DPDFPtr	Align3D (const CMetricMap m1, const CMetricMap m2, const CPose3D &grossEst, float runningTime=NULL, void info=NULL)
	The method for aligning a pair of metric maps, aligning the full 6D pose.
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	options
	The options employed by the ICP align.
Protected Member Functions
float	kernel (const float &x2, const float &rho2)
	Computes: $K(x^2) = \frac{x^2}{x^2+\rho^2}$ or just return the input if options.useKernel = false.
CPosePDFPtr	ICP_Method_Classic (const CMetricMap m1, const CMetricMap m2, const CPosePDFGaussian &initialEstimationPDF, TReturnInfo &outInfo)
	The internal method implementing CICP::AlignPDF when options.ICP_algorithm is icpClassic.
CPosePDFPtr	ICP_Method_LM (const CMetricMap m1, const CMetricMap m2, const CPosePDFGaussian &initialEstimationPDF, TReturnInfo &outInfo)
	The internal method implementing CICP::AlignPDF when options.ICP_algorithm is icpLevenbergMarquardt.
CPosePDFPtr	ICP_Method_IKF (const CMetricMap m1, const CMetricMap m2, const CPosePDFGaussian &initialEstimationPDF, TReturnInfo &outInfo)
	The internal method implementing CICP::AlignPDF when options.ICP_algorithm is icpIKF.
CPose3DPDFPtr	ICP3D_Method_Classic (const CMetricMap m1, const CMetricMap m2, const CPose3DPDFGaussian &initialEstimationPDF, TReturnInfo &outInfo)
	The internal method implementing CICP::Align3DPDF when options.ICP_algorithm is icpClassic.

Constructor & Destructor Documentation

mrpt::slam::CICP::CICP ( ) [inline]

Constructor with the default options.

Definition at line 167 of file CICP.h.

mrpt::slam::CICP::CICP ( const TConfigParams & icpParams ) [inline]

Constructor that directly set the ICP params from a given struct.

See also:: options

Definition at line 169 of file CICP.h.

virtual mrpt::slam::CICP::~CICP ( ) [inline, virtual]

Destructor.

Definition at line 171 of file CICP.h.

Member Function Documentation

CPosePDFPtr mrpt::slam::CMetricMapsAlignmentAlgorithm::Align	(	const CMetricMap *	m1,
		const CMetricMap *	m2,
		const CPose2D &	grossEst,
		float *	runningTime = `NULL`,
		void *	info = `NULL`
	)		`[inherited]`

The method for aligning a pair of metric maps, aligning only 2D + orientation.

The meaning of some parameters and the kind of the maps to be aligned are implementation dependant, so look into the derived classes for instructions. The target is to find a PDF for the pose displacement between maps, thus the pose of m2 relative to m1. This pose is returned as a PDF rather than a single value.

Parameters:

m1	[IN] The first map
m2	[IN] The second map. The pose of this map respect to m1 is to be estimated.
grossEst	[IN] An initial gross estimation for the displacement. If a given algorithm doesn't need it, set to `CPose2D(0,0,0)` for example.
runningTime	[OUT] A pointer to a container for obtaining the algorithm running time in seconds, or NULL if you don't need it.
info	[OUT] See derived classes for details, or NULL if it isn't needed.

Returns:: A smart pointer to the output estimated pose PDF.

See also:: CICP

CPose3DPDFPtr mrpt::slam::CMetricMapsAlignmentAlgorithm::Align3D	(	const CMetricMap *	m1,
		const CMetricMap *	m2,
		const CPose3D &	grossEst,
		float *	runningTime = `NULL`,
		void *	info = `NULL`
	)		`[inherited]`

The method for aligning a pair of metric maps, aligning the full 6D pose.

The meaning of some parameters and the kind of the maps to be aligned are implementation dependant, so look into the derived classes for instructions. The target is to find a PDF for the pose displacement between maps, thus the pose of m2 relative to m1. This pose is returned as a PDF rather than a single value.

Parameters:

m1	[IN] The first map
m2	[IN] The second map. The pose of this map respect to m1 is to be estimated.
grossEst	[IN] An initial gross estimation for the displacement. If a given algorithm doesn't need it, set to `CPose3D(0,0,0)` for example.
runningTime	[OUT] A pointer to a container for obtaining the algorithm running time in seconds, or NULL if you don't need it.
info	[OUT] See derived classes for details, or NULL if it isn't needed.

Returns:: A smart pointer to the output estimated pose PDF.

See also:: CICP

CPose3DPDFPtr mrpt::slam::CICP::Align3DPDF	(	const CMetricMap *	m1,
		const CMetricMap *	m2,
		const CPose3DPDFGaussian &	initialEstimationPDF,
		float *	runningTime = `NULL`,
		void *	info = `NULL`
	)		`[virtual]`

Align a pair of metric maps, aligning the full 6D pose.

The meaning of some parameters are implementation dependant, so look at the derived classes for more details. The goal is to find a PDF for the pose displacement between maps, that is, the pose of m2 relative to m1. This pose is returned as a PDF rather than a single value.

Note:: This method can be configurated with a "options" structure in the implementation classes.

Parameters:

m1	[IN] The first map (MUST BE A COccupancyGridMap2D derived class)
m2	[IN] The second map. (MUST BE A CPointsMap derived class) The pose of this map respect to m1 is to be estimated.
initialEstimationPDF	[IN] An initial gross estimation for the displacement.
runningTime	[OUT] A pointer to a container for obtaining the algorithm running time in seconds, or NULL if you don't need it.
info	[OUT] See derived classes for details, or NULL if it isn't needed.

Returns:: A smart pointer to the output estimated pose PDF.

See also:: CICP

Implements mrpt::slam::CMetricMapsAlignmentAlgorithm.

CPosePDFPtr mrpt::slam::CICP::AlignPDF	(	const CMetricMap *	m1,
		const CMetricMap *	m2,
		const CPosePDFGaussian &	initialEstimationPDF,
		float *	runningTime = `NULL`,
		void *	info = `NULL`
	)		`[virtual]`

An implementation of CMetricMapsAlignmentAlgorithm for the case of a point maps and a occupancy grid/point map.

This method computes the PDF of the displacement (relative pose) between two maps: the relative pose of m2 with respect to m1. This pose is returned as a PDF rather than a single value.

Note:: This method can be configurated with "CICP::options"; The output PDF is a CPosePDFGaussian if "doRANSAC=false", or a CPosePDFSOG otherwise.

Parameters:

m1	[IN] The first map (CAN BE A mrpt::poses::CPointsMap derived class or a mrpt::slam::COccupancyGrid2D class)
m2	[IN] The second map. (MUST BE A mrpt::poses::CPointsMap derived class)The pose of this map respect to m1 is to be estimated.
initialEstimationPDF	[IN] An initial gross estimation for the displacement.
runningTime	[OUT] A pointer to a container for obtaining the algorithm running time in seconds, or NULL if you don't need it.
info	[OUT] A pointer to a CICP::TReturnInfo, or NULL if it isn't needed.

Returns:: A smart pointer to the output estimated pose PDF.

See also:: CMetricMapsAlignmentAlgorithm, CICP::options, CICP::TReturnInfo

Implements mrpt::slam::CMetricMapsAlignmentAlgorithm.

CPose3DPDFPtr mrpt::slam::CICP::ICP3D_Method_Classic	(	const CMetricMap *	m1,
		const CMetricMap *	m2,
		const CPose3DPDFGaussian &	initialEstimationPDF,
		TReturnInfo &	outInfo
	)		`[protected]`

The internal method implementing CICP::Align3DPDF when options.ICP_algorithm is icpClassic.

CPosePDFPtr mrpt::slam::CICP::ICP_Method_Classic	(	const CMetricMap *	m1,
		const CMetricMap *	m2,
		const CPosePDFGaussian &	initialEstimationPDF,
		TReturnInfo &	outInfo
	)		`[protected]`

The internal method implementing CICP::AlignPDF when options.ICP_algorithm is icpClassic.

CPosePDFPtr mrpt::slam::CICP::ICP_Method_IKF	(	const CMetricMap *	m1,
		const CMetricMap *	m2,
		const CPosePDFGaussian &	initialEstimationPDF,
		TReturnInfo &	outInfo
	)		`[protected]`

The internal method implementing CICP::AlignPDF when options.ICP_algorithm is icpIKF.

CPosePDFPtr mrpt::slam::CICP::ICP_Method_LM	(	const CMetricMap *	m1,
		const CMetricMap *	m2,
		const CPosePDFGaussian &	initialEstimationPDF,
		TReturnInfo &	outInfo
	)		`[protected]`

The internal method implementing CICP::AlignPDF when options.ICP_algorithm is icpLevenbergMarquardt.

float mrpt::slam::CICP::kernel	(	const float &	x2,
		const float &	rho2
	)		`[protected]`

Computes:

$K(x^2) = \frac{x^2}{x^2+\rho^2}$

or just return the input if options.useKernel = false.

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

Member Data Documentation

TConfigParams mrpt::slam::CICP::options

The options employed by the ICP align.

Definition at line 163 of file CICP.h.

Detailed Description

Classes

Public Member Functions

Static Public Member Functions

Public Attributes

Protected Member Functions

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation