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.
Definition: TMatchingPair.h:67
std::vector< int32_t > vector_int
Definition: types_simple.h:23
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.5 for MRPT 1.4.0 SVN: at Tue Dec 27 00:53:09 UTC 2022