MRPT logo

mrpt::scan_matching Namespace Reference

A set of scan matching-related static functions. More...


Functions

bool MRPTDLLIMPEXP leastSquareErrorRigidTransformation6D (const CMetricMap::TMatchingPairList &in_correspondences, CPose3D &out_transformation, double &out_scale, CMatrixDouble *in_rotationMatrix=NULL, CMatrixDouble *out_estimateCovariance=NULL, bool forceScaleToUnity=false)
 This method provides the closed-form solution of absolute orientation using unit quaternions to a set of over-constrained correspondences for finding the 6D rigid transformation between two cloud of 3D points.
bool MRPTDLLIMPEXP leastSquareErrorRigidTransformation6DRANSAC (const CMetricMap::TMatchingPairList &in_correspondences, CPose3D &out_transformation, double &out_scale, vector_int &out_inliers_idx, CMatrixD *in_rotationMatrix=NULL, CMatrixD *out_estimateCovariance=NULL, bool forceScaleToUnity=false)
 This method provides the closed-form solution of absolute orientation using unit quaternions to a set of over-constrained correspondences for finding the 6D rigid transformation between two cloud of 3D points using RANSAC.
bool MRPTDLLIMPEXP leastSquareErrorRigidTransformation (CMetricMap::TMatchingPairList &in_correspondences, CPose2D &out_transformation, CMatrixDouble33 *out_estimateCovariance=NULL)
 This method provides the basic least-square-error solution to a set of over-constrained correspondences for finding the (x,y,phi) rigid transformation between two planes.
bool MRPTDLLIMPEXP leastSquareErrorRigidTransformation (CMetricMap::TMatchingPairList &in_correspondences, CPosePDFGaussian &out_transformation)
 This method provides the basic least-square-error solution to a set of over-constrained correspondences for finding the (x,y,phi) rigid transformation between two planes.
void MRPTDLLIMPEXP robustRigidTransformation (mrpt::slam::CMetricMap::TMatchingPairList &in_correspondences, poses::CPosePDFSOG &out_transformation, float normalizationStd, unsigned int ransac_minSetSize=3, unsigned int ransac_maxSetSize=20, float ransac_mahalanobisDistanceThreshold=3.0f, unsigned int ransac_nSimulations=0, mrpt::slam::CMetricMap::TMatchingPairList *out_largestSubSet=NULL, bool ransac_fuseByCorrsMatch=true, float ransac_fuseMaxDiffXY=0.01f, float ransac_fuseMaxDiffPhi=DEG2RAD(0.1f), bool ransac_algorithmForLandmarks=true, double probability_find_good_model=0.999, unsigned int ransac_min_nSimulations=1500)
 This method implements a RANSAC-based robust estimation of the rigid transformation between two planes, returning a probability distribution over all the posibilities as a Sum of Gaussians.


Detailed Description

A set of scan matching-related static functions.

See also:
mrpt::slam::CICP


Function Documentation

bool MRPTDLLIMPEXP mrpt::scan_matching::leastSquareErrorRigidTransformation ( CMetricMap::TMatchingPairList &  in_correspondences,
CPosePDFGaussian &  out_transformation 
)

This method provides the basic least-square-error solution to a set of over-constrained correspondences for finding the (x,y,phi) rigid transformation between two planes.

The optimal transformation q fulfills: $ point_this = q \oplus point_other $

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 (!=NULL) 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 http://babel.isa.uma.es/mrpt/index.php/Paper:Occupancy_Grid_Matching)
Exceptions:
Raises a std::exception if the list "in_correspondences" has not a minimum of two correspondences.
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 MRPTDLLIMPEXP mrpt::scan_matching::leastSquareErrorRigidTransformation ( CMetricMap::TMatchingPairList &  in_correspondences,
CPose2D &  out_transformation,
CMatrixDouble33 *  out_estimateCovariance = NULL 
)

This method provides the basic least-square-error solution to a set of over-constrained correspondences for finding the (x,y,phi) rigid transformation between two planes.

The optimal transformation q fulfills: $ point_this = q \oplus point_other $

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 (!=NULL) 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 http://babel.isa.uma.es/mrpt/index.php/Paper:Occupancy_Grid_Matching)
Exceptions:
Raises a std::exception if the list "in_correspondences" has not a minimum of two correspondences.
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 MRPTDLLIMPEXP mrpt::scan_matching::leastSquareErrorRigidTransformation6D ( const CMetricMap::TMatchingPairList &  in_correspondences,
CPose3D &  out_transformation,
double &  out_scale,
CMatrixDouble *  in_rotationMatrix = NULL,
CMatrixDouble *  out_estimateCovariance = NULL,
bool  forceScaleToUnity = false 
)

This method provides the closed-form solution of absolute orientation using unit quaternions to a set of over-constrained correspondences for finding the 6D rigid transformation between two cloud of 3D points.

The output 3D pose is computed using the method described in "Closed-form solution of absolute orientation using unit quaternions", BKP Horn, Journal of the Optical Society of America, 1987. If supplied, the output covariance matrix is computed using... TODO

Todo:
Explain covariance!!
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 (!=NULL) 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....)
Exceptions:
Raises a std::exception if the list "in_correspondences" has not a minimum of two correspondences.
Returns:
True if there are at least two correspondences, or false if one or none, thus we cannot establish any correspondence. Implemented by FAMD, 2007.
See also:
robustRigidTransformation

bool MRPTDLLIMPEXP mrpt::scan_matching::leastSquareErrorRigidTransformation6DRANSAC ( const CMetricMap::TMatchingPairList &  in_correspondences,
CPose3D &  out_transformation,
double &  out_scale,
vector_int &  out_inliers_idx,
CMatrixD *  in_rotationMatrix = NULL,
CMatrixD *  out_estimateCovariance = NULL,
bool  forceScaleToUnity = false 
)

This method provides the closed-form solution of absolute orientation using unit quaternions to a set of over-constrained correspondences for finding the 6D rigid transformation between two cloud of 3D points using RANSAC.

The output 3D pose is computed using the method described in "Closed-form solution of absolute orientation using unit quaternions", BKP Horn, Journal of the Optical Society of America, 1987. If supplied, the output covariance matrix is computed using... TODO

Todo:
Explain covariance!!
Parameters:
in_correspondences The set of correspondences.
out_transformation The pose that minimizes the mean-square-error between all the correspondences.
out_scale The estimated scale of the rigid transformation (should be very close to 1.0)
out_inliers_idx Indexes within the "in_correspondences" list which corresponds with inliers
out_estimateCovariance If provided (!=NULL) 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....)
Exceptions:
Raises a std::exception if the list "in_correspondences" has not a minimum of two correspondences.
Returns:
True if there are at least two correspondences, or false if one or none, thus we cannot establish any correspondence. Implemented by FAMD, 2008.
See also:
robustRigidTransformation

void MRPTDLLIMPEXP mrpt::scan_matching::robustRigidTransformation ( mrpt::slam::CMetricMap::TMatchingPairList in_correspondences,
poses::CPosePDFSOG &  out_transformation,
float  normalizationStd,
unsigned int  ransac_minSetSize = 3,
unsigned int  ransac_maxSetSize = 20,
float  ransac_mahalanobisDistanceThreshold = 3.0f,
unsigned int  ransac_nSimulations = 0,
mrpt::slam::CMetricMap::TMatchingPairList out_largestSubSet = NULL,
bool  ransac_fuseByCorrsMatch = true,
float  ransac_fuseMaxDiffXY = 0.01f,
float  ransac_fuseMaxDiffPhi = DEG2RAD(0.1f),
bool  ransac_algorithmForLandmarks = true,
double  probability_find_good_model = 0.999,
unsigned int  ransac_min_nSimulations = 1500 
)

This method implements a RANSAC-based robust estimation of the rigid transformation between two planes, returning a probability distribution over all the posibilities as a Sum of Gaussians.

This works are 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:

  • If a pointer is supplied to "out_largestSubSet", the largest consensus sub-set of correspondences will be returned there.
  • The parameter "normalizationStd" is the standard deviation (not variance) of landmarks being matched in X,Y. Used to normalize covariances returned as the SoG.
  • If ransac_nSimulations=0 then an adaptive algorithm is used to determine the number of iterations, such as a good model is found with a probability p=0.999, or that passed as the parameter probability_find_good_model
  • When using "probability_find_good_model", the minimum number of iterations can be set with "ransac_min_nSimulations".

If ransac_fuseByCorrsMatch=true (the default), the weight of Gaussian modes will be increased when an exact match in the subset of correspondences for the modes is found. Otherwise, an approximate method is used as test by just looking at the resulting X,Y,PHI means (Threshold in this case are: ransac_fuseMaxDiffXY, ransac_fuseMaxDiffPhi).

Exceptions:
Raises a std::exception if the list "in_correspondences" has not a minimum of two correspondences.
See also:
leastSquareErrorRigidTransformation




Page generated by Doxygen 1.5.9 for MRPT 0.7.1 SVN: at Mon Aug 17 22:32:05 EDT 2009