Main MRPT website > C++ reference for MRPT 1.4.0
se2.h
Go to the documentation of this file.
1/* +---------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | http://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file |
6 | See: http://www.mrpt.org/Authors - All rights reserved. |
7 | Released under BSD License. See details in http://www.mrpt.org/License |
8 +---------------------------------------------------------------------------+ */
9#pragma once
10
19
20namespace mrpt
21{
22 /** Functions for estimating the optimal transformation between two frames of references given measurements of corresponding points.
23 * \sa mrpt::slam::CICP
24 * \ingroup mrpt_tfest_grp
25 */
26 namespace tfest
27 {
28 /** \addtogroup mrpt_tfest_grp
29 * @{ */
30
31 /** Least-squares (L2 norm) solution to finding the optimal SE(2) (x,y,yaw) between two reference frames.
32 * The optimal transformation `q` fulfills \f$ p_{this} = q \oplus p_{other} \f$, that is, the
33 * transformation of frame `other` with respect to `this`.
34 *
35 * \image html tfest_frames.png
36 *
37 * \param[in] in_correspondences The set of correspondences.
38 * \param[out] out_transformation The pose that minimizes the mean-square-error between all the correspondences.
39 * \param[out] 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 \f$\sigma^2_p\f$, the variance of matched points in \f$x\f$ and \f$y\f$ (see paper http://www.mrpt.org/Paper:Occupancy_Grid_Matching)
40 * \return True if there are at least two correspondences, or false if one or none, thus we cannot establish any correspondence.
41 * \sa robustRigidTransformation
42 *
43 * \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
44 * \note [New in MRPT 1.3.0] This function replaces mrpt::scanmatching::leastSquareErrorRigidTransformation()
45 * \note This function is hand-optimized for SSE2 architectures (if SSE2 is enabled from CMake)
46 * \sa se3_l2, se2_l2_robust
47 * \ingroup sse_optimizations
48 * \ingroup mrpt_tfest_grp
49 */
51 const mrpt::utils::TMatchingPairList & in_correspondences,
52 mrpt::math::TPose2D & out_transformation,
53 mrpt::math::CMatrixDouble33 * out_estimateCovariance = NULL );
54
55 /** \overload */
57 const mrpt::utils::TMatchingPairList & in_correspondences,
58 mrpt::poses::CPosePDFGaussian & out_transformation );
59
60
61 /** Parameters for se2_l2_robust(). See function for more details */
63 {
64 unsigned int ransac_minSetSize; //!< (Default=3)
65 unsigned int ransac_maxSetSize; //!< (Default = 20)
66 double ransac_mahalanobisDistanceThreshold; //!< (Default = 3.0)
67 unsigned int ransac_nSimulations; //!< (Default = 0) If set to 0, 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
68 /** (Default = true) If true, the weight of Gaussian modes will be increased when an exact match in the
69 * subset of correspondences for the modes is found. Otherwise, an approximate method is used as test by just looking at the
70 * resulting X,Y,PHI means. Threshold in this case are: ransac_fuseMaxDiffXY, ransac_fuseMaxDiffPhi */
72 double ransac_fuseMaxDiffXY; //!< (Default = 0.01)
73 double ransac_fuseMaxDiffPhi; //!< (Default=0.1degree) (In radians)
74 bool ransac_algorithmForLandmarks; //!< (Default = true) Use Mahalanobis distance (true) or Euclidean dist (false)
75 double probability_find_good_model; //!< (Default = 0.999) See parameter ransac_nSimulations. When using `probability_find_good_model`, the minimum number of iterations can be set with `ransac_min_nSimulations`
76 unsigned int ransac_min_nSimulations; //!< (Default = 1500) See parameter probability_find_good_model
77 double max_rmse_to_end; //!< Stop searching for solutions when the RMSE of one solution is below this threshold. Special value "0" means "auto", which employs "2*normalizationStd".
78 bool verbose; //!< (Default=false)
79
80 /** If provided, this user callback will be invoked to determine the individual compatibility between each potential pair
81 * of elements. Can check image descriptors, geometrical properties, etc.
82 * \return Must return true if the pair is a potential match, false otherwise.
83 */
84 //std::function<bool(TPotentialMatch)> user_individual_compat_callback; // This could be used in the future when we enforce C++11 to users...
86 void * user_individual_compat_callback_userdata; //!< User data to be passed to user_individual_compat_callback()
87
88 /** Default values */
90 ransac_minSetSize( 3 ),
91 ransac_maxSetSize( 20 ),
92 ransac_mahalanobisDistanceThreshold( 3.0 ),
93 ransac_nSimulations( 0 ),
94 ransac_fuseByCorrsMatch( true ),
95 ransac_fuseMaxDiffXY( 0.01 ),
96 ransac_fuseMaxDiffPhi( mrpt::utils::DEG2RAD(0.1) ),
97 ransac_algorithmForLandmarks( true ),
98 probability_find_good_model( 0.999 ),
99 ransac_min_nSimulations( 1500 ),
100 max_rmse_to_end(0),
101 verbose(false),
102 user_individual_compat_callback(NULL),
103 user_individual_compat_callback_userdata(NULL)
104 {
105 }
106 };
107
108 /** Output placeholder for se2_l2_robust() */
110 {
111 mrpt::poses::CPosePDFSOG transformation; //!< The output as a set of transformations (sum of Gaussians)
112 mrpt::utils::TMatchingPairList largestSubSet; //!< the largest consensus sub-set
113 unsigned int ransac_iters; //!< Number of actual iterations executed
114
115 TSE2RobustResult() : ransac_iters(0) { }
116 };
117
118
119 /** Robust least-squares (L2 norm) solution to finding the optimal SE(2) (x,y,yaw) between two reference frames.
120 * This method implements a RANSAC-based robust estimation, returning a probability distribution over all the posibilities as a Sum of Gaussians.
121 *
122 * The optimal transformation `q` fulfills \f$ p_{this} = q \oplus p_{other} \f$, that is, the
123 * transformation of frame `other` with respect to `this`.
124 *
125 * \image html tfest_frames.png
126 *
127 * The technique was described in the paper:
128 * - 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
129 *
130 * This works as follows:
131 * - Repeat "ransac_nSimulations" times:
132 * - Randomly pick TWO correspondences from the set "in_correspondences".
133 * - Compute the associated rigid transformation.
134 * - For "ransac_maxSetSize" randomly selected correspondences, test for "consensus" with the current group:
135 * - If if is compatible (ransac_mahalanobisDistanceThreshold), grow the "consensus set"
136 * - If not, do not add it.
137 *
138 * For more details refer to the tutorial on <a href="http://www.mrpt.org/Scan_Matching_Algorithms">scan matching methods</a>.
139 * \param[in] in_normalizationStd The <b>standard deviation</b> (not variance) of landmarks/points/features being matched in X,Y. Used to normalize covariances returned as the SoG. (Refer to paper)
140 *
141 * <b>NOTE</b>: Parameter `ransac_maxSetSize` should be set to `in_correspondences.size()` to make sure that every correspondence is tested for each random permutation.
142 *
143 * \return True upon success, false if no subset was found with the minimum number of correspondences.
144 * \note [New in MRPT 1.3.0] This function replaces mrpt::scanmatching::robustRigidTransformation()
145 * \sa se3_l2, se2_l2_robust
146 */
148 const mrpt::utils::TMatchingPairList &in_correspondences,
149 const double in_normalizationStd,
150 const TSE2RobustParams & in_ransac_params,
151 TSE2RobustResult & out_results
152 );
153
154 /** @} */ // end of grouping
155 }
156} // End of namespace
#define DEG2RAD
Definition: bits.h:86
A numeric matrix of compile-time fixed size.
Declares a class that represents a Probability Density function (PDF) of a 2D pose .
Declares a class that represents a Probability Density function (PDF) of a 2D pose .
Definition: CPosePDFSOG.h:37
A list of TMatchingPair.
Definition: TMatchingPair.h:67
bool(* TFunctorCheckPotentialMatch)(const TPotentialMatch &pm, void *user_data)
bool TFEST_IMPEXP se2_l2(const mrpt::utils::TMatchingPairList &in_correspondences, mrpt::math::TPose2D &out_transformation, mrpt::math::CMatrixDouble33 *out_estimateCovariance=NULL)
Least-squares (L2 norm) solution to finding the optimal SE(2) (x,y,yaw) between two reference frames.
bool TFEST_IMPEXP se2_l2_robust(const mrpt::utils::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 ...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Lightweight 2D pose.
Parameters for se2_l2_robust().
Definition: se2.h:63
unsigned int ransac_nSimulations
(Default = 0) If set to 0, an adaptive algorithm is used to determine the number of iterations,...
Definition: se2.h:67
bool verbose
(Default=false)
Definition: se2.h:78
double max_rmse_to_end
Stop searching for solutions when the RMSE of one solution is below this threshold....
Definition: se2.h:77
double ransac_fuseMaxDiffXY
(Default = 0.01)
Definition: se2.h:72
unsigned int ransac_maxSetSize
(Default = 20)
Definition: se2.h:65
bool ransac_algorithmForLandmarks
(Default = true) Use Mahalanobis distance (true) or Euclidean dist (false)
Definition: se2.h:74
double ransac_fuseMaxDiffPhi
(Default=0.1degree) (In radians)
Definition: se2.h:73
double ransac_mahalanobisDistanceThreshold
(Default = 3.0)
Definition: se2.h:66
TSE2RobustParams()
Default values.
Definition: se2.h:89
bool ransac_fuseByCorrsMatch
(Default = true) If true, the weight of Gaussian modes will be increased when an exact match in the s...
Definition: se2.h:71
unsigned int ransac_minSetSize
(Default=3)
Definition: se2.h:64
TFunctorCheckPotentialMatch user_individual_compat_callback
If provided, this user callback will be invoked to determine the individual compatibility between eac...
Definition: se2.h:85
unsigned int ransac_min_nSimulations
(Default = 1500) See parameter probability_find_good_model
Definition: se2.h:76
void * user_individual_compat_callback_userdata
User data to be passed to user_individual_compat_callback()
Definition: se2.h:86
double probability_find_good_model
(Default = 0.999) See parameter ransac_nSimulations. When using probability_find_good_model,...
Definition: se2.h:75
Output placeholder for se2_l2_robust()
Definition: se2.h:110
mrpt::utils::TMatchingPairList largestSubSet
the largest consensus sub-set
Definition: se2.h:112
mrpt::poses::CPosePDFSOG transformation
The output as a set of transformations (sum of Gaussians)
Definition: se2.h:111
unsigned int ransac_iters
Number of actual iterations executed.
Definition: se2.h:113



Page generated by Doxygen 1.9.6 for MRPT 1.4.0 SVN: at Wed Mar 22 04:35:51 UTC 2023