template class sophus::Se3Base

Overview

Se3 base type - implements Se3 class but is storage agnostic. More…

#include <se3.h>

template <class TDerived>
class Se3Base {
public:
    // typedefs

    using Scalar = typename Eigen::internal::traits<TDerived>::Scalar ;
    using TranslationType = typename Eigen::internal::traits<TDerived>::TranslationType ;
    using So3Type = typename Eigen::internal::traits<TDerived>::So3Type ;
    using QuaternionType = typename So3Type::QuaternionType ;
    using Transformation = Eigen::Matrix<Scalar, kMatrixDim, kMatrixDim> ;
    using Point = Eigen::Vector3<Scalar> ;
    using HomogeneousPoint = Eigen::Vector4<Scalar> ;
    using Line = Eigen::ParametrizedLine<Scalar, 3> ;
    using Hyperplane = Eigen::Hyperplane<Scalar, 3> ;
    using Tangent = Eigen::Vector<Scalar, kDoF> ;
    using Adjoint = Eigen::Matrix<Scalar, kDoF, kDoF> ;
    using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<Scalar, typename TOtherDerived::Scalar>::ReturnType ;
    using Se3Product = Se3<ReturnScalar<TOtherDerived>> ;
    using PointProduct = Eigen::Vector3<ReturnScalar<TPointDerived>> ;
    using HomogeneousPointProduct = Eigen::Vector4<ReturnScalar<THPointDerived>> ;

    // fields

    static constexpr int kDoF = 6;
    static constexpr int kNumParameters = 7;
    static constexpr int kMatrixDim = 4;
    static constexpr int kPointDim = 3;

    // methods

    SOPHUS_FUNC Adjoint adj() const;
    Scalar angleX() const;
    Scalar angleY() const;
    Scalar angleZ() const;

    template <class TNewScalarType>
    SOPHUS_FUNC Se3<TNewScalarType> cast() const;

    SOPHUS_FUNC Eigen::Matrix<Scalar, kNumParameters, kDoF> dxThisMulExpXAt0() const;
    SOPHUS_FUNC Eigen::Matrix<Scalar, kDoF, kNumParameters> dxLogThisInvTimesXAtThis() const;
    SOPHUS_FUNC Se3<Scalar> inverse() const;
    SOPHUS_FUNC Tangent log() const;
    SOPHUS_FUNC void normalize();
    SOPHUS_FUNC Transformation matrix() const;
    SOPHUS_FUNC Eigen::Matrix<Scalar, 3, 4> matrix3x4() const;

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

    template <class TOtherDerived>
    SOPHUS_FUNC Se3Product<TOtherDerived> operator*(Se3Base<TOtherDerived> const& other) const;

    template <
        typename TPointDerived,
        typename = typename std::enable_if<IsFixedSizeVector<TPointDerived, 3>::value>::type
        >
    SOPHUS_FUNC PointProduct<TPointDerived> operator*(Eigen::MatrixBase<TPointDerived> const& p) const;

    template <
        typename THPointDerived,
        typename = typename std::enable_if<IsFixedSizeVector<THPointDerived, 4>::value>::type
        >
    SOPHUS_FUNC HomogeneousPointProduct<THPointDerived> operator*(Eigen::MatrixBase<THPointDerived> const& p) const;

    SOPHUS_FUNC Line operator*(Line const& l) const;
    SOPHUS_FUNC Hyperplane operator*(Hyperplane const& p) const;

    template <
        typename TOtherDerived,
        typename = typename std::enable_if<std::is_same<Scalar, ReturnScalar<TOtherDerived>>::value>::type
        >
    SOPHUS_FUNC Se3Base<TDerived>& operator*=(Se3Base<TOtherDerived> const& other);

    SOPHUS_FUNC Eigen::Matrix3<Scalar> rotationMatrix() const;
    SOPHUS_FUNC So3Type& so3();
    SOPHUS_FUNC So3Type const& so3() const;
    SOPHUS_FUNC void setQuaternion(Eigen::Quaternion<Scalar> const& quat);
    SOPHUS_FUNC void setRotationMatrix(Eigen::Matrix3<Scalar> const& mat_r);
    SOPHUS_FUNC Eigen::Vector<Scalar, kNumParameters> params() const;
    SOPHUS_FUNC TranslationType& translation();
    SOPHUS_FUNC TranslationType const& translation() const;
    SOPHUS_FUNC QuaternionType const& unitQuaternion() const;
};

// direct descendants

template <class TScalar, int kOptions>
class Map<sophus::Se3<TScalar> const, kOptions>;

template <class TScalar, int kOptions>
class Map<sophus::Se3<TScalar>, kOptions>;

Detailed Documentation

Se3 base type - implements Se3 class but is storage agnostic.

SE(3) is the group of rotations and translation in 3d. It is the semi-direct product of SO(3) and the 3d Euclidean vector space. The class is represented using a composition of So3 for rotation and a one 3-vector for translation.

SE(3) is neither compact, nor a commutative group.

  • 4x4 Eigen::Matrix representation:

| R t |
| o 1 |

where R is a 3x3 rotation matrix, t a translation 3-vector and o a 3-column vector of zeros.

  • Tangent 6-vector: [upsilon, omega],

where upsilon is the translational velocity 3-vector and omega the rotational velocity 3-vector.

  • Internal 7-representation: [t0, t1, t2, qi0, qi1, qi2, qr],

with t0, t1, t2 are the translational components, and qi0, qi1, q2 the imaginary vector part and qr1 the real part of a unit-length quaternion.

See So3 for more details of the rotation representation in 3d.

Typedefs

using ReturnScalar = typename Eigen::ScalarBinaryOpTraits<Scalar, typename TOtherDerived::Scalar>::ReturnType

For binary operations the return type is determined with the ScalarBinaryOpTraits feature of Eigen. This allows mixing concrete and Map types, as well as other compatible scalar types such as Ceres::Jet and double scalars with Se3 operations.

Fields

static constexpr int kDoF = 6

Degrees of freedom of manifold, number of dimensions in tangent space (three for translation, three for rotation).

static constexpr int kNumParameters = 7

Number of internal parameters used (4-tuple for quaternion, three for translation).

static constexpr int kMatrixDim = 4

Group transformations are 4x4 matrices.

static constexpr int kPointDim = 3

Points are 3-dimensional.

Methods

SOPHUS_FUNC Adjoint adj() const

Adjoint transformation.

This function return the adjoint transformation Ad of the group element A such that for all x it holds that hat(Ad_A * x) = A * hat(x) A^{-1}. See hat-operator below.

Scalar angleX() const

Extract rotation angle about canonical X-axis.

Scalar angleY() const

Extract rotation angle about canonical Y-axis.

Scalar angleZ() const

Extract rotation angle about canonical Z-axis.

template <class TNewScalarType>
SOPHUS_FUNC Se3<TNewScalarType> cast() const

Returns copy of instance casted to NewScalarType.

SOPHUS_FUNC Eigen::Matrix<Scalar, kNumParameters, kDoF> dxThisMulExpXAt0() const

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

SOPHUS_FUNC Eigen::Matrix<Scalar, kDoF, kNumParameters> dxLogThisInvTimesXAtThis() const

Returns derivative of log(this^{-1} * x) by x at x=this.

SOPHUS_FUNC Se3<Scalar> inverse() const

Returns group inverse.

SOPHUS_FUNC Tangent log() const

Logarithmic map.

Computes the logarithm, the inverse of the group exponential which maps element of the group (rigid body transformations) to elements of the tangent space (twist).

To be specific, this function computes vee(logmat(.)) with logmat(.) being the matrix logarithm and vee(.) the vee-operator of SE(3).

SOPHUS_FUNC void normalize()

It re-normalizes the So3 element.

Note: Because of the class invariant of So3, there is typically no need to call this function directly.

SOPHUS_FUNC Transformation matrix() const

Returns 4x4 matrix representation of the instance.

It has the following form:

| R t |
| o 1 |

where R is a 3x3 rotation matrix, t a translation 3-vector and o a 3-column vector of zeros.

SOPHUS_FUNC Eigen::Matrix<Scalar, 3, 4> matrix3x4() const

Returns the significant first three rows of the matrix above.

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

Assignment-like operator from OtherDerived.

template <class TOtherDerived>
SOPHUS_FUNC Se3Product<TOtherDerived> operator*(Se3Base<TOtherDerived> const& other) const

Group multiplication, which is rotation concatenation.

template <
    typename TPointDerived,
    typename = typename std::enable_if<IsFixedSizeVector<TPointDerived, 3>::value>::type
    >
SOPHUS_FUNC PointProduct<TPointDerived> operator*(Eigen::MatrixBase<TPointDerived> const& p) const

Group action on 3-points.

This function rotates and translates a three dimensional point p by the SE(3) element bar_transform_foo = (bar_R_foo, t_bar) (= rigid body transformation):

p_bar = bar_R_foo * p_foo + t_bar.

template <
    typename THPointDerived,
    typename = typename std::enable_if<IsFixedSizeVector<THPointDerived, 4>::value>::type
    >
SOPHUS_FUNC HomogeneousPointProduct<THPointDerived> operator*(Eigen::MatrixBase<THPointDerived> const& p) const

Group action on homogeneous 3-points. See above for more details.

SOPHUS_FUNC Line operator*(Line const& l) const

Group action on lines.

This function rotates and translates a parametrized line l(t) = o + t * d by the SE(3) element:

Origin is transformed using SE(3) action Direction is transformed using rotation part

SOPHUS_FUNC Hyperplane operator*(Hyperplane const& p) const

Group action on planes.

This function rotates and translates a plane n.x + d = 0 by the SE(3) element:

Normal vector n is rotated Offset d is adjusted for translation

template <
    typename TOtherDerived,
    typename = typename std::enable_if<std::is_same<Scalar, ReturnScalar<TOtherDerived>>::value>::type
    >
SOPHUS_FUNC Se3Base<TDerived>& operator*=(Se3Base<TOtherDerived> const& other)

In-place group multiplication. This method is only valid if the return type of the multiplication is compatible with this Se3 ‘s Scalar type.

SOPHUS_FUNC Eigen::Matrix3<Scalar> rotationMatrix() const

Returns rotation matrix.

SOPHUS_FUNC So3Type& so3()

Mutator of So3 group.

SOPHUS_FUNC So3Type const& so3() const

Accessor of So3 group.

SOPHUS_FUNC void setQuaternion(Eigen::Quaternion<Scalar> const& quat)

Takes in quaternion, and normalizes it.

Precondition: The quaternion must not be close to zero.

SOPHUS_FUNC void setRotationMatrix(Eigen::Matrix3<Scalar> const& mat_r)

Sets so3 using rotation_matrix.

Precondition: R must be orthogonal and det(R)=1.

SOPHUS_FUNC Eigen::Vector<Scalar, kNumParameters> params() const

Returns internal parameters of SE(3).

It returns (q.imag[0], q.imag[1], q.imag[2], q.real, t[0], t[1], t[2]), with q being the unit quaternion, t the translation 3-vector.

SOPHUS_FUNC TranslationType& translation()

Mutator of translation vector.

SOPHUS_FUNC TranslationType const& translation() const

Accessor of translation vector.

SOPHUS_FUNC QuaternionType const& unitQuaternion() const

Accessor of unit quaternion.