MRPT  1.9.9
mrpt::poses::SE_traits< 3 > Struct Template Reference

Detailed Description

template<>
struct mrpt::poses::SE_traits< 3 >

Specialization of SE for 3D poses.

See also
SE_traits

Definition at line 31 of file SE_traits.h.

#include <mrpt/poses/SE_traits.h>

Public Types

using array_t = mrpt::math::CArrayDouble< VECTOR_SIZE >
 
using matrix_VxV_t = mrpt::math::CMatrixFixedNumeric< double, VECTOR_SIZE, VECTOR_SIZE >
 
using pose_t = CPose3D
 
using lightweight_pose_t = mrpt::math::TPose3D
 
using point_t = mrpt::math::TPoint3D
 

Static Public Member Functions

static void exp (const array_t &x, CPose3D &P)
 Exponential map in SE(3), with XYZ different from the first three values of "x". More...
 
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". More...
 
static void ln (const CPose3D &P, array_t &x)
 Logarithm map in SE(3) More...
 
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) logarithm is used for the rotation components, but the translation is left unmodified. More...
 
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:

\[ \frac{\partial pseudoLn(P_1 D P_2^{-1}) }{\partial \epsilon_1} \]

\[ \frac{\partial pseudoLn(P_1 D P_2^{-1}) }{\partial \epsilon_2} \]

With $ \epsilon_1 $ and $ \epsilon_2 $ being increments in the linearized manifold for P1 and P2. More...

 
static void jacobian_dDinvP1invP2_depsilon (const CPose3D &Dinv, const CPose3D &P1, const CPose3D &P2, matrix_VxV_t *df_de1, matrix_VxV_t *df_de2)
 Return one or both of the following 3x3 Jacobians, useful in graph-slam problems:

\[ \frac{\partial pseudoLn(D^{-1} P_1^{-1} P_2}{\partial \epsilon_1} \]

\[ \frac{\partial pseudoLn(D^{-1} P_1^{-1} P_2}{\partial \epsilon_1} \]

With $ \epsilon_1 $ and $ \epsilon_2 $ being increments in the linearized manifold for P1 and P2. More...

 

Static Public Attributes

static constexpr size_t VECTOR_SIZE = 6
 

Member Typedef Documentation

◆ array_t

◆ lightweight_pose_t

Definition at line 38 of file SE_traits.h.

◆ matrix_VxV_t

◆ point_t

Definition at line 39 of file SE_traits.h.

◆ pose_t

Definition at line 37 of file SE_traits.h.

Member Function Documentation

◆ exp()

static void mrpt::poses::SE_traits< 3 >::exp ( const array_t x,
CPose3D P 
)
inlinestatic

Exponential map in SE(3), with XYZ different from the first three values of "x".

See also
pseudo_exp

Definition at line 43 of file SE_traits.h.

References mrpt::poses::CPose3D::exp().

◆ jacobian_dDinvP1invP2_depsilon()

static void mrpt::poses::SE_traits< 3 >::jacobian_dDinvP1invP2_depsilon ( const CPose3D Dinv,
const CPose3D P1,
const CPose3D P2,
matrix_VxV_t df_de1,
matrix_VxV_t df_de2 
)
static

Return one or both of the following 3x3 Jacobians, useful in graph-slam problems:

\[ \frac{\partial pseudoLn(D^{-1} P_1^{-1} P_2}{\partial \epsilon_1} \]

\[ \frac{\partial pseudoLn(D^{-1} P_1^{-1} P_2}{\partial \epsilon_1} \]

With $ \epsilon_1 $ and $ \epsilon_2 $ being increments in the linearized manifold for P1 and P2.

◆ jacobian_dP1DP2inv_depsilon()

static void mrpt::poses::SE_traits< 3 >::jacobian_dP1DP2inv_depsilon ( const CPose3D P1DP2inv,
matrix_VxV_t df_de1,
matrix_VxV_t df_de2 
)
static

Return one or both of the following 6x6 Jacobians, useful in graph-slam problems:

\[ \frac{\partial pseudoLn(P_1 D P_2^{-1}) }{\partial \epsilon_1} \]

\[ \frac{\partial pseudoLn(P_1 D P_2^{-1}) }{\partial \epsilon_2} \]

With $ \epsilon_1 $ and $ \epsilon_2 $ being increments in the linearized manifold for P1 and P2.

◆ ln()

static void mrpt::poses::SE_traits< 3 >::ln ( const CPose3D P,
array_t x 
)
inlinestatic

Logarithm map in SE(3)

Definition at line 56 of file SE_traits.h.

References mrpt::poses::CPose3D::ln().

◆ pseudo_exp()

static void mrpt::poses::SE_traits< 3 >::pseudo_exp ( const array_t x,
CPose3D P 
)
inlinestatic

Pseudo-Exponential map in SE(3), with XYZ copied from the first three values of "x".

See also
exp

Definition at line 50 of file SE_traits.h.

References mrpt::poses::CPose3D::exp().

◆ pseudo_ln()

static void mrpt::poses::SE_traits< 3 >::pseudo_ln ( const CPose3D P,
array_t x 
)
static

A pseudo-Logarithm map in SE(3), where the output = [X,Y,Z, Ln(ROT)], that is, the normal SO(3) logarithm is used for the rotation components, but the translation is left unmodified.

Member Data Documentation

◆ VECTOR_SIZE

constexpr size_t mrpt::poses::SE_traits< 3 >::VECTOR_SIZE = 6
static

Definition at line 33 of file SE_traits.h.




Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020