MRPT  2.0.1
se2.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10 
11 #include <mrpt/math/CMatrixFixed.h>
12 #include <mrpt/math/math_frwds.h>
13 #include <mrpt/poses/CPosePDFSOG.h>
14 #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  * https://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{3};
77  /** (Default = 20) */
78  unsigned int ransac_maxSetSize{20};
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{0};
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) */
94  double ransac_fuseMaxDiffXY{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 */
105  unsigned int ransac_min_nSimulations{1500};
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". */
109  double max_rmse_to_end{0};
110  /** (Default=false) */
111  bool verbose{false};
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 */
126  TSE2RobustParams() = default;
127 };
128 
129 /** Output placeholder for se2_l2_robust() */
131 {
132  /** The output as a set of transformations (sum of Gaussians) */
134  /** the largest consensus sub-set */
136  /** Number of actual iterations executed */
137  unsigned int ransac_iters{0};
138 
139  TSE2RobustResult() = default;
140 };
141 
142 /** Robust least-squares (L2 norm) solution to finding the optimal SE(2)
143  * (x,y,yaw) between two reference frames.
144  * This method implements a RANSAC-based robust estimation, returning a
145  * probability distribution over all the possibilities as a Sum of Gaussians.
146  *
147  * The optimal transformation `q` fulfills \f$ p_{this} = q \oplus p_{other}
148  * \f$, that is, the
149  * transformation of frame `other` with respect to `this`.
150  *
151  * \image html tfest_frames.png
152  *
153  * The technique was described in the paper:
154  * - J.L. Blanco, J. Gonzalez-Jimenez, J.A. Fernandez-Madrigal, "A Robust,
155  * Multi-Hypothesis Approach to Matching Occupancy Grid Maps", Robotica, 2013.
156  * http://dx.doi.org/10.1017/S0263574712000732
157  *
158  * This works as follows:
159  * - Repeat "ransac_nSimulations" times:
160  * - Randomly pick TWO correspondences from the set "in_correspondences".
161  * - Compute the associated rigid transformation.
162  * - For "ransac_maxSetSize" randomly selected correspondences, test for
163  * "consensus" with the current group:
164  * - If if is compatible (ransac_mahalanobisDistanceThreshold), grow
165  * the "consensus set"
166  * - If not, do not add it.
167  *
168  * For more details refer to the tutorial on <a
169  * href="http://www.mrpt.org/Scan_Matching_Algorithms">scan matching
170  * methods</a>.
171  * \param[in] in_normalizationStd The <b>standard deviation</b> (not variance)
172  * of landmarks/points/features being matched in X,Y. Used to normalize
173  * covariances returned as the SoG. (Refer to paper)
174  *
175  * <b>NOTE</b>: Parameter `ransac_maxSetSize` should be set to
176  * `in_correspondences.size()` to make sure that every correspondence is tested
177  * for each random permutation.
178  *
179  * \return True upon success, false if no subset was found with the minimum
180  * number of correspondences.
181  * \note [New in MRPT 1.3.0] This function replaces
182  * mrpt::scanmatching::robustRigidTransformation()
183  * \sa se3_l2, se2_l2_robust
184  */
185 bool se2_l2_robust(
186  const mrpt::tfest::TMatchingPairList& in_correspondences,
187  const double in_normalizationStd, const TSE2RobustParams& in_ransac_params,
188  TSE2RobustResult& out_results);
189 
190 /** @} */ // end of grouping
191 } // namespace tfest
192 } // namespace mrpt
void * user_individual_compat_callback_userdata
User data to be passed to user_individual_compat_callback()
Definition: se2.h:123
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
Definition: CPosePDFSOG.h:34
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:92
Parameters for se2_l2_robust().
Definition: se2.h:73
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 ...
constexpr double DEG2RAD(const double x)
Degrees to radians.
A list of TMatchingPair.
Definition: TMatchingPair.h:70
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:137
TSE2RobustParams()=default
Default values.
Lightweight 2D pose.
Definition: TPose2D.h:22
mrpt::poses::CPosePDFSOG transformation
The output as a set of transformations (sum of Gaussians)
Definition: se2.h:133
mrpt::tfest::TMatchingPairList largestSubSet
the largest consensus sub-set
Definition: se2.h:135
Output placeholder for se2_l2_robust()
Definition: se2.h:130
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 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020