Class CICP

Nested Relationships

Nested Types

Inheritance Relationships

Base Type

Class Documentation

class CICP : public mrpt::slam::CMetricMapsAlignmentAlgorithm

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: https://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

Note

3-D ICP: Only the icpClassic variant is implemented for 3-D point-cloud alignment (see Align3DPDF()). For production-quality, feature-rich 3-D ICP with support for different point-to-plane and point-to-line metrics, robust kernels, and multi-layer maps, use the independent mp2p_icp library (https://github.com/MOLAorg/mp2p_icp) which is the recommended replacement for 3-D scan matching in MRPT 3.

Public Functions

inline CICP()

Constructor with the default options

inline CICP(const TConfigParams &icpParams)

Constructor that directly set the ICP params from a given struct

See also

options

~CICP() override = default

Destructor

virtual mrpt::poses::CPosePDF::Ptr AlignPDF(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPosePDFGaussian &initialEstimationPDF, mrpt::optional_ref<TMetricMapAlignmentResult> outInfo = std::nullopt) override

See base method docs. This class offers an implementation for the case of “m1” being a point map and “m2” either an occupancy grid or a point map.

See also

CMetricMapsAlignmentAlgorithm, CICP::options, CICP::TReturnInfo

Note

This method can be configurated with “CICP::options”

Note

The output PDF is a CPosePDFGaussian if “doRANSAC=false”, or a CPosePDFSOG otherwise.

virtual mrpt::poses::CPose3DPDF::Ptr Align3DPDF(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose3DPDFGaussian &initialEstimationPDF, mrpt::optional_ref<TMetricMapAlignmentResult> outInfo = std::nullopt) override

Public Members

TConfigParams options

The options employed by the ICP align.

Protected Functions

float kernel(float x2, float rho2)

Computes:

\[ K(x^2) = \frac{x^2}{x^2+\rho^2} \]
or just return the input if options.useKernel = false.

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)
class TConfigParams : public mrpt::config::CLoadableOptions

The ICP algorithm configuration data

Algorithms selection

TICPAlgorithm ICP_algorithm = {icpClassic}

The algorithm to use (default: icpClassic). See https://www.mrpt.org/tutorials/programming/scan-matching-and-icp/ for details

TICPCovarianceMethod ICP_covariance_method = {icpCovFiniteDifferences}

The method to use for covariance estimation (Default: icpCovFiniteDifferences)

Correspondence-finding criteria

bool onlyUniqueRobust = {false}

Termination criteria

if this option is enabled only the closest correspondence for each reference point will be kept (default=false).

unsigned int maxIterations = {40}

Maximum number of iterations to run.

double minAbsStep_trans = {1e-6}

If the correction in all translation coordinates (X,Y,Z) is below this threshold (in meters), iterations are terminated (Default:1e-6)

double minAbsStep_rot = {1e-6}

If the correction in all rotation coordinates (yaw,pitch,roll) is below this threshold (in radians), iterations are terminated (Default:1e-6)

RANSAC-step options for mrpt::tfest::se2_l2_robust() if \a

doRANSAC=true

unsigned int ransac_minSetSize = {3}
unsigned int ransac_maxSetSize = {20}
unsigned int ransac_nSimulations = {100}
double ransac_mahalanobisDistanceThreshold = {3.0}
double normalizationStd = {0.02}

RANSAC-step option: The standard deviation in X,Y of landmarks/points which are being matched (used to compute covariances in the SoG)

bool ransac_fuseByCorrsMatch = {true}
double ransac_fuseMaxDiffXY = {0.01}
double ransac_fuseMaxDiffPhi = {0.1 * M_PI / 180.0}

Public Functions

void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
void saveToConfigFile(mrpt::config::CConfigFileBase &c, const std::string &s) const override

Public Members

double thresholdDist = {0.75}

Initial threshold distance for two points to become a correspondence.

double thresholdAng = {0.15 * M_PI / 180.0}
double ALFA = {0.5}

The scale factor for thresholds every time convergence is achieved.

double smallestThresholdDist = {0.1}

The size for threshold such that iterations will stop, since it is considered precise enough.

double covariance_varPoints = {0.02 * 0.02}

This is the normalization constant \( \sigma^2_p \) that is used to scale the whole 3x3 covariance. This has a default value of \( (0.02)^2 \)

, that is, a 2cm sigma. See paper: J.L. Blanco, J. Gonzalez-Jimenez, J.A. Fernandez-Madrigal, “A Robust, Multi-Hypothesis Approach to Matching

Occupancy Grid Maps”, Robotica, vol. 31, no. 5, pp. 687-701, 2013.

bool doRANSAC = {false}

Perform a RANSAC step, mrpt::tfest::se2_l2_robust(), after the ICP convergence, to obtain a better estimation of the pose PDF.

double kernel_rho = {0.07}

Cauchy kernel rho, for estimating the optimal transformation covariance, in meters (default = 0.07m)

bool use_kernel = {true}

Whether to use kernel_rho to smooth distances, or use distances directly (default=true)

double Axy_aprox_derivatives = {0.05}

[LM method only] The size of the perturbance in x & y used to estimate the Jacobians of the square error (default=0.05)

double LM_initial_lambda = {1e-4}

[LM method only] The initial value of the lambda parameter in the LM method (default=1e-4)

bool skip_cov_calculation = {false}

Skip the computation of the covariance (saves some time) (default=false)

bool skip_quality_calculation = {true}

Skip the (sometimes) expensive evaluation of the term ‘quality’ at ICP output (Default=true)

uint32_t corresponding_points_decimation = {5}

Decimation of the point cloud being registered against the reference one (default=5) - set to 1 to have the older (MRPT <0.9.5) behavior of not approximating ICP by ignoring the correspondence of some points. The speed-up comes from a decimation of the number of KD-tree queries, the most expensive step in ICP

struct TReturnInfo : public mrpt::slam::TMetricMapAlignmentResult

The ICP algorithm return information

Public Functions

TReturnInfo() = default
virtual ~TReturnInfo() override = default

Public Members

unsigned int nIterations = 0

The number of executed iterations until convergence

double goodness = 0

A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences.

double quality = 0

A measure of the ‘quality’ of the local minimum of the sqr. error found by the method. Higher values are better. Low values will be found in ill-conditioned situations (e.g. a corridor)