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



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