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



Page generated by Doxygen 1.9.7 for MRPT 1.4.0 SVN: at Tue Jun 27 15:23:24 UTC 2023