MRPT  2.0.1
Classes | Namespaces | Typedefs | Functions
[mrpt-tfest]

Detailed Description

Algorithms to find optimal transformations from sets of correspondences.

Back to list of all libraries | See all modules

Library `mrpt-tfest`

[New in MRPT 2.0.0]

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

    sudo apt install libmrpt-tfest-dev

See: Using MRPT from your CMake project

Transformation estimation (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

Classes

struct  mrpt::tfest::TPotentialMatch
 For each individual-compatibility (IC) test, the indices of the candidate match between elements in both reference frames. More...
 
struct  mrpt::tfest::TSE2RobustParams
 Parameters for se2_l2_robust(). More...
 
struct  mrpt::tfest::TSE2RobustResult
 Output placeholder for se2_l2_robust() More...
 
struct  mrpt::tfest::TSE3RobustParams
 Parameters for se3_l2_robust(). More...
 
struct  mrpt::tfest::TSE3RobustResult
 Output placeholder for se3_l2_robust() More...
 

Namespaces

 mrpt::tfest
 Functions for estimating the optimal transformation between two frames of references given measurements of corresponding points.
 

Typedefs

using mrpt::tfest::TFunctorCheckPotentialMatch = std::function< bool(const TPotentialMatch &)>
 

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. More...
 
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)
 Robust least-squares (L2 norm) solution to finding the optimal SE(2) (x,y,yaw) between two reference frames. More...
 
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: More...
 
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). More...
 
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: More...
 

Typedef Documentation

◆ TFunctorCheckPotentialMatch

using mrpt::tfest::TFunctorCheckPotentialMatch = typedef std::function<bool(const TPotentialMatch&)>

Definition at line 29 of file indiv-compat-decls.h.

Function Documentation

◆ se2_l2() [1/2]

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.

tfest_frames.png
Parameters
[in]in_correspondencesThe set of correspondences.
[out]out_transformationThe pose that minimizes the mean-square-error between all the correspondences.
[out]out_estimateCovarianceIf 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
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
[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)
See also
se3_l2, se2_l2_robust

Definition at line 92 of file se2_l2.cpp.

References mrpt::tfest::internal::se2_l2_impl_return_t< T >::Ax, mrpt::tfest::internal::se2_l2_impl_return_t< T >::Ay, mrpt::tfest::internal::se2_l2_impl_return_t< T >::mean_x_a, mrpt::tfest::internal::se2_l2_impl_return_t< T >::mean_x_b, mrpt::tfest::internal::se2_l2_impl_return_t< T >::mean_y_a, mrpt::tfest::internal::se2_l2_impl_return_t< T >::mean_y_b, MRPT_END, MRPT_START, mrpt::math::TPose2D::phi, se2_l2_impl(), mrpt::square(), mrpt::cpu::SSE2, mrpt::cpu::supports(), mrpt::math::TPose2D::x, and mrpt::math::TPose2D::y.

Referenced by mrpt::slam::CGridMapAligner::AlignPDF_robustMatch(), mrpt::slam::CICP::ICP_Method_Classic(), ransac_data_assoc_run(), mrpt::tfest::se2_l2(), and mrpt::tfest::se2_l2_robust().

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

◆ se2_l2() [2/2]

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.

Definition at line 27 of file se2_l2.cpp.

References mrpt::poses::CPosePDFGaussian::cov, mrpt::poses::CPosePDFGaussian::mean, and mrpt::tfest::se2_l2().

Here is the call graph for this function:

◆ se2_l2_robust()

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.

tfest_frames.png

The technique was described in the paper:

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.

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

    NOTE: Parameter ransac_maxSetSize should be set to in_correspondences.size() to make sure that every correspondence is tested for each random permutation.

Returns
True upon success, false if no subset was found with the minimum number of correspondences.
Note
[New in MRPT 1.3.0] This function replaces mrpt::scanmatching::robustRigidTransformation()
See also
se3_l2, se2_l2_robust

Definition at line 79 of file se2_l2_ransac.cpp.

References ASSERT_, mrpt::math::chi2inv(), mrpt::poses::CPose2D::composePoint(), mrpt::poses::CPosePDFGaussian::composePoint(), mrpt::poses::CPosePDFSOG::TGaussianMode::cov, mrpt::poses::CPosePDFGaussian::cov, mrpt::DEG2RAD(), mrpt::math::distanceBetweenPoints(), mrpt::random::getRandomGenerator(), mrpt::tfest::TPotentialMatch::idx_other, mrpt::tfest::TPotentialMatch::idx_this, mrpt::poses::CPosePDFSOG::TGaussianMode::log_w, mrpt::poses::CPoint2DPDFGaussian::mahalanobisDistanceToPoint(), markAsPicked(), mrpt::poses::CPosePDFGaussian::mean, mrpt::poses::CPosePDFSOG::TGaussianMode::mean, MRPT_END_WITH_CLEAN_UP, MRPT_START, normalizationStd, mrpt::tfest::TMatchingPair::other_idx, mrpt::tfest::TMatchingPair::other_x, mrpt::tfest::TMatchingPair::other_y, params, mrpt::random::CRandomGenerator::permuteVector(), mrpt::poses::CPose2D::phi(), results, mrpt::round(), mrpt::tfest::se2_l2(), mrpt::square(), mrpt::tfest::TMatchingPair::this_idx, mrpt::tfest::TMatchingPair::this_x, mrpt::tfest::TMatchingPair::this_y, mrpt::vision::TStereoCalibParams::verbose, and mrpt::math::wrapToPi().

Referenced by mrpt::slam::CGridMapAligner::AlignPDF_robustMatch(), mrpt::slam::CICP::ICP_Method_Classic(), and ransac_data_assoc_run().

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

◆ se3_l2() [1/2]

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.

tfest_frames.png
Parameters
[in]in_correspondencesThe coordinates of the input points for the two coordinate systems "this" and "other"
[out]out_transformThe output transformation
[out]out_scaleThe computed scale of the optimal transformation (will be 1.0 for a perfectly rigid translation + rotation).
[in]forceScaleToUnityWhether or not force the scale employed to rotate the coordinate systems to one (rigid transformation)
Note
[New in MRPT 1.3.0] This function replaces mrpt::scanmatching::leastSquareErrorRigidTransformation6DRANSAC() and mrpt::scanmatching::HornMethod()
See also
se2_l2, se3_l2_robust

Definition at line 219 of file se3_l2.cpp.

References se3_l2_internal().

Referenced by mrpt::slam::CICP::ICP3D_Method_Classic(), mrpt::topography::path_from_rtk_gps(), mrpt::tfest::se3_l2_robust(), and TEST().

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

◆ se3_l2() [2/2]

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

Definition at line 204 of file se3_l2.cpp.

References se3_l2_internal().

Here is the call graph for this function:

◆ se3_l2_robust()

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.

tfest_frames.png
Parameters
[in]in_correspondencesThe set of correspondences.
[in]in_paramsMethod parameters (see docs for TSE3RobustParams)
[out]out_resultsResults: transformation, scale, etc.
Returns
True if the minimum number of correspondences was found, false otherwise.
Note
Implemented by FAMD, 2008. Re-factored by JLBC, 2015.
[New in MRPT 1.3.0] This function replaces mrpt::scanmatching::leastSquareErrorRigidTransformation6DRANSAC()
See also
se2_l2, se3_l2

Definition at line 32 of file se3_l2_ransac.cpp.

References ASSERTMSG_, mrpt::random::getRandomGenerator(), mrpt::tfest::TPotentialMatch::idx_other, mrpt::tfest::TPotentialMatch::idx_this, mrpt::math::linspace(), MRPT_END, MRPT_START, params, mrpt::random::CRandomGenerator::permuteVector(), mrpt::poses::CPose3D::pitch(), results, mrpt::poses::CPose3D::roll(), mrpt::round(), mrpt::tfest::se3_l2(), mrpt::square(), mrpt::vision::TStereoCalibParams::verbose, mrpt::poses::CPoseOrPoint< DERIVEDCLASS, DIM >::x(), mrpt::poses::CPoseOrPoint< DERIVEDCLASS, DIM >::y(), and mrpt::poses::CPose3D::yaw().

Referenced by TEST().

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


Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020