template struct mrpt::poses::Lie::SE<2>

Overview

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

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

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

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

    typedef mrpt::math::CVectorFixedDouble<DOFs> tangent_vector;
    typedef mrpt::math::CVectorFixedDouble<MANIFOLD_DIM> manifold_vector;
    typedef CPose2D type;
    typedef mrpt::math::TPose2D light_type;
    typedef mrpt::math::CMatrixDouble33 matrix_TxT;
    typedef mrpt::math::CMatrixDouble33 matrix_MxM;
    typedef mrpt::math::CMatrixDouble33 tang2mat_jacob;
    typedef mrpt::math::CMatrixDouble33 mat2tang_jacob;

    // fields

    static constexpr static size_t DOFs = 3;
    static constexpr static size_t MANIFOLD_DIM = 3;

    // 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 matrix_MxM jacob_dAB_dA(const type& A, const type& B);
    static matrix_MxM jacob_dAB_dB(const type& A, const type& B);
    static tang2mat_jacob jacob_dDexpe_de(const type& D);

    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::CMatrixDouble33 matrix_TxT

Type for Jacobians between SO(n) transformations.

typedef mrpt::math::CMatrixDouble33 matrix_MxM

In SE(3), this type represents Jacobians between SO(n) (sub)matrices in the manifold, but in SE(2) it simply models Jacobians between (x,y,phi) vectors.

typedef mrpt::math::CMatrixDouble33 tang2mat_jacob

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

typedef mrpt::math::CMatrixDouble33 mat2tang_jacob

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

Fields

static constexpr static size_t DOFs = 3

Number of actual degrees of freedom for this transformation.

static constexpr static size_t MANIFOLD_DIM = 3

Dimensionality of the matrix manifold; this should be 3x3=9 for SE(2), but for efficiency, we define it as simply 3x1=3 and use the (x,y,phi) representation as well as the “manifold”.

This is done for API consistency with SE(3), where the actual matrix is used instead.

Methods

static type exp(const tangent_vector& x)

Exponential map in SE(2), takes [x,y,phi] and returns a CPose2D.

static tangent_vector log(const type& P)

Logarithm map in SE(2), takes a CPose2D and returns [X,Y, phi].

static manifold_vector asManifoldVector(const type& pose)

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

For SE(2), though, it directly returns the vector [x,y,phi] for efficiency in comparison to 3x3 homogeneous coordinates.

static type fromManifoldVector(const manifold_vector& v)

The inverse operation of asManifoldVector()

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

Jacobian of the pose composition A*B for SE(2) with respect to A.

See appendix B of [5]

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

Jacobian of the pose composition A*B for SE(2) with respect to B.

See appendix B of [5]

static tang2mat_jacob jacob_dDexpe_de(const type& D)

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

See appendix B in [5]

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 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\) increments in the linearized manifold for P1 and P2. See appendix B in [5]