template struct mrpt::poses::Lie::SE<3>

Traits for SE(3), rigid-body transformations in R^3 space.

See indidual members for documentation, or [4] for a general overview.

#include <mrpt/poses/Lie/SE.h>

template <>
struct SE<3>
{
    // typedefs

    typedef mrpt::math::CVectorFixedDouble<DOFs> tangent_vector;
    typedef mrpt::math::CVectorFixedDouble<MANIFOLD_DIM> manifold_vector;
    typedef CPose3D type;
    typedef mrpt::math::TPose3D light_type;
    typedef mrpt::math::CMatrixDouble12_6 tang2mat_jacob;
    typedef mrpt::math::CMatrixDouble6_12 mat2tang_jacob;
    typedef mrpt::math::CMatrixDouble66 matrix_TxT;
    typedef mrpt::math::CMatrixFixed<double, 12, 12> matrix_MxM;

    //
fields

    static constexpr static size_t DOFs = 6;
    static constexpr static size_t MANIFOLD_DIM = 3* 4;

    //
methods

    static type exp(const tangent_vector& x);
    static tangent_vector log(const type& P);
    static manifold_vector asManifoldVector(const type& pose);
    static type fromManifoldVector(const manifold_vector& v);
    static tang2mat_jacob jacob_dexpe_de(const tangent_vector& x);
    static mat2tang_jacob jacob_dlogv_dv(const type& P);
    static tang2mat_jacob jacob_dexpeD_de(const CPose3D& D);
    static tang2mat_jacob jacob_dDexpe_de(const CPose3D& D);
    static tang2mat_jacob jacob_dAexpeD_de(const CPose3D& A, const CPose3D& D);
    static matrix_MxM jacob_dAB_dA(const type& A, const type& B);
    static matrix_MxM jacob_dAB_dB(const type& A, const type& B);

    static void jacob_dDinvP1invP2_de1e2(
        const type& Dinv,
        const type& P1,
        const type& P2,
        mrpt::optional_ref<matrix_TxT> df_de1 = std::nullopt,
        mrpt::optional_ref<matrix_TxT> df_de2 = std::nullopt
        );
};

Typedefs

typedef mrpt::math::CMatrixDouble12_6 tang2mat_jacob

Type for Jacobian: tangent space -> SO(n) matrix.

typedef mrpt::math::CMatrixDouble6_12 mat2tang_jacob

Type for Jacobian: SO(n) matrix -> tangent space.

typedef mrpt::math::CMatrixDouble66 matrix_TxT

Type for Jacobians between so(n) vectors on the tangent space.

typedef mrpt::math::CMatrixFixed<double, 12, 12> matrix_MxM

Type for Jacobians between SO(n) 3x4 (sub)matrices in the manifold.

Fields

static constexpr static size_t DOFs = 6

Number of actual degrees of freedom for this transformation.

static constexpr static size_t MANIFOLD_DIM = 3* 4

Dimensionality of the matrix manifold (3x4=12 upper part of the 4x4)

Methods

static type exp(const tangent_vector& x)

Retraction to SE(3), a pseudo-exponential map \(x \rightarrow PseudoExp(x^\wedge)\) and its Jacobian.

Input: 6-len vector in Lie algebra se(3) [x,y,z, rx,ry,rz]
Output: translation and rotation in SE(3) as CPose3D Note that this method implements retraction via a pseudo-exponential, where only the rotational part undergoes a real matrix exponential, while the translation is left unmodified. This is done for computational efficiency, and does not change the results of optimizations as long as the corresponding local coordinates (pseudo-logarithm) are used as well.

See section 9.4.2 in [4]

static tangent_vector log(const type& P)

SE(3) pseudo-logarithm map \(\mathbf{R} \rightarrow \log(\mathbf{R}^\vee)\).

Input: translation and rotation in SE(3) as CPose3D
Output: 6-len vector in Lie algebra se(3) [x,y,z, rx,ry,rz]

See exp() for the explanation about the “pseudo” name. For the formulas, see section 9.4.2 in [4]

static manifold_vector asManifoldVector(const type& pose)

Returns a vector with all manifold matrix elements in column-major order.

For SE(3), it is a 3x4=12 vector.

static type fromManifoldVector(const manifold_vector& v)

The inverse operation of asManifoldVector()

static tang2mat_jacob jacob_dexpe_de(const tangent_vector& x)

Jacobian for the pseudo-exponential exp().

See 10.3.1 in [4]

Input: 6-len vector in Lie algebra se(3)
Jacobian: Jacobian of the 3x4 matrix (stacked as a column major 12-vector) wrt the vector in the tangent space.

static mat2tang_jacob jacob_dlogv_dv(const type& P)

Jacobian for the pseudo-logarithm log() See 10.3.11 in [4].

Input: a SE(3) pose as a CPose3D object
Jacobian: Jacobian of the tangent space vector wrt the 3x4 matrix elements (stacked as a column major 12-vector).

static tang2mat_jacob jacob_dexpeD_de(const CPose3D& D)

Jacobian d (e^eps * D) / d eps , with eps=increment in Lie Algebra.

Section 10.3.3 in [4]

static tang2mat_jacob jacob_dDexpe_de(const CPose3D& D)

Jacobian d (D * e^eps) / d eps , with eps=increment in Lie Algebra.

Section 10.3.4 in [4]

static tang2mat_jacob jacob_dAexpeD_de(const CPose3D& A, const CPose3D& D)

Jacobian d (A * e^eps * D) / d eps , with eps=increment in Lie Algebra.

Eq. 10.3.7 in [4]

static matrix_MxM jacob_dAB_dA(const type& A, const type& B)

Jacobian of the pose composition A*B for SE(3) 3x4 (sub)matrices, with respect to A.

See section 7.3.1 of in [4]

static matrix_MxM jacob_dAB_dB(const type& A, const type& B)

Jacobian of the pose composition A*B for SE(3) 3x4 (sub)matrices, with respect to B.

See section 7.3.1 of in [4]

static void jacob_dDinvP1invP2_de1e2(
    const type& Dinv,
    const type& P1,
    const type& P2,
    mrpt::optional_ref<matrix_TxT> df_de1 = std::nullopt,
    mrpt::optional_ref<matrix_TxT> df_de2 = std::nullopt
    )

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

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

With \(\epsilon_1\) and \(\epsilon_2\) increments in the linearized manifold for P1 and P2. Section 10.3.10 in [4]