Function mrpt::tfest::se2_l2(const mrpt::tfest::TMatchingPairList&, mrpt::math::TPose2D&, mrpt::math::CMatrixDouble33 *)

• Defined in File se2.h

Function Documentation

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.

See also

robustRigidTransformation

See also

se3_l2, se2_l2_robust

Note

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

Note

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

Note

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

Parameters:

• in_correspondences – [in] The set of correspondences.

• out_transformation – [out] The pose that minimizes the mean-square-error between all the correspondences.

• out_estimateCovariance – [out] 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.