Main MRPT website > C++ reference for MRPT 1.5.7
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-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/CPose3DQuat.h>
17 #include <mrpt/poses/poses_frwds.h>
19 
20 namespace 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  */
42  bool TFEST_IMPEXP se3_l2(
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  */
52  bool TFEST_IMPEXP se3_l2(
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 
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
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
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 ...
Parameters for se3_l2_robust().
Definition: se3.h:60
GLenum GLenum GLenum GLenum GLenum scale
Definition: glext.h:5717
bool forceScaleToUnity
(Default=true)
Definition: se3.h:68
bool verbose
(Default=false)
Definition: se3.h:69
unsigned int ransac_nmaxSimulations
(Default=50) The maximum number of iterations of the RANSAC algorithm
Definition: se3.h:63
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
mrpt::vector_int inliers_idx
Indexes within the in_correspondences list which corresponds with inliers.
Definition: se3.h:99
A list of TMatchingPair.
Definition: TMatchingPair.h:78
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 ...
Definition: se3_l2.cpp:201
double scale
The estimated scale of the rigid transformation (should be very close to 1.0)
Definition: se3.h:98
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:41
#define DEG2RAD
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
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
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
unsigned int ransac_minSetSize
(Default=5) The minimum amount of points in a set to start a consensus set.
Definition: se3.h:62
mrpt::poses::CPose3DQuat transformation
The best transformation found.
Definition: se3.h:97
void * user_individual_compat_callback_userdata
User data to be passed to user_individual_compat_callback()
Definition: se3.h:77
Output placeholder for se3_l2_robust()
Definition: se3.h:95
bool(* TFunctorCheckPotentialMatch)(const TPotentialMatch &pm, void *user_data)
std::vector< int32_t > vector_int



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019