Main MRPT website > C++ reference for MRPT 1.9.9
se3.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/CPose3DQuat.h>
15 #include <mrpt/poses/poses_frwds.h>
17 
18 namespace mrpt
19 {
20 namespace tfest
21 {
22 /** \addtogroup mrpt_tfest_grp
23  * @{ */
24 
25 /** Least-squares (L2 norm) solution to finding the optimal SE(3) transform
26  * between two reference frames using the "quaternion" or Horn's method:
27  * - "Closed-form solution of absolute orientation using unit quaternions",
28  * BKP Horn, Journal of the Optical Society of America, 1987.
29  *
30  * The optimal transformation `q` fulfills \f$ p_{this} = q \oplus p_{other}
31  * \f$, that is, the
32  * transformation of frame `other` with respect to `this`.
33  *
34  * \image html tfest_frames.png
35  *
36  * \param[in] in_correspondences The coordinates of the input points for the
37  * two coordinate systems "this" and "other"
38  * \param[out] out_transform The output transformation
39  * \param[out] out_scale The computed scale of the optimal
40  * transformation (will be 1.0 for a perfectly rigid translation + rotation).
41  * \param[in] forceScaleToUnity Whether or not force the scale employed to
42  * rotate the coordinate systems to one (rigid transformation)
43  * \note [New in MRPT 1.3.0] This function replaces
44  * mrpt::scanmatching::leastSquareErrorRigidTransformation6DRANSAC() and
45  * mrpt::scanmatching::HornMethod()
46  * \sa se2_l2, se3_l2_robust
47  */
48 bool se3_l2(
49  const mrpt::tfest::TMatchingPairList& in_correspondences,
50  mrpt::poses::CPose3DQuat& out_transform, double& out_scale,
51  bool forceScaleToUnity = false);
52 
53 /** \overload
54  *
55  * This version accepts corresponding points as two vectors of TPoint3D (must
56  * have identical length).
57  */
58 bool se3_l2(
59  const std::vector<mrpt::math::TPoint3D>& in_points_this,
60  const std::vector<mrpt::math::TPoint3D>& in_points_other,
61  mrpt::poses::CPose3DQuat& out_transform, double& out_scale,
62  bool forceScaleToUnity = false);
63 
64 /** Parameters for se3_l2_robust(). See function for more details */
66 {
67  /** (Default=5) The minimum amount of points in a set to start a consensus
68  * set. \sa ransac_maxSetSizePct */
69  unsigned int ransac_minSetSize{5};
70  /** (Default=50) The maximum number of iterations of the RANSAC algorithm */
71  unsigned int ransac_nmaxSimulations{50};
72  /** (Default=0.5) The minimum ratio (0.0 - 1.0) of the input set that is
73  * considered to be inliers. *Important*: The minimum size of a consensus
74  * set to be accepted will be "INPUT_CORRESPONDENCES*ransac_maxSetSizePct".
75  */
76  double ransac_maxSetSizePct{0.5};
77  /** (Default=0.05) The maximum distance in X,Y,Z for a solution to be
78  * considered as matching a candidate solution (In meters) */
79  double ransac_threshold_lin{0.05};
80  /** (Default=1 deg) The maximum angle (yaw,pitch,roll) for a solution to be
81  * considered as matching a candidate solution (In radians) */
83  /** (Default=0.03) The maximum difference in scale for a solution to be
84  * considered as matching a candidate solution (dimensionless) */
85  double ransac_threshold_scale{0.03};
86  /** (Default=true) */
87  bool forceScaleToUnity{true};
88  /** (Default=false) */
89  bool verbose{false};
90 
91  /** If provided, this user callback will be invoked to determine the
92  * individual compatibility between each potential pair
93  * of elements. Can check image descriptors, geometrical properties, etc.
94  * \return Must return true if the pair is a potential match, false
95  * otherwise.
96  */
97  // std::function<bool(TPotentialMatch)> user_individual_compat_callback; //
98  // This could be used in the future when we enforce C++11 to users...
100 };
101 
102 /** Output placeholder for se3_l2_robust() */
104 {
105  /** The best transformation found */
107  /** The estimated scale of the rigid transformation (should be very close to
108  * 1.0) */
109  double scale{.0};
110  /** Indexes within the `in_correspondences` list which corresponds with
111  * inliers */
112  std::vector<uint32_t> inliers_idx;
113 };
114 
115 /** Least-squares (L2 norm) solution to finding the optimal SE(3) transform
116  * between two reference frames using RANSAC and the "quaternion" or Horn's
117  * method:
118  * - "Closed-form solution of absolute orientation using unit quaternions",
119  * BKP Horn, Journal of the Optical Society of America, 1987.
120  *
121  * The optimal transformation `q` fulfills \f$ p_{this} = q \oplus p_{other}
122  * \f$, that is, the
123  * transformation of frame `other` with respect to `this`.
124  *
125  * \image html tfest_frames.png
126  *
127  * \param[in] in_correspondences The set of correspondences.
128  * \param[in] in_params Method parameters (see docs for TSE3RobustParams)
129  * \param[out] out_results Results: transformation, scale, etc.
130  *
131  * \return True if the minimum number of correspondences was found, false
132  * otherwise.
133  * \note Implemented by FAMD, 2008. Re-factored by JLBC, 2015.
134  * \note [New in MRPT 1.3.0] This function replaces
135  * mrpt::scanmatching::leastSquareErrorRigidTransformation6DRANSAC()
136  * \sa se2_l2, se3_l2
137  */
138 bool se3_l2_robust(
139  const mrpt::tfest::TMatchingPairList& in_correspondences,
140  const TSE3RobustParams& in_params, TSE3RobustResult& out_results);
141 
142 /** @} */ // end of grouping
143 } // namespace tfest
144 
145 } // namespace mrpt
mrpt::tfest::se3_l2_robust
bool se3_l2_robust(const mrpt::tfest::TMatchingPairList &in_correspondences, const TSE3RobustParams &in_params, TSE3RobustResult &out_results)
Least-squares (L2 norm) solution to finding the optimal SE(3) transform between two reference frames ...
Definition: se3_l2_ransac.cpp:32
mrpt::tfest::TSE3RobustParams
Parameters for se3_l2_robust().
Definition: se3.h:65
mrpt::tfest::TSE3RobustParams::ransac_maxSetSizePct
double ransac_maxSetSizePct
(Default=0.5) The minimum ratio (0.0 - 1.0) of the input set that is considered to be inliers.
Definition: se3.h:76
CMatrixFixedNumeric.h
mrpt::tfest::TSE3RobustParams::verbose
bool verbose
(Default=false)
Definition: se3.h:89
mrpt::tfest::TSE3RobustParams::user_individual_compat_callback
TFunctorCheckPotentialMatch user_individual_compat_callback
If provided, this user callback will be invoked to determine the individual compatibility between eac...
Definition: se3.h:99
mrpt::tfest::TSE3RobustParams::ransac_threshold_lin
double ransac_threshold_lin
(Default=0.05) The maximum distance in X,Y,Z for a solution to be considered as matching a candidate ...
Definition: se3.h:79
mrpt::tfest::TSE3RobustParams::ransac_minSetSize
unsigned int ransac_minSetSize
(Default=5) The minimum amount of points in a set to start a consensus set.
Definition: se3.h:69
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:30
mrpt::tfest::TSE3RobustResult::transformation
mrpt::poses::CPose3DQuat transformation
The best transformation found.
Definition: se3.h:106
mrpt::poses::CPose3DQuat
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,...
Definition: CPose3DQuat.h:48
mrpt::tfest::TFunctorCheckPotentialMatch
std::function< bool(const TPotentialMatch &)> TFunctorCheckPotentialMatch
Definition: indiv-compat-decls.h:31
mrpt::tfest::se3_l2
bool se3_l2(const mrpt::tfest::TMatchingPairList &in_correspondences, mrpt::poses::CPose3DQuat &out_transform, double &out_scale, bool forceScaleToUnity=false)
Least-squares (L2 norm) solution to finding the optimal SE(3) transform between two reference frames ...
Definition: se3_l2.cpp:221
CPose3DQuat.h
TMatchingPair.h
math_frwds.h
mrpt::tfest::TSE3RobustParams::forceScaleToUnity
bool forceScaleToUnity
(Default=true)
Definition: se3.h:87
mrpt::tfest::TSE3RobustParams::ransac_nmaxSimulations
unsigned int ransac_nmaxSimulations
(Default=50) The maximum number of iterations of the RANSAC algorithm
Definition: se3.h:71
mrpt::tfest::TSE3RobustResult::inliers_idx
std::vector< uint32_t > inliers_idx
Indexes within the in_correspondences list which corresponds with inliers.
Definition: se3.h:112
scale
GLenum GLenum GLenum GLenum GLenum scale
Definition: glext.h:6503
mrpt::tfest::TSE3RobustParams::ransac_threshold_ang
double ransac_threshold_ang
(Default=1 deg) The maximum angle (yaw,pitch,roll) for a solution to be considered as matching a cand...
Definition: se3.h:82
poses_frwds.h
mrpt::tfest::TSE3RobustResult
Output placeholder for se3_l2_robust()
Definition: se3.h:103
mrpt::tfest::TMatchingPairList
A list of TMatchingPair.
Definition: TMatchingPair.h:83
indiv-compat-decls.h
mrpt::tfest::TSE3RobustParams::ransac_threshold_scale
double ransac_threshold_scale
(Default=0.03) The maximum difference in scale for a solution to be considered as matching a candidat...
Definition: se3.h:85
mrpt::DEG2RAD
double DEG2RAD(const double x)
Degrees to radians.
Definition: core/include/mrpt/core/bits_math.h:42



Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST