Main MRPT website > C++ reference for MRPT 1.5.6
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  }; // end SE_traits
63 
64  /** Specialization of SE for 2D poses \sa SE_traits */
65  template <> struct BASE_IMPEXP SE_traits<2>
66  {
67  enum { VECTOR_SIZE = 3 };
70  typedef CPose2D pose_t;
73 
74  /** Exponential map in SE(2) */
75  static inline void exp(const array_t &x, CPose2D &P) { P.x(x[0]); P.y(x[1]); P.phi(x[2]); }
76 
77  /** Pseudo-Exponential map in SE(2), in this case identical to exp() */
78  static inline void pseudo_exp(const array_t &x, CPose2D &P) { exp(x,P); }
79 
80  /** Logarithm map in SE(2) */
81  static inline void ln(const CPose2D &P, array_t &x) { x[0] = P.x(); x[1] = P.y(); x[2] = P.phi(); } //-V537
82 
83  /** A pseudo-Logarithm map in SE(2), where the output = [X,Y, Ln(ROT)], that is, the normal
84  * SO(2) logarithm is used for the rotation components, but the translation is left unmodified.
85  */
86  static inline void pseudo_ln(const CPose2D &P, array_t &x) { ln(P,x); }
87 
88  /** Return one or both of the following 3x3 Jacobians, useful in graph-slam problems:
89  * \f[ \frac{\partial pseudoLn(P_1 D P_2^{-1}) }{\partial \epsilon_1} \f]
90  * \f[ \frac{\partial pseudoLn(P_1 D P_2^{-1}) }{\partial \epsilon_2} \f]
91  * With \f$ \epsilon_1 \f$ and \f$ \epsilon_2 \f$ being increments in the linearized manifold for P1 and P2.
92  */
93  static void jacobian_dP1DP2inv_depsilon(
94  const CPose2D &P1DP2inv,
95  matrix_VxV_t *df_de1,
96  matrix_VxV_t *df_de2);
97 
98  }; // end SE_traits
99 
100  /** @} */ // end of grouping
101 
102  } // End of namespace
103 } // End of namespace
104 
105 #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
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
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:68
static void ln(const CPose2D &P, array_t &x)
Logarithm map in SE(2)
Definition: SE_traits.h:81
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:69
A numeric matrix of compile-time fixed size.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
static void exp(const array_t &x, CPose2D &P)
Exponential map in SE(2)
Definition: SE_traits.h:75
mrpt::math::TPoint2D point_t
Definition: SE_traits.h:72
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
mrpt::math::CArrayDouble< VECTOR_SIZE > array_t
Definition: SE_traits.h:33
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
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:86
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 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:78
Lightweight 3D point.
Lightweight 2D point.
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:71



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018