template class sophus::Sim3

Overview

Sim3 using default storage; derived from Sim3Base. More…

#include <sim3.h>

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

    using Base = Sim3Base<Sim3<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 RxSo3Member = RxSo3<Scalar, kOptions> ;
    using TranslationMember = Eigen::Matrix<Scalar, 3, 1, kOptions> ;

    // fields

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

    // construction

    Sim3();
    Sim3(Sim3 const& other);

    template <class TOtherDerived>
    Sim3(Sim3Base<TOtherDerived> const& other);

    template <class TOtherDerived, class TD>
    Sim3(
        RxSo3Base<TOtherDerived> const& rxso3,
        Eigen::MatrixBase<TD> const& translation
        );

    template <class TD1T, class TD2T>
    Sim3(
        Eigen::QuaternionBase<TD1T> const& quaternion,
        Eigen::MatrixBase<TD2T> const& translation
        );

    template <class TD1T, class TD2T>
    Sim3(
        Scalar const& scale,
        Eigen::QuaternionBase<TD1T> const& unit_quaternion,
        Eigen::MatrixBase<TD2T> const& translation
        );

    Sim3(Eigen::Matrix<Scalar, 4, 4> const& t);

    // methods

    SOPHUS_FUNC Sim3& operator=(Sim3 const& other);
    SOPHUS_FUNC Scalar* data();
    SOPHUS_FUNC Scalar const* data() const;
    SOPHUS_FUNC RxSo3Member& rxso3();
    SOPHUS_FUNC RxSo3Member const& rxso3() const;
    SOPHUS_FUNC TranslationMember& translation();
    SOPHUS_FUNC TranslationMember const& translation() const;

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

    static SOPHUS_FUNC Eigen::Matrix<Scalar, kNumParameters, kDoF> dxExpXAt0();
    static SOPHUS_FUNC Eigen::Matrix<Scalar, kNumParameters, kDoF> dxExpX(Tangent const& vec_a);
    static SOPHUS_FUNC Eigen::Matrix<Scalar, 3, kDoF> dxExpXTimesPointAt0(Point const& point);
    static SOPHUS_FUNC Transformation dxiExpmatXAt0(int i);
    static SOPHUS_FUNC Sim3<Scalar> exp(Tangent const& vec_a);
    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);

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

    static SOPHUS_FUNC Tangent vee(Transformation const& mat_omega);

protected:
    // fields

    RxSo3Member rxso3_;
    TranslationMember translation_;
};

Detailed Documentation

Sim3 using default storage; derived from Sim3Base.

Construction

Sim3()

Default constructor initializes similarity transform to the identity.

Sim3(Sim3 const& other)

Copy constructor.

template <class TOtherDerived>
Sim3(Sim3Base<TOtherDerived> const& other)

Copy-like constructor from OtherDerived.

template <class TOtherDerived, class TD>
Sim3(
    RxSo3Base<TOtherDerived> const& rxso3,
    Eigen::MatrixBase<TD> const& translation
    )

Constructor from RxSo3 and translation vector.

template <class TD1T, class TD2T>
Sim3(
    Eigen::QuaternionBase<TD1T> const& quaternion,
    Eigen::MatrixBase<TD2T> const& translation
    )

Constructor from quaternion and translation vector.

Precondition: quaternion must not be close to zero.

template <class TD1T, class TD2T>
Sim3(
    Scalar const& scale,
    Eigen::QuaternionBase<TD1T> const& unit_quaternion,
    Eigen::MatrixBase<TD2T> const& translation
    )

Constructor from scale factor, unit quaternion, and translation vector.

Precondition: quaternion must not be close to zero.

Sim3(Eigen::Matrix<Scalar, 4, 4> const& t)

Constructor from 4x4 matrix.

Precondition: Top-left 3x3 matrix needs to be “scaled-orthogonal” with positive determinant. The last row must be (0, 0, 0, 1).

Methods

SOPHUS_FUNC Sim3& operator=(Sim3 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. Sim(3) is represented by an Eigen::Quaternion (four parameters) and a 3-vector. When using direct write access, the user needs to take care of that the quaternion is not set close to zero.

SOPHUS_FUNC Scalar const* data() const

Const version of data() above.

SOPHUS_FUNC RxSo3Member& rxso3()

Accessor of RxSo3.

SOPHUS_FUNC RxSo3Member const& rxso3() const

Mutator of RxSo3.

SOPHUS_FUNC TranslationMember& translation()

Mutator of translation vector.

SOPHUS_FUNC TranslationMember const& translation() const

Accessor of translation vector.

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

Assignment-like operator from OtherDerived.

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, kNumParameters, kDoF> dxExpX(Tangent const& vec_a)

Returns derivative of exp(x) wrt. x.

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 Transformation dxiExpmatXAt0(int i)

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

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

Group exponential.

This functions takes in an element of tangent space and returns the corresponding element of the group Sim(3).

The first three components of a represent the translational part upsilon in the tangent space of Sim(3), the following three components of a represents the rotation vector omega and the final component represents the logarithm of the scaling factor sigma. To be more specific, this function computes expmat(hat(a)) with expmat(.) being the matrix exponential and hat(.) the hat-operator of Sim(3), see below.

static SOPHUS_FUNC Transformation generator(int i)

Returns the ith infinitesimal generators of Sim(3).

The infinitesimal generators of Sim(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 |

      |  1  0  0  0 |
G_6 = |  0  1  0  0 |
      |  0  0  1  0 |
      |  0  0  0  0 |

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

static SOPHUS_FUNC Transformation hat(Tangent const& vec_a)

hat-operator

It takes in the 7-vector representation and returns the corresponding matrix representation of Lie algebra element.

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

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

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

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 Sim(3). To be more specific, it computes

[omega_1, omega_2]_sim3 := 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 Sim(3).

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

Draw uniform sample from Sim(3) manifold.

Translations are drawn component-wise from the range [-1, 1]. The scale factor is drawn uniformly in log2-space from [-1, 1], hence the scale is in [0.5, 2].

static SOPHUS_FUNC Tangent vee(Transformation const& mat_omega)

vee-operator

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

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

Precondition: Omega must have the following structure:

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