MRPT logo

mrpt::slam::CICP Class Reference

Several implementations of ICP (Iterative closest point) algorithms for aligning two point maps. More...

#include <mrpt/slam/CICP.h>

Inheritance diagram for mrpt::slam::CICP:

mrpt::slam::CMetricMapsAlignmentAlgorithm mrpt::utils::CDebugOutputCapable

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

Public Attributes

mrpt::slam::CICP::TConfigParams options
 The ICP algorithm configuration data.

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.


Detailed Description

Several implementations of ICP (Iterative closest point) algorithms for aligning two point maps.

See CICP::AlignPDF for the entry point of the algorithm, and CICP::TConfigParams for all the parameters to the method. The algorithm has been exteneded with multihypotheses-support for the correspondences, which generates a Sum-of-Gaussians (SOG) PDF as output. See scan_matching::robustRigidTransformation.

For further details on the method, check the wiki: http://babel.isa.uma.es/mrpt/index.php/Scan_Matching_Algorithms

See also:
CMetricMapsAlignmentAlgorithm

Definition at line 66 of file CICP.h.


Constructor & Destructor Documentation

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

Definition at line 69 of file CICP.h.

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

Destructor.

Definition at line 74 of file CICP.h.


Member Function Documentation

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.


Member Data Documentation

The ICP algorithm configuration data.




Page generated by Doxygen 1.5.7.1 for MRPT 0.7.1 SVN: at Mon Aug 17 23:10:56 EDT 2009