Main MRPT website > C++ reference for MRPT 1.9.9
SE_traits.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 #ifndef MRPT_SE3_TRAITS_H
10 #define MRPT_SE3_TRAITS_H
11 
12 #include <mrpt/poses/CPose3D.h>
13 #include <mrpt/poses/CPose2D.h>
16 
17 namespace mrpt
18 {
19 namespace poses
20 {
21 /** \addtogroup poses_grp
22  * @{ */
23 
24 /** A helper class for SE(2) and SE(3) geometry-related transformations,
25  * on-manifold optimization Jacobians, etc.
26  * \sa SE_traits<2>, SE_traits<3>, CPose3D, CPose2D
27  */
28 template <size_t DOF>
29 struct SE_traits;
30 
31 /** Specialization of SE for 3D poses \sa SE_traits */
32 template <>
33 struct SE_traits<3>
34 {
35  constexpr static size_t VECTOR_SIZE = 6;
37  using matrix_VxV_t =
39  using pose_t = CPose3D;
42 
43  /** Exponential map in SE(3), with XYZ different from the first three values
44  * of "x" \sa pseudo_exp */
45  static inline void exp(const array_t& x, CPose3D& P)
46  {
47  CPose3D::exp(x, P, false);
48  }
49 
50  /** Pseudo-Exponential map in SE(3), with XYZ copied from the first three
51  * values of "x" \sa exp */
52  static inline void pseudo_exp(const array_t& x, CPose3D& P)
53  {
54  CPose3D::exp(x, P, true);
55  }
56 
57  /** Logarithm map in SE(3) */
58  static inline void ln(const CPose3D& P, array_t& x) { P.ln(x); }
59  /** A pseudo-Logarithm map in SE(3), where the output = [X,Y,Z, Ln(ROT)],
60  * that is, the normal
61  * SO(3) logarithm is used for the rotation components, but the
62  * translation is left unmodified. */
63  static void pseudo_ln(const CPose3D& P, array_t& x);
64 
65  /** Return one or both of the following 6x6 Jacobians, useful in graph-slam
66  * problems:
67  * \f[ \frac{\partial pseudoLn(P_1 D P_2^{-1}) }{\partial \epsilon_1}
68  * \f]
69  * \f[ \frac{\partial pseudoLn(P_1 D P_2^{-1}) }{\partial \epsilon_2}
70  * \f]
71  * With \f$ \epsilon_1 \f$ and \f$ \epsilon_2 \f$ being increments in the
72  * linearized manifold for P1 and P2.
73  */
74  static void jacobian_dP1DP2inv_depsilon(
75  const CPose3D& P1DP2inv, matrix_VxV_t* df_de1, matrix_VxV_t* df_de2);
76 
77 }; // end SE_traits
78 
79 /** Specialization of SE for 2D poses \sa SE_traits */
80 template <>
81 struct SE_traits<2>
82 {
83  constexpr static size_t VECTOR_SIZE = 3;
85  using matrix_VxV_t =
87  using pose_t = CPose2D;
90 
91  /** Exponential map in SE(2) */
92  static inline void exp(const array_t& x, CPose2D& P)
93  {
94  P.x(x[0]);
95  P.y(x[1]);
96  P.phi(x[2]);
97  }
98 
99  /** Pseudo-Exponential map in SE(2), in this case identical to exp() */
100  static inline void pseudo_exp(const array_t& x, CPose2D& P) { exp(x, P); }
101  /** Logarithm map in SE(2) */
102  static inline void ln(const CPose2D& P, array_t& x)
103  {
104  x[0] = P.x();
105  x[1] = P.y();
106  x[2] = P.phi();
107  } //-V537
108 
109  /** A pseudo-Logarithm map in SE(2), where the output = [X,Y, Ln(ROT)], that
110  * is, the normal
111  * SO(2) logarithm is used for the rotation components, but the
112  * translation is left unmodified.
113  */
114  static inline void pseudo_ln(const CPose2D& P, array_t& x) { ln(P, x); }
115  /** Return one or both of the following 3x3 Jacobians, useful in graph-slam
116  * problems:
117  * \f[ \frac{\partial pseudoLn(P_1 D P_2^{-1}) }{\partial \epsilon_1}
118  * \f]
119  * \f[ \frac{\partial pseudoLn(P_1 D P_2^{-1}) }{\partial \epsilon_2}
120  * \f]
121  * With \f$ \epsilon_1 \f$ and \f$ \epsilon_2 \f$ being increments in the
122  * linearized manifold for P1 and P2.
123  */
124  static void jacobian_dP1DP2inv_depsilon(
125  const CPose2D& P1DP2inv, matrix_VxV_t* df_de1, matrix_VxV_t* df_de2);
126 
127 }; // end SE_traits
128 
129 /** @} */ // end of grouping
130 
131 } // namespace poses
132 } // namespace mrpt
133 
134 #endif
static void exp(const array_t &x, CPose3D &P)
Exponential map in SE(3), with XYZ different from the first three values of "x".
Definition: SE_traits.h:45
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...
Definition: CArrayNumeric.h:25
static void ln(const CPose2D &P, array_t &x)
Logarithm map in SE(2)
Definition: SE_traits.h:102
static void ln(const CPose3D &P, array_t &x)
Logarithm map in SE(3)
Definition: SE_traits.h:58
A numeric matrix of compile-time fixed size.
static void exp(const array_t &x, CPose2D &P)
Exponential map in SE(2)
Definition: SE_traits.h:92
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:40
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:82
A helper class for SE(2) and SE(3) geometry-related transformations, on-manifold optimization Jacobia...
Definition: SE_traits.h:29
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Lightweight 2D pose.
static void pseudo_exp(const array_t &x, CPose3D &P)
Pseudo-Exponential map in SE(3), with XYZ copied from the first three values of "x".
Definition: SE_traits.h:52
static void pseudo_ln(const CPose2D &P, array_t &x)
A pseudo-Logarithm map in SE(2), where the output = [X,Y, Ln(ROT)], that is, the normal SO(2) logarit...
Definition: SE_traits.h:114
static void pseudo_exp(const array_t &x, CPose2D &P)
Pseudo-Exponential map in SE(2), in this case identical to exp()
Definition: SE_traits.h:100
GLenum GLint x
Definition: glext.h:3538
Lightweight 3D point.
Lightweight 2D point.
void ln(mrpt::math::CArrayDouble< 6 > &out_ln) const
Take the logarithm of the 3x4 matrix defined by this pose, generating the corresponding vector in the...
Definition: CPose3D.cpp:801
static CPose3D exp(const mrpt::math::CArrayNumeric< double, 6 > &vect, bool pseudo_exponential=false)
Exponentiate a Vector in the SE(3) Lie Algebra to generate a new CPose3D (static method).
Definition: CPose3D.cpp:758



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019