Template Class SE3

Defined in File se3.hpp

Inheritance Relationships

Base Type

public Sophus::SE3Base< SE3< Scalar_, Options > > (Template Class SE3Base)

Class Documentation

template<class Scalar_, int Options> class SE3 : public Sophus::SE3Base<SE3<Scalar_, Options>>

SE3 using default storage; derived from SE3Base.

Public Types

using Scalar = Scalar_

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, Options>

using TranslationMember = Vector3<Scalar, Options>

Public Functions

SOPHUS_FUNC SE3 & operator= (SE3 const &other)=default: 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).

EIGEN_MAKE_ALIGNED_OPERATOR_NEW SOPHUS_FUNC SE3 (): Default constructor initializes rigid body motion to the identity.

SOPHUS_FUNC SE3(SE3 const &other) = default: Copy constructor

template<class OtherDerived> inline SOPHUS_FUNC SE3(SE3Base<OtherDerived> const &other): Copy-like constructor from OtherDerived.

template<class OtherDerived, class D> inline SOPHUS_FUNC SE3(SO3Base<OtherDerived> const &so3, Eigen::MatrixBase<D> const &translation): Constructor from SO3 and translation vector

inline SOPHUS_FUNC SE3(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.

inline SOPHUS_FUNC SE3(Eigen::Quaternion<Scalar> const &quaternion, Point const &translation)

Constructor from quaternion and translation vector.

Precondition: quaternion must not be close to zero.

inline explicit SOPHUS_FUNC SE3(Matrix4<Scalar> const &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).

inline 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.

inline SOPHUS_FUNC Scalar const * data () const: Const version of data() above.

inline SOPHUS_FUNC SO3Member & so3 (): Mutator of SO3

inline SOPHUS_FUNC SO3Member const & so3 () const: Accessor of SO3

inline SOPHUS_FUNC TranslationMember & translation (): Mutator of translation vector

inline SOPHUS_FUNC TranslationMember const & translation () const: Accessor of translation vector

Public Static Functions

static inline SOPHUS_FUNC Matrix3< Scalar > jacobianUpperRightBlock (Vector3< Scalar > const &upsilon, Vector3< Scalar > const &omega)

static inline SOPHUS_FUNC Sophus::Matrix< Scalar, DoF, DoF > leftJacobian (Tangent const &upsilon_omega)

static inline SOPHUS_FUNC Sophus::Matrix< Scalar, DoF, DoF > leftJacobianInverse (Tangent const &upsilon_omega)

static inline SOPHUS_FUNC Sophus::Matrix< Scalar, num_parameters, DoF > Dx_exp_x (Tangent const &upsilon_omega): Returns derivative of exp(x) wrt. x.

static inline SOPHUS_FUNC Sophus::Matrix< Scalar, num_parameters, DoF > Dx_exp_x_at_0 (): Returns derivative of exp(x) wrt. x_i at x=0.

static inline SOPHUS_FUNC Transformation Dxi_exp_x_matrix_at_0 (int i): Returns derivative of exp(x).matrix() wrt. x_i at x=0.

static inline SOPHUS_FUNC Sophus::Matrix< Scalar, 3, DoF > Dx_exp_x_times_point_at_0 (Point const &point): Returns derivative of exp(x) * p wrt. x_i at x=0.

static inline SOPHUS_FUNC SE3< Scalar > exp (Tangent const &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 S = Scalar> static inline SOPHUS_FUNC enable_if_t< std::is_floating_point< S >::value, SE3 > fitToSE3 (Matrix4< Scalar > const &T): Returns closest SE3 given arbitrary 4x4 matrix.

static inline 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 inline 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 inline 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 inline SOPHUS_FUNC SE3 rotX (Scalar const &x): Construct x-axis rotation.

static inline SOPHUS_FUNC SE3 rotY (Scalar const &y): Construct y-axis rotation.

static inline SOPHUS_FUNC SE3 rotZ (Scalar const &z): Construct z-axis rotation.

template<class UniformRandomBitGenerator> static inline SE3 sampleUniform(UniformRandomBitGenerator &generator)

Draw uniform sample from SE(3) manifold.

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

template<class T0, class T1, class T2> static inline SOPHUS_FUNC SE3 trans (T0 const &x, T1 const &y, T2 const &z): Construct a translation only SE3 instance.

static inline SOPHUS_FUNC SE3 trans (Vector3< Scalar > const &xyz)

static inline SOPHUS_FUNC SE3 transX (Scalar const &x): Construct x-axis translation.

static inline SOPHUS_FUNC SE3 transY (Scalar const &y): Construct y-axis translation.

static inline SOPHUS_FUNC SE3 transZ (Scalar const &z): Construct z-axis translation.

static inline 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 | .

Public Static Attributes

static constexpr int DoF = Base::DoF

static constexpr int num_parameters = Base::num_parameters

Protected Attributes

SO3Member so3_

TranslationMember translation_