Main MRPT website > C++ reference for MRPT 1.9.9
List of all members | Classes | Public Member Functions | Public Attributes | Protected Member Functions
mrpt::slam::CICP Class Reference

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 mrpt::tfest::se2_l2_robust()

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, "Mobile robot motion estimation by 2D scan matching with genetic and iterative closest point algorithms", Journal of Field Robotics, vol. 23, no. 1, pp. 21-34, 2006. ( http://babel.isa.uma.es/~jlblanco/papers/martinez2006gil.pdf )

See also
CMetricMapsAlignmentAlgorithm

Definition at line 67 of file CICP.h.

#include <mrpt/slam/CICP.h>

Inheritance diagram for mrpt::slam::CICP:
Inheritance graph

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. More...
 
 CICP (const TConfigParams &icpParams)
 Constructor that directly set the ICP params from a given struct. More...
 
virtual ~CICP ()
 Destructor. More...
 
mrpt::poses::CPosePDF::Ptr AlignPDF (const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPosePDFGaussian &initialEstimationPDF, float *runningTime=nullptr, void *info=nullptr)
 An implementation of CMetricMapsAlignmentAlgorithm for the case of a point maps and a occupancy grid/point map. More...
 
mrpt::poses::CPose3DPDF::Ptr Align3DPDF (const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose3DPDFGaussian &initialEstimationPDF, float *runningTime=nullptr, void *info=nullptr)
 The virtual method for aligning a pair of metric maps, aligning the full 6D pose. More...
 
mrpt::poses::CPosePDF::Ptr Align (const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose2D &grossEst, float *runningTime=nullptr, void *info=nullptr)
 The method for aligning a pair of metric maps, aligning only 2D + orientation. More...
 
mrpt::poses::CPose3DPDF::Ptr Align3D (const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose3D &grossEst, float *runningTime=nullptr, void *info=nullptr)
 The method for aligning a pair of metric maps, aligning the full 6D pose. More...
 

Public Attributes

TConfigParams options
 The options employed by the ICP align. More...
 

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

 
mrpt::poses::CPosePDF::Ptr ICP_Method_Classic (const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPosePDFGaussian &initialEstimationPDF, TReturnInfo &outInfo)
 
mrpt::poses::CPosePDF::Ptr ICP_Method_LM (const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPosePDFGaussian &initialEstimationPDF, TReturnInfo &outInfo)
 
mrpt::poses::CPose3DPDF::Ptr ICP3D_Method_Classic (const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose3DPDFGaussian &initialEstimationPDF, TReturnInfo &outInfo)
 

Constructor & Destructor Documentation

◆ CICP() [1/2]

mrpt::slam::CICP::CICP ( )
inline

Constructor with the default options.

Definition at line 188 of file CICP.h.

◆ CICP() [2/2]

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 191 of file CICP.h.

◆ ~CICP()

virtual mrpt::slam::CICP::~CICP ( )
inlinevirtual

Destructor.

Definition at line 193 of file CICP.h.

Member Function Documentation

◆ Align()

CPosePDF::Ptr CMetricMapsAlignmentAlgorithm::Align ( const mrpt::maps::CMetricMap m1,
const mrpt::maps::CMetricMap m2,
const mrpt::poses::CPose2D grossEst,
float *  runningTime = nullptr,
void info = nullptr 
)
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 nullptr if you don't need it.
info[OUT] See derived classes for details, or nullptr if it isn't needed.
Returns
A smart pointer to the output estimated pose PDF.
See also
CICP

Definition at line 26 of file CMetricMapsAlignmentAlgorithm.cpp.

Referenced by ICPTests::align2scans(), mrpt::slam::CGridMapAligner::AlignPDF_robustMatch(), mrpt::maps::CMultiMetricMapPDF::prediction_and_update< mrpt::slam::OptimalProposal >(), mrpt::hmtslam::CTopLCDetector_GridMatching::computeTopologicalObservationModel(), mrpt::hmtslam::CLocalMetricHypothesis::prediction_and_update(), and mrpt::slam::CMetricMapBuilderICP::processObservation().

Here is the caller graph for this function:

◆ Align3D()

CPose3DPDF::Ptr CMetricMapsAlignmentAlgorithm::Align3D ( const mrpt::maps::CMetricMap m1,
const mrpt::maps::CMetricMap m2,
const mrpt::poses::CPose3D grossEst,
float *  runningTime = nullptr,
void info = nullptr 
)
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 nullptr if you don't need it.
info[OUT] See derived classes for details, or nullptr if it isn't needed.
Returns
A smart pointer to the output estimated pose PDF.
See also
CICP

Definition at line 37 of file CMetricMapsAlignmentAlgorithm.cpp.

References mrpt::poses::CPose3DPDFGaussian::mean.

Referenced by TEST_F().

Here is the caller graph for this function:

◆ Align3DPDF()

CPose3DPDF::Ptr CICP::Align3DPDF ( const mrpt::maps::CMetricMap m1,
const mrpt::maps::CMetricMap m2,
const mrpt::poses::CPose3DPDFGaussian initialEstimationPDF,
float *  runningTime = nullptr,
void info = nullptr 
)
virtual

The virtual method for aligning 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 nullptr if you don't need it.
info[OUT] See derived classes for details, or nullptr if it isn't needed.
Returns
A smart pointer to the output estimated pose PDF.
See also
CICP

Implements mrpt::slam::CMetricMapsAlignmentAlgorithm.

Definition at line 1043 of file CICP.cpp.

References ICP3D_Method_Classic(), mrpt::slam::CICP::TConfigParams::ICP_algorithm, mrpt::slam::icpClassic, mrpt::slam::icpLevenbergMarquardt, MRPT_END, MRPT_START, MRPT_TODO, options, THROW_EXCEPTION, and THROW_EXCEPTION_FMT.

Here is the call graph for this function:

◆ AlignPDF()

CPosePDF::Ptr CICP::AlignPDF ( const mrpt::maps::CMetricMap m1,
const mrpt::maps::CMetricMap m2,
const mrpt::poses::CPosePDFGaussian initialEstimationPDF,
float *  runningTime = nullptr,
void info = nullptr 
)
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 nullptr if you don't need it.
info[OUT] A pointer to a CICP::TReturnInfo, or nullptr 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.

Definition at line 58 of file CICP.cpp.

References mrpt::slam::icpClassic, mrpt::slam::icpLevenbergMarquardt, MRPT_END, MRPT_START, and THROW_EXCEPTION_FMT.

◆ ICP3D_Method_Classic()

CPose3DPDF::Ptr CICP::ICP3D_Method_Classic ( const mrpt::maps::CMetricMap m1,
const mrpt::maps::CMetricMap m2,
const mrpt::poses::CPose3DPDFGaussian initialEstimationPDF,
TReturnInfo outInfo 
)
protected

Definition at line 1087 of file CICP.cpp.

References mrpt::slam::CICP::TConfigParams::ALFA, mrpt::maps::TMatchingParams::angularDistPivotPoint, ASSERT_, CLASS_ID, mrpt::maps::TMatchingExtraResults::correspondencesRatio, mrpt::slam::CICP::TConfigParams::corresponding_points_decimation, mrpt::maps::TMatchingParams::decimation_other_map_points, mrpt::utils::TRuntimeClassId::derivedFrom(), mrpt::maps::CMetricMap::determineMatching3D(), mrpt::maps::CMetricMap::GetRuntimeClass(), mrpt::slam::CICP::TReturnInfo::goodness, mrpt::maps::TMatchingParams::maxAngularDistForCorrespondence, mrpt::maps::TMatchingParams::maxDistForCorrespondence, mrpt::slam::CICP::TConfigParams::maxIterations, mrpt::poses::CPose3DPDFGaussian::mean, mrpt::slam::CICP::TConfigParams::minAbsStep_rot, mrpt::slam::CICP::TConfigParams::minAbsStep_trans, MRPT_END, MRPT_START, mrpt::slam::CICP::TReturnInfo::nIterations, mrpt::maps::TMatchingParams::offset_other_map_points, mrpt::slam::CICP::TConfigParams::onlyClosestCorrespondences, mrpt::maps::TMatchingParams::onlyKeepTheClosest, mrpt::maps::TMatchingParams::onlyUniqueRobust, mrpt::slam::CICP::TConfigParams::onlyUniqueRobust, options, mrpt::poses::CPose3D::pitch(), mrpt::slam::CICP::TReturnInfo::quality, mrpt::poses::CPose3D::roll(), mrpt::tfest::se3_l2(), mrpt::slam::CICP::TConfigParams::skip_cov_calculation, mrpt::slam::CICP::TConfigParams::smallestThresholdDist, mrpt::slam::CICP::TConfigParams::thresholdAng, mrpt::slam::CICP::TConfigParams::thresholdDist, mrpt::math::wrapToPi(), mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::x(), mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::y(), and mrpt::poses::CPose3D::yaw().

Referenced by Align3DPDF().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ ICP_Method_Classic()

CPosePDF::Ptr CICP::ICP_Method_Classic ( const mrpt::maps::CMetricMap m1,
const mrpt::maps::CMetricMap m2,
const mrpt::poses::CPosePDFGaussian initialEstimationPDF,
TReturnInfo outInfo 
)
protected

Definition at line 282 of file CICP.cpp.

References mrpt::slam::CICP::TConfigParams::ALFA, mrpt::maps::TMatchingParams::angularDistPivotPoint, ASSERT_, mrpt::maps::TMatchingExtraResults::correspondencesRatio, mrpt::slam::CICP::TConfigParams::corresponding_points_decimation, mrpt::slam::CICP::TConfigParams::covariance_varPoints, mrpt::maps::TMatchingParams::decimation_other_map_points, mrpt::maps::CMetricMap::determineMatching2D(), mrpt::slam::CICP::TConfigParams::doRANSAC, mrpt::slam::CICP::TReturnInfo::goodness, mrpt::slam::CICP::TConfigParams::ICP_covariance_method, mrpt::slam::icpCovFiniteDifferences, mrpt::slam::icpCovLinealMSE, mrpt::maps::CMetricMap::isEmpty(), kernel(), mrpt::slam::CICP::TConfigParams::kernel_rho, mrpt::maps::TMatchingParams::maxAngularDistForCorrespondence, mrpt::maps::TMatchingParams::maxDistForCorrespondence, mrpt::slam::CICP::TConfigParams::maxIterations, mrpt::poses::CPosePDFGaussian::mean, mrpt::slam::CICP::TConfigParams::minAbsStep_rot, mrpt::slam::CICP::TConfigParams::minAbsStep_trans, MRPT_END, MRPT_START, mrpt::slam::CICP::TReturnInfo::nIterations, mrpt::slam::CICP::TConfigParams::normalizationStd, mrpt::maps::TMatchingParams::offset_other_map_points, mrpt::slam::CICP::TConfigParams::onlyClosestCorrespondences, mrpt::maps::TMatchingParams::onlyKeepTheClosest, mrpt::maps::TMatchingParams::onlyUniqueRobust, mrpt::slam::CICP::TConfigParams::onlyUniqueRobust, options, mrpt::poses::CPose2D::phi(), mrpt::math::TPose2D::phi, mrpt::slam::CICP::TReturnInfo::quality, mrpt::slam::CICP::TConfigParams::ransac_fuseByCorrsMatch, mrpt::slam::CICP::TConfigParams::ransac_fuseMaxDiffPhi, mrpt::slam::CICP::TConfigParams::ransac_fuseMaxDiffXY, mrpt::slam::CICP::TConfigParams::ransac_mahalanobisDistanceThreshold, mrpt::slam::CICP::TConfigParams::ransac_maxSetSize, mrpt::slam::CICP::TConfigParams::ransac_minSetSize, mrpt::slam::CICP::TConfigParams::ransac_nSimulations, mrpt::tfest::se2_l2(), mrpt::tfest::se2_l2_robust(), mrpt::slam::CICP::TConfigParams::skip_cov_calculation, mrpt::slam::CICP::TConfigParams::skip_quality_calculation, mrpt::slam::CICP::TConfigParams::smallestThresholdDist, mrpt::math::square(), mrpt::slam::CICP::TConfigParams::thresholdAng, mrpt::slam::CICP::TConfigParams::thresholdDist, THROW_EXCEPTION_FMT, mrpt::tfest::TSE2RobustResult::transformation, mrpt::math::wrapToPi(), mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::x(), mrpt::math::TPose2D::x, mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::x_incr(), mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::y(), mrpt::math::TPose2D::y, and mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::y_incr().

Here is the call graph for this function:

◆ ICP_Method_LM()

CPosePDF::Ptr CICP::ICP_Method_LM ( const mrpt::maps::CMetricMap m1,
const mrpt::maps::CMetricMap m2,
const mrpt::poses::CPosePDFGaussian initialEstimationPDF,
TReturnInfo outInfo 
)
protected

Definition at line 654 of file CICP.cpp.

References mrpt::slam::CICP::TConfigParams::ALFA, mrpt::maps::TMatchingParams::angularDistPivotPoint, ASSERT_, mrpt::slam::CICP::TConfigParams::Axy_aprox_derivatives, CLASS_ID, mrpt::math::closestSquareDistanceFromPointToLine(), mrpt::maps::TMatchingExtraResults::correspondencesRatio, mrpt::slam::CICP::TConfigParams::corresponding_points_decimation, mrpt::maps::TMatchingParams::decimation_other_map_points, mrpt::utils::TRuntimeClassId::derivedFrom(), mrpt::maps::CMetricMap::GetRuntimeClass(), mrpt::slam::CICP::TReturnInfo::goodness, mrpt::maps::CMetricMap::isEmpty(), kernel(), mrpt::slam::CICP::TConfigParams::kernel_rho, mrpt::slam::CICP::TConfigParams::LM_initial_lambda, mrpt::maps::TMatchingParams::maxAngularDistForCorrespondence, mrpt::maps::TMatchingParams::maxDistForCorrespondence, mrpt::slam::CICP::TConfigParams::maxIterations, mrpt::poses::CPosePDFGaussian::mean, mrpt::slam::CICP::TConfigParams::minAbsStep_rot, mrpt::slam::CICP::TConfigParams::minAbsStep_trans, MRPT_END, MRPT_START, mrpt::slam::CICP::TReturnInfo::nIterations, mrpt::maps::TMatchingParams::offset_other_map_points, mrpt::slam::CICP::TConfigParams::onlyClosestCorrespondences, mrpt::maps::TMatchingParams::onlyKeepTheClosest, mrpt::maps::TMatchingParams::onlyUniqueRobust, mrpt::slam::CICP::TConfigParams::onlyUniqueRobust, mrpt::utils::operator<<(), options, mrpt::system::pause(), mrpt::poses::CPose2D::phi(), mrpt::slam::CICP::TReturnInfo::quality, mrpt::slam::CICP::TConfigParams::smallestThresholdDist, mrpt::math::square(), mrpt::utils::TMatchingPairList::squareErrorVector(), mrpt::math::sum(), mrpt::slam::CICP::TConfigParams::thresholdAng, mrpt::slam::CICP::TConfigParams::thresholdDist, mrpt::math::wrapToPi(), mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::x(), and mrpt::poses::CPoseOrPoint< DERIVEDCLASS >::y().

Here is the call graph for this function:

◆ kernel()

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

Definition at line 272 of file CICP.cpp.

References options, and mrpt::slam::CICP::TConfigParams::use_kernel.

Referenced by ICP_Method_Classic(), and ICP_Method_LM().

Here is the caller graph for this function:

Member Data Documentation

◆ options

TConfigParams mrpt::slam::CICP::options



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019