[mrpt-tfest]

Algorithms to find optimal transformations from sets of correspondences.

Library mrpt-tfest

This C++ library is part of MRPT and can be installed in Debian-based systems with:

sudo apt install libmrpt-tfest-dev

Read also how to import MRPT into your CMake scripts.

T rans f ormation est imation (tfest): This module provides functions in charge of solving the optimization problem of aligning a set of 2D or 3D corresponding points, estimating the optimal transformation between the two frames of reference.

Note that this does not include the related iterative ICP algorithm (see mrpt::slam::CICP), included in the library [mrpt-slam]

See list of all functions: mrpt::tfest

Library contents

// namespaces

namespace mrpt::tfest;
    namespace mrpt::tfest::internal;

// typedefs

typedef std::function<bool(const TPotentialMatch&)> mrpt::tfest::TFunctorCheckPotentialMatch;
typedef TMatchingPairTempl<float> mrpt::tfest::TMatchingPair;
typedef TMatchingPairTempl<double> mrpt::tfest::TMatchingPair_d;
typedef TMatchingPairListTempl<float> mrpt::tfest::TMatchingPairList;
typedef TMatchingPairListTempl<double> mrpt::tfest::TMatchingPairList_d;

// structs

template <typename T>
struct mrpt::tfest::TMatchingPairTempl;

struct mrpt::tfest::TPotentialMatch;
struct mrpt::tfest::TSE2RobustParams;
struct mrpt::tfest::TSE2RobustResult;
struct mrpt::tfest::TSE3RobustParams;
struct mrpt::tfest::TSE3RobustResult;

// classes

template <typename T>
class mrpt::tfest::TMatchingPairListTempl;

// global functions

bool mrpt::tfest::se2_l2(
    const mrpt::tfest::TMatchingPairList& in_correspondences,
    mrpt::math::TPose2D& out_transformation,
    mrpt::math::CMatrixDouble33* out_estimateCovariance = nullptr
    );

bool mrpt::tfest::se2_l2(const mrpt::tfest::TMatchingPairList& in_correspondences, mrpt::poses::CPosePDFGaussian& out_transformation);

bool mrpt::tfest::se2_l2_robust(
    const mrpt::tfest::TMatchingPairList& in_correspondences,
    const double in_normalizationStd,
    const TSE2RobustParams& in_ransac_params,
    TSE2RobustResult& out_results
    );

bool mrpt::tfest::se3_l2(
    const mrpt::tfest::TMatchingPairList& in_correspondences,
    mrpt::poses::CPose3DQuat& out_transform,
    double& out_scale,
    bool forceScaleToUnity = false
    );

bool mrpt::tfest::se3_l2(
    const mrpt::tfest::TMatchingPairList_d& in_correspondences,
    mrpt::poses::CPose3DQuat& out_transform,
    double& out_scale,
    bool forceScaleToUnity = false
    );

bool mrpt::tfest::se3_l2(
    const std::vector<mrpt::math::TPoint3D>& in_points_this,
    const std::vector<mrpt::math::TPoint3D>& in_points_other,
    mrpt::poses::CPose3DQuat& out_transform,
    double& out_scale,
    bool forceScaleToUnity = false
    );

bool mrpt::tfest::se3_l2_robust(const mrpt::tfest::TMatchingPairList& in_correspondences, const TSE3RobustParams& in_params, TSE3RobustResult& out_results);

template <typename T>
mrpt::serialization::CArchive& mrpt::tfest::operator << (
    mrpt::serialization::CArchive& out,
    const TMatchingPairTempl<T>& obj
    );

template <typename T>
mrpt::serialization::CArchive& mrpt::tfest::operator >> (
    mrpt::serialization::CArchive& in,
    TMatchingPairTempl<T>& obj
    );

template <typename T>
std::ostream& mrpt::tfest::operator << (
    std::ostream& o,
    const mrpt::tfest::TMatchingPairTempl<T>& pairs
    );

template <typename T>
bool mrpt::tfest::operator < (const TMatchingPairTempl<T>& a, const TMatchingPairTempl<T>& b);

template <typename T>
bool mrpt::tfest::operator == (const TMatchingPairTempl<T>& a, const TMatchingPairTempl<T>& b);

template <typename T>
bool mrpt::tfest::operator == (const TMatchingPairListTempl<T>& a, const TMatchingPairListTempl<T>& b);

Typedefs

typedef TMatchingPairTempl<float> mrpt::tfest::TMatchingPair

A structure for holding correspondences between two sets of points or points-like entities in 2D or 3D.

Using float to save space since large point clouds are likely stored in local coordinates using float.

See also:

TMatchingPair_d

typedef TMatchingPairTempl<double> mrpt::tfest::TMatchingPair_d

A structure for holding correspondences between two sets of points or points-like entities in 2D or 3D.

Using double for maximum accuracy when required.

See also:

TMatchingPair

typedef TMatchingPairListTempl<float> mrpt::tfest::TMatchingPairList

A list of TMatchingPair (T=float).

typedef TMatchingPairListTempl<double> mrpt::tfest::TMatchingPairList_d

A list of TMatchingPair (T=double).

Global Functions

bool mrpt::tfest::se2_l2(
    const mrpt::tfest::TMatchingPairList& in_correspondences,
    mrpt::math::TPose2D& out_transformation,
    mrpt::math::CMatrixDouble33* out_estimateCovariance = nullptr
    )

Least-squares (L2 norm) solution to finding the optimal SE(2) (x,y,yaw) between two reference frames.

The optimal transformation q fulfills \(p_{this} = q \oplus p_{other}\), that is, the transformation of frame other with respect to this.

Reference for covariance calculation: J.L. Blanco, J. Gonzalez-Jimenez, J.A. Fernandez-Madrigal, “A Robust, Multi-Hypothesis Approach to Matching Occupancy Grid Maps”, Robotica, 2013. http://dx.doi.org/10.1017/S0263574712000732

[New in MRPT 1.3.0] This function replaces mrpt::scanmatching::leastSquareErrorRigidTransformation()

This function is hand-optimized for SSE2 architectures (if SSE2 is enabled from CMake)

Parameters:

in_correspondences	The set of correspondences.
out_transformation	The pose that minimizes the mean-square-error between all the correspondences.
out_estimateCovariance	If provided (!=nullptr) this will contain on return a 3x3 covariance matrix with the NORMALIZED optimal estimate uncertainty. This matrix must be multiplied by \(\sigma^2_p\), the variance of matched points in \(x\) and \(y\) (see paper https://www.mrpt.org/Paper:Occupancy_Grid_Matching)

Returns:

True if there are at least two correspondences, or false if one or none, thus we cannot establish any correspondence.

See also:

robustRigidTransformation

se3_l2, se2_l2_robust

bool mrpt::tfest::se2_l2(const mrpt::tfest::TMatchingPairList& in_correspondences, mrpt::poses::CPosePDFGaussian& out_transformation)

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

bool mrpt::tfest::se2_l2_robust(
    const mrpt::tfest::TMatchingPairList& in_correspondences,
    const double in_normalizationStd,
    const TSE2RobustParams& in_ransac_params,
    TSE2RobustResult& out_results
    )

Robust least-squares (L2 norm) solution to finding the optimal SE(2) (x,y,yaw) between two reference frames.

This method implements a RANSAC-based robust estimation, returning a probability distribution over all the possibilities as a Sum of Gaussians.

The optimal transformation q fulfills \(p_{this} = q \oplus p_{other}\), that is, the transformation of frame other with respect to this.

The technique was described in the paper:

• J.L. Blanco, J. Gonzalez-Jimenez, J.A. Fernandez-Madrigal, “A Robust, Multi-Hypothesis Approach to Matching Occupancy Grid Maps”, Robotica, 2013. http://dx.doi.org/10.1017/S0263574712000732

This works as follows:

• Repeat “ransac_nSimulations” times:

  • Randomly pick TWO correspondences from the set “in_correspondences”.

  • Compute the associated rigid transformation.

  • For “ransac_maxSetSize” randomly selected correspondences, test for “consensus” with the current group:

    • If if is compatible (ransac_mahalanobisDistanceThreshold), grow the “consensus set”

    • If not, do not add it.

  For more details refer to the tutorial on scan matching methods. NOTE : Parameter ransac_maxSetSize should be set to in_correspondences.size() to make sure that every correspondence is tested for each random permutation.

[New in MRPT 1.3.0] This function replaces mrpt::scanmatching::robustRigidTransformation()

Parameters:

in_normalizationStd

The standard deviation (not variance) of landmarks/points/features being matched in X,Y. Used to normalize covariances returned as the SoG. (Refer to paper)

Returns:

True upon success, false if no subset was found with the minimum number of correspondences.

See also:

se3_l2, se2_l2_robust

bool mrpt::tfest::se3_l2(
    const mrpt::tfest::TMatchingPairList& in_correspondences,
    mrpt::poses::CPose3DQuat& out_transform,
    double& out_scale,
    bool forceScaleToUnity = false
    )

Least-squares (L2 norm) solution to finding the optimal SE(3) transform between two reference frames using the “quaternion” or Horn’s method:

• “Closed-form solution of absolute orientation using unit quaternions”, BKP Horn, Journal of the Optical Society of America, 1987.

The optimal transformation q fulfills \(p_{this} = q \oplus p_{other}\), that is, the transformation of frame other with respect to this.

[New in MRPT 1.3.0] This function replaces mrpt::scanmatching::leastSquareErrorRigidTransformation6DRANSAC() and mrpt::scanmatching::HornMethod()

Parameters:

in_correspondences	The coordinates of the input points for the two coordinate systems “this” and “other”
out_transform	The output transformation
out_scale	The computed scale of the optimal transformation (will be 1.0 for a perfectly rigid translation + rotation).
forceScaleToUnity	Whether or not force the scale employed to rotate the coordinate systems to one (rigid transformation)

See also:

se2_l2, se3_l2_robust

bool mrpt::tfest::se3_l2(
    const mrpt::tfest::TMatchingPairList_d& in_correspondences,
    mrpt::poses::CPose3DQuat& out_transform,
    double& out_scale,
    bool forceScaleToUnity = false
    )

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

bool mrpt::tfest::se3_l2(
    const std::vector<mrpt::math::TPoint3D>& in_points_this,
    const std::vector<mrpt::math::TPoint3D>& in_points_other,
    mrpt::poses::CPose3DQuat& out_transform,
    double& out_scale,
    bool forceScaleToUnity = false
    )

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

This version accepts corresponding points as two vectors of TPoint3D (must have identical length).

bool mrpt::tfest::se3_l2_robust(
    const mrpt::tfest::TMatchingPairList& in_correspondences,
    const TSE3RobustParams& in_params,
    TSE3RobustResult& out_results
    )

Least-squares (L2 norm) solution to finding the optimal SE(3) transform between two reference frames using RANSAC and the “quaternion” or Horn’s method:

• “Closed-form solution of absolute orientation using unit quaternions”, BKP Horn, Journal of the Optical Society of America, 1987.

The optimal transformation q fulfills \(p_{this} = q \oplus p_{other}\), that is, the transformation of frame other with respect to this.

Implemented by FAMD, 2008. Re-factored by JLBC, 2015.

[New in MRPT 1.3.0] This function replaces mrpt::scanmatching::leastSquareErrorRigidTransformation6DRANSAC()

Parameters:

in_correspondences	The set of correspondences.
in_params	Method parameters (see docs for TSE3RobustParams)
out_results	Results: transformation, scale, etc.

Returns:

True if the minimum number of correspondences was found, false otherwise.

See also:

se2_l2, se3_l2

template <typename T>
bool mrpt::tfest::operator < (const TMatchingPairTempl<T>& a, const TMatchingPairTempl<T>& b)

Comparison operator, first sorts by globalIdx, if equals, by localIdx.

template <typename T>
bool mrpt::tfest::operator == (
    const TMatchingPairTempl<T>& a,
    const TMatchingPairTempl<T>& b
    )

A comparison operator.

template <typename T>
bool mrpt::tfest::operator == (
    const TMatchingPairListTempl<T>& a,
    const TMatchingPairListTempl<T>& b
    )

A comparison operator.