Main MRPT website > C++ reference for MRPT 1.9.9
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-2018, 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 
11 #include <mrpt/math/math_frwds.h>
13 #include <mrpt/poses/CPosePDFSOG.h>
15 #include <mrpt/poses/poses_frwds.h>
17 
18 namespace mrpt
19 {
20 /** Functions for estimating the optimal transformation between two frames of
21  * references given measurements of corresponding points.
22  * \sa mrpt::slam::CICP
23  * \ingroup mrpt_tfest_grp
24  */
25 namespace tfest
26 {
27 /** \addtogroup mrpt_tfest_grp
28  * @{ */
29 
30 /** Least-squares (L2 norm) solution to finding the optimal SE(2) (x,y,yaw)
31  * between two reference frames.
32  * The optimal transformation `q` fulfills \f$ p_{this} = q \oplus p_{other}
33  * \f$, that is, the
34  * transformation of frame `other` with respect to `this`.
35  *
36  * \image html tfest_frames.png
37  *
38  * \param[in] in_correspondences The set of correspondences.
39  * \param[out] out_transformation The pose that minimizes the mean-square-error
40  * between all the correspondences.
41  * \param[out] out_estimateCovariance If provided (!=nullptr) this will contain
42  * on return a 3x3 covariance matrix with the NORMALIZED optimal estimate
43  * uncertainty. This matrix must be multiplied by \f$\sigma^2_p\f$, the variance
44  * of matched points in \f$x\f$ and \f$y\f$ (see paper
45  * http://www.mrpt.org/Paper:Occupancy_Grid_Matching)
46  * \return True if there are at least two correspondences, or false if one or
47  * none, thus we cannot establish any correspondence.
48  * \sa robustRigidTransformation
49  *
50  * \note Reference for covariance calculation: J.L. Blanco, J.
51  * Gonzalez-Jimenez, J.A. Fernandez-Madrigal, "A Robust, Multi-Hypothesis
52  * Approach to Matching Occupancy Grid Maps", Robotica, 2013.
53  * http://dx.doi.org/10.1017/S0263574712000732
54  * \note [New in MRPT 1.3.0] This function replaces
55  * mrpt::scanmatching::leastSquareErrorRigidTransformation()
56  * \note This function is hand-optimized for SSE2 architectures (if SSE2 is
57  * enabled from CMake)
58  * \sa se3_l2, se2_l2_robust
59  * \ingroup sse_optimizations
60  * \ingroup mrpt_tfest_grp
61  */
62 bool se2_l2(
63  const mrpt::tfest::TMatchingPairList& in_correspondences,
64  mrpt::math::TPose2D& out_transformation,
65  mrpt::math::CMatrixDouble33* out_estimateCovariance = nullptr);
66 
67 /** \overload */
68 bool se2_l2(
69  const mrpt::tfest::TMatchingPairList& in_correspondences,
70  mrpt::poses::CPosePDFGaussian& out_transformation);
71 
72 /** Parameters for se2_l2_robust(). See function for more details */
74 {
75  /** (Default=3) */
76  unsigned int ransac_minSetSize;
77  /** (Default = 20) */
78  unsigned int ransac_maxSetSize;
79  /** (Default = 3.0) */
81  /** (Default = 0) If set to 0, an adaptive algorithm is used to determine
82  * the number of iterations, such as a good model is found with a
83  * probability p=0.999, or that passed as the parameter
84  * probability_find_good_model */
85  unsigned int ransac_nSimulations;
86  /** (Default = true) If true, the weight of Gaussian modes will be
87  * increased when an exact match in the
88  * subset of correspondences for the modes is found. Otherwise, an
89  * approximate method is used as test by just looking at the
90  * resulting X,Y,PHI means. Threshold in this case are:
91  * ransac_fuseMaxDiffXY, ransac_fuseMaxDiffPhi */
93  /** (Default = 0.01) */
95  /** (Default=0.1degree) (In radians) */
97  /** (Default = true) Use Mahalanobis distance (true) or Euclidean dist
98  * (false) */
100  /** (Default = 0.999) See parameter ransac_nSimulations. When using
101  * `probability_find_good_model`, the minimum number of iterations can be
102  * set with `ransac_min_nSimulations` */
104  /** (Default = 1500) See parameter probability_find_good_model */
106  /** Stop searching for solutions when the RMSE of one solution is below this
107  * threshold. Special value "0" means "auto", which employs
108  * "2*normalizationStd". */
110  /** (Default=false) */
111  bool verbose;
112 
113  /** If provided, this user callback will be invoked to determine the
114  * individual compatibility between each potential pair
115  * of elements. Can check image descriptors, geometrical properties, etc.
116  * \return Must return true if the pair is a potential match, false
117  * otherwise.
118  */
119  // std::function<bool(TPotentialMatch)> user_individual_compat_callback; //
120  // This could be used in the future when we enforce C++11 to users...
122  /** User data to be passed to user_individual_compat_callback() */
124 
125  /** Default values */
127  : ransac_minSetSize(3),
128  ransac_maxSetSize(20),
132  ransac_fuseMaxDiffXY(0.01),
137  max_rmse_to_end(0),
138  verbose(false)
139  {
140  }
141 };
142 
143 /** Output placeholder for se2_l2_robust() */
145 {
146  /** The output as a set of transformations (sum of Gaussians) */
148  /** the largest consensus sub-set */
150  /** Number of actual iterations executed */
151  unsigned int ransac_iters;
152 
154 };
155 
156 /** Robust least-squares (L2 norm) solution to finding the optimal SE(2)
157  * (x,y,yaw) between two reference frames.
158  * This method implements a RANSAC-based robust estimation, returning a
159  * probability distribution over all the posibilities as a Sum of Gaussians.
160  *
161  * The optimal transformation `q` fulfills \f$ p_{this} = q \oplus p_{other}
162  * \f$, that is, the
163  * transformation of frame `other` with respect to `this`.
164  *
165  * \image html tfest_frames.png
166  *
167  * The technique was described in the paper:
168  * - J.L. Blanco, J. Gonzalez-Jimenez, J.A. Fernandez-Madrigal, "A Robust,
169  * Multi-Hypothesis Approach to Matching Occupancy Grid Maps", Robotica, 2013.
170  * http://dx.doi.org/10.1017/S0263574712000732
171  *
172  * This works as follows:
173  * - Repeat "ransac_nSimulations" times:
174  * - Randomly pick TWO correspondences from the set "in_correspondences".
175  * - Compute the associated rigid transformation.
176  * - For "ransac_maxSetSize" randomly selected correspondences, test for
177  * "consensus" with the current group:
178  * - If if is compatible (ransac_mahalanobisDistanceThreshold), grow
179  * the "consensus set"
180  * - If not, do not add it.
181  *
182  * For more details refer to the tutorial on <a
183  * href="http://www.mrpt.org/Scan_Matching_Algorithms">scan matching
184  * methods</a>.
185  * \param[in] in_normalizationStd The <b>standard deviation</b> (not variance)
186  * of landmarks/points/features being matched in X,Y. Used to normalize
187  * covariances returned as the SoG. (Refer to paper)
188  *
189  * <b>NOTE</b>: Parameter `ransac_maxSetSize` should be set to
190  * `in_correspondences.size()` to make sure that every correspondence is tested
191  * for each random permutation.
192  *
193  * \return True upon success, false if no subset was found with the minimum
194  * number of correspondences.
195  * \note [New in MRPT 1.3.0] This function replaces
196  * mrpt::scanmatching::robustRigidTransformation()
197  * \sa se3_l2, se2_l2_robust
198  */
199 bool se2_l2_robust(
200  const mrpt::tfest::TMatchingPairList& in_correspondences,
201  const double in_normalizationStd, const TSE2RobustParams& in_ransac_params,
202  TSE2RobustResult& out_results);
203 
204 /** @} */ // end of grouping
205 } // namespace tfest
206 } // namespace mrpt
void * user_individual_compat_callback_userdata
User data to be passed to user_individual_compat_callback()
Definition: se2.h:123
TSE2RobustParams()
Default values.
Definition: se2.h:126
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
Definition: CPosePDFSOG.h:37
double max_rmse_to_end
Stop searching for solutions when the RMSE of one solution is below this threshold.
Definition: se2.h:109
std::function< bool(const TPotentialMatch &)> TFunctorCheckPotentialMatch
bool 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...
Definition: se2_l2.cpp:45
Parameters for se2_l2_robust().
Definition: se2.h:73
double DEG2RAD(const double x)
Degrees to radians.
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:92
unsigned int ransac_nSimulations
(Default = 0) If set to 0, an adaptive algorithm is used to determine the number of iterations...
Definition: se2.h:85
unsigned int ransac_minSetSize
(Default=3)
Definition: se2.h:76
bool verbose
(Default=false)
Definition: se2.h:111
bool 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 ...
unsigned int ransac_maxSetSize
(Default = 20)
Definition: se2.h:78
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
A list of TMatchingPair.
Definition: TMatchingPair.h:83
bool ransac_algorithmForLandmarks
(Default = true) Use Mahalanobis distance (true) or Euclidean dist (false)
Definition: se2.h:99
double ransac_mahalanobisDistanceThreshold
(Default = 3.0)
Definition: se2.h:80
double ransac_fuseMaxDiffPhi
(Default=0.1degree) (In radians)
Definition: se2.h:96
unsigned int ransac_min_nSimulations
(Default = 1500) See parameter probability_find_good_model
Definition: se2.h:105
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
TFunctorCheckPotentialMatch user_individual_compat_callback
If provided, this user callback will be invoked to determine the individual compatibility between eac...
Definition: se2.h:121
unsigned int ransac_iters
Number of actual iterations executed.
Definition: se2.h:151
Lightweight 2D pose.
mrpt::poses::CPosePDFSOG transformation
The output as a set of transformations (sum of Gaussians)
Definition: se2.h:147
mrpt::tfest::TMatchingPairList largestSubSet
the largest consensus sub-set
Definition: se2.h:149
Output placeholder for se2_l2_robust()
Definition: se2.h:144
double ransac_fuseMaxDiffXY
(Default = 0.01)
Definition: se2.h:94
double probability_find_good_model
(Default = 0.999) See parameter ransac_nSimulations.
Definition: se2.h:103



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019