template class sophus::Se2

Overview

Se2 using default storage; derived from Se2Base. More…

#include <se2.h>

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

    using Base = Se2Base<Se2<TScalar, kOptions>> ;
    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 SO2Member = So2<Scalar, kOptions> ;
    using TranslationMember = Eigen::Matrix<Scalar, 2, 1, kOptions> ;

    // fields

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

    // construction

    Se2();
    Se2(Se2 const& other);

    template <class TOtherDerived>
    Se2(Se2Base<TOtherDerived> const& other);

    template <class TOtherDerived, class TD>
    Se2(
        So2Base<TOtherDerived> const& so2,
        Eigen::MatrixBase<TD> const& translation
        );

    Se2(typename So2<Scalar>::Transformation const& rotation_matrix, Point const& translation);
    Se2(Scalar const& theta, Point const& translation);
    Se2(Eigen::Vector2<Scalar> const& complex, Point const& translation);
    Se2(Transformation const& mat3x3);

    // methods

    SOPHUS_FUNC Se2& operator=(Se2 const& other);
    SOPHUS_FUNC Scalar* data();
    SOPHUS_FUNC Scalar const* data() const;
    SOPHUS_FUNC SO2Member& so2();
    SOPHUS_FUNC SO2Member const& so2() const;
    SOPHUS_FUNC TranslationMember& translation();
    SOPHUS_FUNC TranslationMember const& translation() const;

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

    static SOPHUS_FUNC Eigen::Matrix<Scalar, kNumParameters, kDoF> dxExpX(Tangent const& upsilon_theta);
    static SOPHUS_FUNC Eigen::Matrix<Scalar, kNumParameters, kDoF> dxExpXAt0();
    static SOPHUS_FUNC Eigen::Matrix<Scalar, 2, kDoF> dxExpXTimesPointAt0(Point const& point);
    static SOPHUS_FUNC Transformation dxiExpmatXAt0(int i);
    static SOPHUS_FUNC Se2<Scalar> exp(Tangent const& vec_a);

    template <class TS = Scalar>
    static SOPHUS_FUNC std::enable_if_t<std::is_floating_point<TS>::value, Se2> fitToSe2(Eigen::Matrix3<Scalar> const& mat3x3);

    static SOPHUS_FUNC Transformation generator(int i);
    static SOPHUS_FUNC Transformation hat(Tangent const& vec_a);
    static SOPHUS_FUNC Tangent lieBracket(Tangent const& vec_a, Tangent const& vec_b);
    static SOPHUS_FUNC Se2 rot(Scalar const& x);

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

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

    static SOPHUS_FUNC Se2 trans(Eigen::Vector2<Scalar> const& xy);
    static SOPHUS_FUNC Se2 transX(Scalar const& x);
    static SOPHUS_FUNC Se2 transY(Scalar const& y);
    static SOPHUS_FUNC Tangent vee(Transformation const& omega);

protected:
    // fields

    SO2Member so2_;
    TranslationMember translation_;
};

Detailed Documentation

Se2 using default storage; derived from Se2Base.

Construction

Se2()

Default constructor initializes rigid body motion to the identity.

Se2(Se2 const& other)

Copy constructor.

template <class TOtherDerived>
Se2(Se2Base<TOtherDerived> const& other)

Copy-like constructor from OtherDerived.

template <class TOtherDerived, class TD>
Se2(
    So2Base<TOtherDerived> const& so2,
    Eigen::MatrixBase<TD> const& translation
    )

Constructor from So3 and translation vector.

Se2(typename So2<Scalar>::Transformation const& rotation_matrix, Point const& translation)

Constructor from rotation matrix and translation vector.

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

Se2(Scalar const& theta, Point const& translation)

Constructor from rotation angle and translation vector.

Se2(Eigen::Vector2<Scalar> const& complex, Point const& translation)

Constructor from complex number and translation vector.

Precondition: complex must not be close to zero.

Se2(Transformation const& mat3x3)

Constructor from 3x3 matrix.

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

Methods

SOPHUS_FUNC Se2& operator=(Se2 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(2) is represented by a complex number (two parameters). When using direct write access, the user needs to take care of that the complex number stays normalized.

SOPHUS_FUNC Scalar const* data() const

Const version of data() above.

SOPHUS_FUNC SO2Member& so2()

Accessor of So3.

SOPHUS_FUNC SO2Member const& so2() const

Mutator 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 Se2Base<TDerived>& operator=(Se2Base<TOtherDerived> const& other)

Assignment-like operator from OtherDerived.

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

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 Eigen::Matrix<Scalar, 2, kDoF> dxExpXTimesPointAt0(Point const& point)

Returns derivative of exp(x) * p 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 Se2<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(2).

The first two components of a represent the translational part upsilon in the tangent space of SE(2), 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(2), see below.

template <class TS = Scalar>
static SOPHUS_FUNC std::enable_if_t<std::is_floating_point<TS>::value, Se2> fitToSe2(Eigen::Matrix3<Scalar> const& mat3x3)

Returns closest Se3 given arbitrary 4x4 matrix.

static SOPHUS_FUNC Transformation generator(int i)

Returns the ith infinitesimal generators of SE(2).

The infinitesimal generators of SE(2) are:

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

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

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

Precondition: i must be in 0, 1 or 2.

static SOPHUS_FUNC Transformation hat(Tangent const& vec_a)

hat-operator

It takes in the 3-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^3 -> R^{3x33}, hat(a) = sum_i a_i * G_i (for i=0,1,2)

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

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

static SOPHUS_FUNC Tangent lieBracket(Tangent const& vec_a, Tangent const& vec_b)

Lie bracket.

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

[omega_1, omega_2]_se2 := 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(2).

static SOPHUS_FUNC Se2 rot(Scalar const& x)

Construct pure rotation.

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

Draw uniform sample from SE(2) manifold.

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

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

Construct a translation only SE(2) instance.

static SOPHUS_FUNC Se2 transX(Scalar const& x)

Construct x-axis translation.

static SOPHUS_FUNC Se2 transY(Scalar const& y)

Construct y-axis translation.

static SOPHUS_FUNC Tangent vee(Transformation const& omega)

vee-operator

It takes the 3x3-matrix representation Omega and maps it to the corresponding 3-vector representation of Lie algebra.

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

Precondition: Omega must have the following structure:

|  0 -d  a |
|  d  0  b |
|  0  0  0 |