Main MRPT website > C++ reference for MRPT 1.5.7
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-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 #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, on-manifold optimization Jacobians, etc.
25  * \sa SE_traits<2>, SE_traits<3>, CPose3D, CPose2D
26  */
27  template <size_t DOF> struct BASE_IMPEXP SE_traits;
28 
29  /** Specialization of SE for 3D poses \sa SE_traits */
30  template <> struct BASE_IMPEXP SE_traits<3>
31  {
32  enum { VECTOR_SIZE = 6 };
35  typedef CPose3D pose_t;
38 
39  /** Exponential map in SE(3), with XYZ different from the first three values of "x" \sa pseudo_exp */
40  static inline void exp(const array_t &x, CPose3D &P) { CPose3D::exp(x, P, false); }
41 
42  /** Pseudo-Exponential map in SE(3), with XYZ copied from the first three values of "x" \sa exp */
43  static inline void pseudo_exp(const array_t &x, CPose3D &P) { CPose3D::exp(x, P, true); }
44 
45  /** Logarithm map in SE(3) */
46  static inline void ln(const CPose3D &P, array_t &x) { P.ln(x); }
47 
48  /** A pseudo-Logarithm map in SE(3), where the output = [X,Y,Z, Ln(ROT)], that is, the normal
49  * SO(3) logarithm is used for the rotation components, but the translation is left unmodified. */
50  static void pseudo_ln(const CPose3D &P, array_t &x);
51 
52  /** Return one or both of the following 6x6 Jacobians, useful in graph-slam problems:
53  * \f[ \frac{\partial pseudoLn(P_1 D P_2^{-1}) }{\partial \epsilon_1} \f]
54  * \f[ \frac{\partial pseudoLn(P_1 D P_2^{-1}) }{\partial \epsilon_2} \f]
55  * With \f$ \epsilon_1 \f$ and \f$ \epsilon_2 \f$ being increments in the linearized manifold for P1 and P2.
56  */
57  static void jacobian_dP1DP2inv_depsilon(
58  const CPose3D &P1DP2inv,
59  matrix_VxV_t *df_de1,
60  matrix_VxV_t *df_de2);
61 
62  /** Return one or both of the following 3x3 Jacobians, useful in graph-slam
63  * problems:
64  * \f[ \frac{\partial pseudoLn(D^{-1} P_1^{-1} P_2}{\partial \epsilon_1} \f]
65  * \f[ \frac{\partial pseudoLn(D^{-1} P_1^{-1} P_2}{\partial \epsilon_1} \f]
66  * With \f$ \epsilon_1 \f$ and \f$ \epsilon_2 \f$ being increments in the linearized manifold for P1 and P2.
67  */
68  static void jacobian_dDinvP1invP2_depsilon(
69  const CPose3D& Dinv, const CPose3D& P1, const CPose3D& P2,
70  matrix_VxV_t* df_de1, matrix_VxV_t* df_de2);
71 
72  }; // end SE_traits
73 
74  /** Specialization of SE for 2D poses \sa SE_traits */
75  template <> struct BASE_IMPEXP SE_traits<2>
76  {
77  enum { VECTOR_SIZE = 3 };
80  typedef CPose2D pose_t;
83 
84  /** Exponential map in SE(2) */
85  static inline void exp(const array_t &x, CPose2D &P) { P.x(x[0]); P.y(x[1]); P.phi(x[2]); }
86 
87  /** Pseudo-Exponential map in SE(2), in this case identical to exp() */
88  static inline void pseudo_exp(const array_t &x, CPose2D &P) { exp(x,P); }
89 
90  /** Logarithm map in SE(2) */
91  static inline void ln(const CPose2D &P, array_t &x) { x[0] = P.x(); x[1] = P.y(); x[2] = P.phi(); } //-V537
92 
93  /** A pseudo-Logarithm map in SE(2), where the output = [X,Y, Ln(ROT)], that is, the normal
94  * SO(2) logarithm is used for the rotation components, but the translation is left unmodified.
95  */
96  static inline void pseudo_ln(const CPose2D &P, array_t &x) { ln(P,x); }
97 
98  /** Return one or both of the following 3x3 Jacobians, useful in graph-slam problems:
99  * \f[ \frac{\partial pseudoLn(P_1 D P_2^{-1}) }{\partial \epsilon_1} \f]
100  * \f[ \frac{\partial pseudoLn(P_1 D P_2^{-1}) }{\partial \epsilon_2} \f]
101  * With \f$ \epsilon_1 \f$ and \f$ \epsilon_2 \f$ being increments in the linearized manifold for P1 and P2.
102  */
103  static void jacobian_dP1DP2inv_depsilon(
104  const CPose2D &P1DP2inv,
105  matrix_VxV_t *df_de1,
106  matrix_VxV_t *df_de2);
107 
108  /** Return one or both of the following 3x3 Jacobians, useful in graph-slam
109  * problems:
110  * \f[ \frac{\partial pseudoLn(D^{-1} P_1^{-1} P_2}{\partial \epsilon_1} \f]
111  * \f[ \frac{\partial pseudoLn(D^{-1} P_1^{-1} P_2}{\partial \epsilon_1} \f]
112  * With \f$ \epsilon_1 \f$ and \f$ \epsilon_2 \f$ being increments in the linearized manifold for P1 and P2.
113  */
114  static void jacobian_dDinvP1invP2_depsilon(
115  const CPose2D& Dinv, const CPose2D& P1, const CPose2D& P2,
116  matrix_VxV_t* df_de1, matrix_VxV_t* df_de2);
117 
118  }; // end SE_traits
119 
120  /** @} */ // end of grouping
121 
122  } // End of namespace
123 } // End of namespace
124 
125 #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:40
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
mrpt::math::CMatrixFixedNumeric< double, VECTOR_SIZE, VECTOR_SIZE > matrix_VxV_t
Definition: SE_traits.h:34
mrpt::math::CArrayDouble< VECTOR_SIZE > array_t
Definition: SE_traits.h:78
static void ln(const CPose2D &P, array_t &x)
Logarithm map in SE(2)
Definition: SE_traits.h:91
static void ln(const CPose3D &P, array_t &x)
Logarithm map in SE(3)
Definition: SE_traits.h:46
mrpt::math::CMatrixFixedNumeric< double, VECTOR_SIZE, VECTOR_SIZE > matrix_VxV_t
Definition: SE_traits.h:79
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:85
mrpt::math::TPoint2D point_t
Definition: SE_traits.h:82
mrpt::math::CArrayDouble< VECTOR_SIZE > array_t
Definition: SE_traits.h:33
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
struct BASE_IMPEXP SE_traits
A helper class for SE(2) and SE(3) geometry-related transformations, on-manifold optimization Jacobia...
Definition: SE_traits.h:27
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
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:43
A partial specialization of CArrayNumeric for double numbers.
Definition: CArrayNumeric.h:74
mrpt::math::TPose3D lightweight_pose_t
Definition: SE_traits.h:36
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:96
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:88
GLenum GLint x
Definition: glext.h:3516
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:793
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:757
mrpt::math::TPoint3D point_t
Definition: SE_traits.h:37
mrpt::math::TPose2D lightweight_pose_t
Definition: SE_traits.h:81



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