Main MRPT website > C++ reference for MRPT 1.4.0
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-2016, 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
16namespace 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 };
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 */
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 };
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(); } //-V537
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 */
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
A partial specialization of CArrayNumeric for double numbers.
Definition: CArrayNumeric.h:75
A numeric matrix of compile-time fixed size.
A class used to store a 2D pose.
Definition: CPose2D.h:37
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:84
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:73
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...
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
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
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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 void jacobian_dP1DP2inv_depsilon(const CPose2D &P1DP2inv, matrix_VxV_t *df_de1, matrix_VxV_t *df_de2)
Return one or both of the following 3x3 Jacobians, useful in graph-slam problems:
static void ln(const CPose2D &P, array_t &x)
Logarithm map in SE(2)
Definition: SE_traits.h:76
mrpt::math::CArrayDouble< VECTOR_SIZE > array_t
Definition: SE_traits.h:65
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
mrpt::math::CMatrixFixedNumeric< double, VECTOR_SIZE, VECTOR_SIZE > matrix_VxV_t
Definition: SE_traits.h:66
static void exp(const array_t &x, CPose2D &P)
Exponential map in SE(2)
Definition: SE_traits.h:70
static void jacobian_dP1DP2inv_depsilon(const CPose3D &P1DP2inv, matrix_VxV_t *df_de1, matrix_VxV_t *df_de2)
Return one or both of the following 6x6 Jacobians, useful in graph-slam problems:
mrpt::math::CArrayDouble< VECTOR_SIZE > array_t
Definition: SE_traits.h:32
mrpt::math::CMatrixFixedNumeric< double, VECTOR_SIZE, VECTOR_SIZE > matrix_VxV_t
Definition: SE_traits.h:33
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
static void pseudo_ln(const CPose3D &P, array_t &x)
A pseudo-Logarithm map in SE(3), where the output = [X,Y,Z, Ln(ROT)], that is, the normal SO(3) logar...
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 ln(const CPose3D &P, array_t &x)
Logarithm map in SE(3)
Definition: SE_traits.h:43



Page generated by Doxygen 1.9.6 for MRPT 1.4.0 SVN: at Wed Mar 22 09:54:56 UTC 2023