template class sophus::So3Base

Overview

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

#include <so3.h>

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

    using Scalar = typename Eigen::internal::traits<TDerived>::Scalar ;
    using QuaternionType = typename Eigen::internal::traits<TDerived>::QuaternionType ;
    using QuaternionTemporaryType = Eigen::Quaternion<Scalar, kOptions> ;
    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 So3Product = So3<ReturnScalar<TOtherDerived>> ;
    using PointProduct = Eigen::Vector3<ReturnScalar<TPointDerived>> ;
    using HomogeneousPointProduct = Eigen::Vector4<ReturnScalar<THPointDerived>> ;

    // structs

    struct TangentAndTheta;

    // fields

    static constexpr int kOptions = Eigen::internal::traits<TDerived>::kOptions;
    static constexpr int kDoF = 3;
    static constexpr int kNumParameters = 4;
    static constexpr int kMatrixDim = 3;
    static constexpr int kPointDim = 3;

    // methods

    SOPHUS_FUNC Adjoint adj() const;

    template <class TS = Scalar>
    SOPHUS_FUNC std::enable_if_t<std::is_floating_point<TS>::value, TS> angleX() const;

    template <class TS = Scalar>
    SOPHUS_FUNC std::enable_if_t<std::is_floating_point<TS>::value, TS> angleY() const;

    template <class TS = Scalar>
    SOPHUS_FUNC std::enable_if_t<std::is_floating_point<TS>::value, TS> angleZ() const;

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

    SOPHUS_FUNC Scalar* data();
    SOPHUS_FUNC Scalar const* data() const;
    SOPHUS_FUNC Eigen::Matrix<Scalar, kNumParameters, kDoF> dxThisMulExpXAt0() const;
    SOPHUS_FUNC Eigen::Matrix<Scalar, kDoF, kNumParameters> dxLogThisInvTimesXAtThis() const;
    SOPHUS_FUNC Eigen::Vector<Scalar, kNumParameters> params() const;
    SOPHUS_FUNC So3<Scalar> inverse() const;
    SOPHUS_FUNC Tangent log() const;
    SOPHUS_FUNC TangentAndTheta logAndTheta() const;
    SOPHUS_FUNC void normalize();
    SOPHUS_FUNC Transformation matrix() const;

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

    template <class TOtherDerived>
    SOPHUS_FUNC So3Product<TOtherDerived> operator*(So3Base<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 So3Base<TDerived>& operator*=(So3Base<TOtherDerived> const& other);

    SOPHUS_FUNC void setQuaternion(Eigen::Quaternion<Scalar> const& quaternion);
    SOPHUS_FUNC QuaternionType const& unitQuaternion() const;

    template <
        typename TQuaternionProductType,
        typename TQuaternionTypeA,
        typename TQuaternionTypeB
        >
    static TQuaternionProductType quaternionProduct(
        TQuaternionTypeA const& a,
        TQuaternionTypeB const& b
        );
};

// direct descendants

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

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

Detailed Documentation

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

SO(3) is the group of rotations in 3d. As a matrix group, it is the set of matrices which are orthogonal such that R * R' = I (with R' being the transpose of R) and have a positive determinant. In particular, the determinant is 1. Internally, the group is represented as a unit quaternion. Unit quaternion can be seen as members of the special unitary group SU(2). SU(2) is a double cover of SO(3). Hence, for every rotation matrix R, there exist two unit quaternions: (r, v) and (-r, -v), with r the real part and v being the imaginary 3-vector part of the quaternion.

SO(3) is a compact, but non-commutative group. First it is compact since the set of rotation matrices is a closed and bounded set. Second it is non-commutative since the equation R_1 * R_2 = R_2 * R_1 does not hold in general. For example rotating an object by some degrees about its x -axis and then by some degrees about its y axis, does not lead to the same orientation when rotation first about y and then about x.

Class invariant: The 2-norm of unit_quaternion must be close to 1. Technically speaking, it must hold that:

|unit_quaternion().squaredNorm() - 1| <= Constants::epsilon().

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 So3 operations.

Fields

static constexpr int kDoF = 3

Degrees of freedom of group, number of dimensions in tangent space.

static constexpr int kNumParameters = 4

Number of internal parameters used (quaternion is a 4-tuple).

static constexpr int kMatrixDim = 3

Group transformations are 3x3 matrices.

static constexpr int kPointDim = 3

Points are 3-dimensional/ Include only the selective set of Eigen headers that we need.

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. For SO(3), it simply returns the rotation matrix corresponding to A.

template <class TS = Scalar>
SOPHUS_FUNC std::enable_if_t<std::is_floating_point<TS>::value, TS> angleX() const

Extract rotation angle about canonical X-axis.

template <class TS = Scalar>
SOPHUS_FUNC std::enable_if_t<std::is_floating_point<TS>::value, TS> angleY() const

Extract rotation angle about canonical Y-axis.

template <class TS = Scalar>
SOPHUS_FUNC std::enable_if_t<std::is_floating_point<TS>::value, TS> angleZ() const

Extract rotation angle about canonical Z-axis.

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

Returns copy of instance casted to NewScalarType.

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.

Note: The first three Scalars represent the imaginary parts, while the forth Scalar represent the real part.

SOPHUS_FUNC Scalar const* data() const

Const version of data() above.

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

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

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

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

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

Returns internal parameters of SO(3).

It returns (q.imag[0], q.imag[1], q.imag[2], q.real), with q being the unit quaternion.

SOPHUS_FUNC So3<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 (rotation matrices) to elements of the tangent space (rotation-vector).

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

SOPHUS_FUNC TangentAndTheta logAndTheta() const

As above, but also returns theta = |omega|.

SOPHUS_FUNC void normalize()

It re-normalizes unit_quaternion to unit length.

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

SOPHUS_FUNC Transformation matrix() const

Returns 3x3 matrix representation of the instance.

For SO(3), the matrix representation is an orthogonal matrix R with det(R)=1, thus the so-called “rotation matrix”.

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

Assignment-like operator from OtherDerived.

template <class TOtherDerived>
SOPHUS_FUNC So3Product<TOtherDerived> operator*(So3Base<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 a 3 dimensional point p by the So3 element bar_R_foo (= rotation matrix): p_bar = bar_R_foo * p_foo.

Since So3 is internally represented by a unit quaternion q, it is implemented as p_bar = q * p_foo * q^{*} with q^{*} being the quaternion conjugate of q.

Geometrically, p is rotated by angle |omega| around the axis omega/|omega| with omega := vee(log(bar_R_foo)).

For vee -operator, see below.

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 a parametrized line l(t) = o + t * d by the So3 element:

Both direction d and origin o are rotated as a 3 dimensional point

SOPHUS_FUNC Hyperplane operator*(Hyperplane const& p) const

Group action on planes.

This function rotates a plane n.x + d = 0 by the So3 element:

Normal vector n is rotated Offset d is left unchanged

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

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

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

Takes in quaternion, and normalizes it.

Precondition: The quaternion must not be close to zero.

SOPHUS_FUNC QuaternionType const& unitQuaternion() const

Accessor of unit quaternion.