9 #ifndef MRPT_SE3_TRAITS_H
10 #define MRPT_SE3_TRAITS_H
31 enum { VECTOR_SIZE = 6 };
43 static inline void ln(
const CPose3D &P, array_t &x) { P.
ln(x); }
47 static void pseudo_ln(
const CPose3D &P, array_t &x);
54 static void jacobian_dP1DP2inv_depsilon(
57 matrix_VxV_t *df_de2);
64 enum { VECTOR_SIZE = 3 };
70 static inline void exp(
const array_t &x,
CPose2D &P) { P.
x(x[0]); P.
y(x[1]); P.
phi(x[2]); }
76 static inline void ln(
const CPose2D &P, array_t &x) { x[0] = P.
x(); x[1] = P.
y(); x[2] = P.
phi(); }
88 static void jacobian_dP1DP2inv_depsilon(
91 matrix_VxV_t *df_de2);
static void exp(const array_t &x, CPose3D &P)
Exponential map in SE(3), with XYZ different from the first three values of "x".
const double & phi() const
Get the phi angle of the 2D pose (in radians)
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...
mrpt::math::CMatrixFixedNumeric< double, VECTOR_SIZE, VECTOR_SIZE > matrix_VxV_t
mrpt::math::CArrayDouble< VECTOR_SIZE > array_t
static void ln(const CPose2D &P, array_t &x)
Logarithm map in SE(2)
static void ln(const CPose3D &P, array_t &x)
Logarithm map in SE(3)
mrpt::math::CMatrixFixedNumeric< double, VECTOR_SIZE, VECTOR_SIZE > matrix_VxV_t
A numeric matrix of compile-time fixed size.
double x() const
Common members of all points & poses classes.
static void exp(const array_t &x, CPose2D &P)
Exponential map in SE(2)
mrpt::math::CArrayDouble< VECTOR_SIZE > array_t
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...
A class used to store a 2D pose.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
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".
A partial specialization of CArrayNumeric for double numbers.
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...
static void pseudo_exp(const array_t &x, CPose2D &P)
Pseudo-Exponential map in SE(2), in this case identical to exp()
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).