template class sophus::Se3

Overview

Se3 using default storage; derived from Se3Base. More…

#include <se3.h>

template <class TScalar, int kOptions>
class Se3 {
public:
    // typedefs

    using Scalar = TScalar ;
    using Transformation = typename Base::Transformation ;
    using Point = typename Base::Point ;
    using HomogeneousPoint = typename Base::HomogeneousPoint ;
    using Tangent = typename Base::Tangent ;
    using Adjoint = typename Base::Adjoint ;
    using SO3Member = So3<Scalar, kOptions> ;
    using TranslationMember = Eigen::Matrix<Scalar, 3, 1, kOptions> ;

    // fields

    static constexpr int kDoF = Base::kDoF;
    static constexpr int kNumParameters = Base::kNumParameters;

    // construction

    Se3();
    Se3(Se3 const& other);

    template <class TOtherDerived>
    Se3(Se3Base<TOtherDerived> const& other);

    template <class TOtherDerived, class TD>
    Se3(
        So3Base<TOtherDerived> const& so3,
        Eigen::MatrixBase<TD> const& translation
        );

    Se3(Eigen::Matrix3<Scalar> const& rotation_matrix, Point const& translation);
    Se3(Eigen::Quaternion<Scalar> const& quaternion, Point const& translation);
    Se3(Eigen::Matrix4<Scalar> const& mat_t);

    // methods

    SOPHUS_FUNC Se3& operator=(Se3 const& other);
    SOPHUS_FUNC Scalar* data();
    SOPHUS_FUNC Scalar const* data() const;
    SOPHUS_FUNC SO3Member& so3();
    SOPHUS_FUNC SO3Member const& so3() const;
    SOPHUS_FUNC TranslationMember& translation();
    SOPHUS_FUNC TranslationMember const& translation() const;

    template <class TOtherDerived>
    SOPHUS_FUNC Se3Base<TDerived>& operator=(Se3Base<TOtherDerived> const& other);

    static SOPHUS_FUNC Eigen::Matrix3<Scalar> jacobianUpperRightBlock(
        Eigen::Vector3<Scalar> const& vec_upsilon,
        Eigen::Vector3<Scalar> const& vec_omega
        );

    static SOPHUS_FUNC Eigen::Matrix<Scalar, kDoF, kDoF> leftJacobian(Tangent const& vec_upsilon_omega);
    static SOPHUS_FUNC Eigen::Matrix<Scalar, kDoF, kDoF> leftJacobianInverse(Tangent const& vec_upsilon_omega);
    static SOPHUS_FUNC Eigen::Matrix<Scalar, kNumParameters, kDoF> dxExpX(Tangent const& upsilon_omega);
    static SOPHUS_FUNC Eigen::Matrix<Scalar, kNumParameters, kDoF> dxExpXAt0();
    static SOPHUS_FUNC Transformation dxiExpmatXAt0(int i);
    static SOPHUS_FUNC Eigen::Matrix<Scalar, 3, kDoF> dxExpXTimesPointAt0(Point const& point);
    static SOPHUS_FUNC Se3<Scalar> exp(Tangent const& vec_a);

    template <class TS = Scalar>
    static SOPHUS_FUNC std::enable_if_t<std::is_floating_point<TS>::value, Se3> fitToSe3(Eigen::Matrix4<Scalar> const& t);

    static SOPHUS_FUNC Transformation generator(int i);
    static SOPHUS_FUNC Transformation hat(Tangent const& a);
    static SOPHUS_FUNC Tangent lieBracket(Tangent const& a, Tangent const& b);
    static SOPHUS_FUNC Se3 rotX(Scalar const& x);
    static SOPHUS_FUNC Se3 rotY(Scalar const& y);
    static SOPHUS_FUNC Se3 rotZ(Scalar const& z);

    template <class TUniformRandomBitGenerator>
    static Se3 sampleUniform(TUniformRandomBitGenerator& generator);

    template <class TX, class TY, class TZ>
    static SOPHUS_FUNC Se3 trans(
        TX const& x,
        TY const& y,
        TZ const& z
        );

    static SOPHUS_FUNC Se3 trans(Eigen::Vector3<Scalar> const& xyz);
    static SOPHUS_FUNC Se3 transX(Scalar const& x);
    static SOPHUS_FUNC Se3 transY(Scalar const& y);
    static SOPHUS_FUNC Se3 transZ(Scalar const& z);
    static SOPHUS_FUNC Tangent vee(Transformation const& omega);

protected:
    // fields

    SO3Member so3_;
    TranslationMember translation_;
};

Detailed Documentation

Se3 using default storage; derived from Se3Base.

Construction

Se3()

Default constructor initializes rigid body motion to the identity.

Se3(Se3 const& other)

Copy constructor.

template <class TOtherDerived>
Se3(Se3Base<TOtherDerived> const& other)

Copy-like constructor from OtherDerived.

template <class TOtherDerived, class TD>
Se3(
    So3Base<TOtherDerived> const& so3,
    Eigen::MatrixBase<TD> const& translation
    )

Constructor from So3 and translation vector.

Se3(Eigen::Matrix3<Scalar> const& rotation_matrix, Point const& translation)

Constructor from rotation matrix and translation vector.

Precondition: Rotation matrix needs to be orthogonal with determinant of 1.

Se3(Eigen::Quaternion<Scalar> const& quaternion, Point const& translation)

Constructor from quaternion and translation vector.

Precondition: quaternion must not be close to zero.

Se3(Eigen::Matrix4<Scalar> const& mat_t)

Constructor from 4x4 matrix.

Precondition: Rotation matrix needs to be orthogonal with determinant of 1. The last row must be (0, 0, 0, 1).

Methods

SOPHUS_FUNC Se3& operator=(Se3 const& other)

Define copy-assignment operator explicitly. The definition of implicit copy assignment operator is deprecated in presence of a user-declared copy constructor (-Wdeprecated-copy in clang >= 13).

SOPHUS_FUNC Scalar* data()

This provides unsafe read/write access to internal data. SO(3) is represented by an Eigen::Quaternion (four parameters). When using direct write access, the user needs to take care of that the quaternion stays normalized.

SOPHUS_FUNC Scalar const* data() const

Const version of data() above.

SOPHUS_FUNC SO3Member& so3()

Mutator of So3.

SOPHUS_FUNC SO3Member const& so3() const

Accessor of So3.

SOPHUS_FUNC TranslationMember& translation()

Mutator of translation vector.

SOPHUS_FUNC TranslationMember const& translation() const

Accessor of translation vector.

template <class TOtherDerived>
SOPHUS_FUNC Se3Base<TDerived>& operator=(Se3Base<TOtherDerived> const& other)

Assignment-like operator from OtherDerived.

static SOPHUS_FUNC Eigen::Matrix<Scalar, kNumParameters, kDoF> dxExpX(Tangent const& upsilon_omega)

Returns derivative of exp(x) wrt. x.

static SOPHUS_FUNC Eigen::Matrix<Scalar, kNumParameters, kDoF> dxExpXAt0()

Returns derivative of exp(x) wrt. x_i at x=0.

static SOPHUS_FUNC Transformation dxiExpmatXAt0(int i)

Returns derivative of expmat(x) wrt. x_i at x=0, with expmat(.) being the matrix exponential.

static SOPHUS_FUNC Eigen::Matrix<Scalar, 3, kDoF> dxExpXTimesPointAt0(Point const& point)

Returns derivative of exp(x) * p wrt. x_i at x=0.

static SOPHUS_FUNC Se3<Scalar> exp(Tangent const& vec_a)

Group exponential.

This functions takes in an element of tangent space (= twist a) and returns the corresponding element of the group SE(3).

The first three components of a represent the translational part upsilon in the tangent space of SE(3), while the last three components of a represents the rotation vector omega. To be more specific, this function computes expmat(hat(a)) with expmat(.) being the matrix exponential and hat(.) the hat-operator of SE(3), see below.

template <class TS = Scalar>
static SOPHUS_FUNC std::enable_if_t<std::is_floating_point<TS>::value, Se3> fitToSe3(Eigen::Matrix4<Scalar> const& t)

Returns closest Se3 given arbitrary 4x4 matrix.

static SOPHUS_FUNC Transformation generator(int i)

Returns the ith infinitesimal generators of SE(3).

The infinitesimal generators of SE(3) are:

      |  0  0  0  1 |
G_0 = |  0  0  0  0 |
      |  0  0  0  0 |
      |  0  0  0  0 |

      |  0  0  0  0 |
G_1 = |  0  0  0  1 |
      |  0  0  0  0 |
      |  0  0  0  0 |

      |  0  0  0  0 |
G_2 = |  0  0  0  0 |
      |  0  0  0  1 |
      |  0  0  0  0 |

      |  0  0  0  0 |
G_3 = |  0  0 -1  0 |
      |  0  1  0  0 |
      |  0  0  0  0 |

      |  0  0  1  0 |
G_4 = |  0  0  0  0 |
      | -1  0  0  0 |
      |  0  0  0  0 |

      |  0 -1  0  0 |
G_5 = |  1  0  0  0 |
      |  0  0  0  0 |
      |  0  0  0  0 |

Precondition: i must be in [0, 5].

static SOPHUS_FUNC Transformation hat(Tangent const& a)

hat-operator

It takes in the 6-vector representation (= twist) and returns the corresponding matrix representation of Lie algebra element.

Formally, the hat() -operator of SE(3) is defined as

hat(.): R^6 -> R^{4x4}, hat(a) = sum_i a_i * G_i (for i=0,…,5)

with G_i being the ith infinitesimal generator of SE(3).

The corresponding inverse is the vee() -operator, see below.

static SOPHUS_FUNC Tangent lieBracket(Tangent const& a, Tangent const& b)

Lie bracket.

It computes the Lie bracket of SE(3). To be more specific, it computes

[omega_1, omega_2]_se3 := vee([hat(omega_1), hat(omega_2)])

with [A,B] := AB-BA being the matrix commutator, hat(.) the hat() -operator and vee(.) the vee() -operator of SE(3).

static SOPHUS_FUNC Se3 rotX(Scalar const& x)

Construct x-axis rotation.

static SOPHUS_FUNC Se3 rotY(Scalar const& y)

Construct y-axis rotation.

static SOPHUS_FUNC Se3 rotZ(Scalar const& z)

Construct z-axis rotation.

template <class TUniformRandomBitGenerator>
static Se3 sampleUniform(TUniformRandomBitGenerator& generator)

Draw uniform sample from SE(3) manifold.

Translations are drawn component-wise from the range [-1, 1].

template <class TX, class TY, class TZ>
static SOPHUS_FUNC Se3 trans(
    TX const& x,
    TY const& y,
    TZ const& z
    )

Construct a translation only Se3 instance.

static SOPHUS_FUNC Se3 transX(Scalar const& x)

Construct x-axis translation.

static SOPHUS_FUNC Se3 transY(Scalar const& y)

Construct y-axis translation.

static SOPHUS_FUNC Se3 transZ(Scalar const& z)

Construct z-axis translation.

static SOPHUS_FUNC Tangent vee(Transformation const& omega)

vee-operator

It takes 4x4-matrix representation Omega and maps it to the corresponding 6-vector representation of Lie algebra.

This is the inverse of the hat() -operator, see above.

Precondition: Omega must have the following structure:

|  0 -f  e  a |
|  f  0 -d  b |
| -e  d  0  c
|  0  0  0  0 | .