[mrpt-tfest]
Overview
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
// 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 void mrpt::tfest::registerAllClasses_mrpt_tfest(); 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:
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:
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
void mrpt::tfest::registerAllClasses_mrpt_tfest()
Forces manual RTTI registration of all serializable classes in this namespace.
Should never be required to be explicitly called by users, except if building MRPT as a static library.
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
.
![_images/tfest_frames.png](_images/tfest_frames.png)
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
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
.
![_images/tfest_frames.png](_images/tfest_frames.png)
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 toin_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:
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 [5].
The optimal transformation q
fulfills \(p_{this} = q \oplus p_{other}\), that is, the transformation of frame other
with respect to this
.
![_images/tfest_frames.png](_images/tfest_frames.png)
[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:
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 [5].
The optimal transformation q
fulfills \(p_{this} = q \oplus p_{other}\), that is, the transformation of frame other
with respect to this
.
![_images/tfest_frames.png](_images/tfest_frames.png)
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:
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.