template struct mrpt::poses::Lie::SO<3>

Traits for SO(3), rotations in R^3 space.

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

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

    typedef mrpt::math::CVectorFixedDouble<DOFs> tangent_vector;
    typedef mrpt::math::CMatrixDouble33 type;
    typedef mrpt::math::CMatrixDouble93 tang2mat_jacob;
    typedef mrpt::math::CMatrixDouble39 mat2tang_jacob;

    //
fields

    static constexpr static size_t DOFs = 3;

    //
methods

    static type exp(const tangent_vector& x);
    static tangent_vector log(const type& R);
    static type fromYPR(const double yaw, const double pitch, const double roll);
    static tangent_vector vee_RmRt(const type& R);
    static tang2mat_jacob jacob_dexpe_de(const tangent_vector& x);
    static mat2tang_jacob jacob_dlogv_dv(const type& R);
};

Typedefs

typedef mrpt::math::CMatrixDouble93 tang2mat_jacob

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

typedef mrpt::math::CMatrixDouble39 mat2tang_jacob

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

Methods

static type exp(const tangent_vector& x)

SO(3) exponential map \(x \rightarrow \exp(x^\wedge)\).

This is exactly the same than the Rodrigues formula. See 9.4.1 in [4] for the exponential map definition.

  • Input: 3-len vector in Lie algebra so(3)

  • Output: 3x3 rotation matrix in SO(3)

static tangent_vector log(const type& R)

SO(3) logarithm map \(\mathbf{R} \rightarrow \log(\mathbf{R}^\vee)\).

See 10.3.1 in [4]

  • Input: 3x3 rotation matrix in SO(3)

  • Output: 3-len vector in Lie algebra so(3)

static type fromYPR(const double yaw, const double pitch, const double roll)

Returns the 3x3 SO(3) rotation matrix from yaw, pitch, roll angles.

See CPose3D for the axis conventions and a picture.

static tangent_vector vee_RmRt(const type& R)

Returns vee(R-R’), which is an approximation to 2*vee(logmat(R)) for small rotations.

static tang2mat_jacob jacob_dexpe_de(const tangent_vector& x)

Jacobian for exp().

See 10.3.1 in [4]

  • Input: 3-len vector in Lie algebra so(3)

  • Jacobian: Jacobian of the 3x3 matrix (stacked as a column major 9-vector) wrt the vector in the tangent space.

static mat2tang_jacob jacob_dlogv_dv(const type& R)

Jacobian for log() See 10.3.2 in [4].

  • Input: 3x3 rotation matrix in SO(3)

  • Jacobian: Jacobian of the tangent space vector wrt the 3x3 matrix (stacked as a column major 9-vector).