Main MRPT website > C++ reference for MRPT 1.5.6
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-2017, 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/utils/utils_defs.h>
12 #include <mrpt/math/math_frwds.h>
14 #include <mrpt/poses/CPosePDFSOG.h>
17 #include <mrpt/poses/poses_frwds.h>
19 
20 namespace 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  */
50  bool TFEST_IMPEXP se2_l2(
51  const mrpt::utils::TMatchingPairList & in_correspondences,
52  mrpt::math::TPose2D & out_transformation,
53  mrpt::math::CMatrixDouble33 * out_estimateCovariance = NULL );
54 
55  /** \overload */
56  bool TFEST_IMPEXP se2_l2(
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 ),
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
void * user_individual_compat_callback_userdata
User data to be passed to user_individual_compat_callback()
Definition: se2.h:86
TSE2RobustParams()
Default values.
Definition: se2.h:89
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. Special value "0" means "auto", which employs "2*normalizationStd".
Definition: se2.h:77
Parameters for se2_l2_robust().
Definition: se2.h:62
mrpt::utils::TMatchingPairList largestSubSet
the largest consensus sub-set
Definition: se2.h:112
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_nSimulations
(Default = 0) If set to 0, an adaptive algorithm is used to determine the number of iterations...
Definition: se2.h:67
unsigned int ransac_minSetSize
(Default=3)
Definition: se2.h:64
const float ransac_mahalanobisDistanceThreshold
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 ...
bool verbose
(Default=false)
Definition: se2.h:78
unsigned int ransac_maxSetSize
(Default = 20)
Definition: se2.h:65
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
A list of TMatchingPair.
Definition: TMatchingPair.h:78
bool ransac_algorithmForLandmarks
(Default = true) Use Mahalanobis distance (true) or Euclidean dist (false)
Definition: se2.h:74
#define DEG2RAD
double ransac_mahalanobisDistanceThreshold
(Default = 3.0)
Definition: se2.h:66
bool(* TFunctorCheckPotentialMatch)(const TPotentialMatch &pm, void *user_data)
double ransac_fuseMaxDiffPhi
(Default=0.1degree) (In radians)
Definition: se2.h:73
unsigned int ransac_min_nSimulations
(Default = 1500) See parameter probability_find_good_model
Definition: se2.h:76
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_iters
Number of actual iterations executed.
Definition: se2.h:113
Lightweight 2D pose.
mrpt::poses::CPosePDFSOG transformation
The output as a set of transformations (sum of Gaussians)
Definition: se2.h:111
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...
Definition: se2_l2.cpp:45
Output placeholder for se2_l2_robust()
Definition: se2.h:109
double ransac_fuseMaxDiffXY
(Default = 0.01)
Definition: se2.h:72
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
Definition: se2.h:75



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018