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



Page generated by Doxygen 1.8.14 for MRPT 1.1.0 SVN: at lun oct 28 00:54:49 CET 2019 Hosted on:
SourceForge.net Logo