template class sophus::So3

Overview

So3 using default storage; derived from So3Base. More…

#include <so3.h>

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

    using Base = So3Base<So3<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 QuaternionMember = Eigen::Quaternion<Scalar, kOptions> ;

    // structs

    struct So3AndTheta;

    // fields

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

    // construction

    So3();
    So3(So3 const& other);

    template <class TOtherDerived>
    So3(So3Base<TOtherDerived> const& other);

    So3(Transformation const& r);

    template <class TD>
    So3(Eigen::QuaternionBase<TD> const& quat);

    // methods

    SOPHUS_FUNC So3& operator=(So3 const& other);
    SOPHUS_FUNC QuaternionMember const& unitQuaternion() const;
    SOPHUS_FUNC Eigen::Matrix<Scalar, kDoF, kDoF> dxThisMulX() const;

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

    static SOPHUS_FUNC Eigen::Matrix<Scalar, kDoF, kDoF> leftJacobian(
        Tangent const& omega,
        std::optional<Scalar> const& maybe_theta = std::nullopt
        );

    static SOPHUS_FUNC Eigen::Matrix<Scalar, kDoF, kDoF> leftJacobianInverse(
        Tangent const& omega,
        std::optional<Scalar> const& maybe_theta = std::nullopt
        );

    static SOPHUS_FUNC Eigen::Matrix<Scalar, kNumParameters, kDoF> dxExpX(Tangent const& omega);
    static SOPHUS_FUNC Eigen::Matrix<Scalar, kNumParameters, kDoF> dxExpXAt0();
    static SOPHUS_FUNC Eigen::Matrix<Scalar, 3, kDoF> dxExpXTimesPointAt0(Point const& point);
    static SOPHUS_FUNC Transformation dxiExpmatXAt0(int i);
    static SOPHUS_FUNC So3<Scalar> exp(Tangent const& omega);
    static SOPHUS_FUNC So3AndTheta expAndTheta(Tangent const& omega);

    template <class TS = Scalar>
    static SOPHUS_FUNC std::enable_if_t<std::is_floating_point<TS>::value, So3> fitToSo3(Transformation const& r);

    static SOPHUS_FUNC Transformation generator(int i);
    static SOPHUS_FUNC Transformation hat(Tangent const& omega);
    static SOPHUS_FUNC Tangent lieBracket(Tangent const& omega1, Tangent const& omega2);
    static SOPHUS_FUNC So3 rotX(Scalar const& x);
    static SOPHUS_FUNC So3 rotY(Scalar const& y);
    static SOPHUS_FUNC So3 rotZ(Scalar const& z);

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

    static SOPHUS_FUNC Tangent vee(Transformation const& omega);

protected:
    // fields

    QuaternionMember unit_quaternion_;

    // methods

    SOPHUS_FUNC QuaternionMember& mutUnitQuaternion();
};

Detailed Documentation

So3 using default storage; derived from So3Base.

Construction

So3()

Default constructor initializes unit quaternion to identity rotation.

So3(So3 const& other)

Copy constructor.

template <class TOtherDerived>
So3(So3Base<TOtherDerived> const& other)

Copy-like constructor from OtherDerived.

So3(Transformation const& r)

Constructor from rotation matrix.

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

template <class TD>
So3(Eigen::QuaternionBase<TD> const& quat)

Constructor from quaternion.

Precondition: quaternion must not be close to zero.

Methods

SOPHUS_FUNC So3& operator=(So3 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 QuaternionMember const& unitQuaternion() const

Accessor of unit quaternion.

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

Returns derivative of this * x wrt x at x=0.

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

Assignment-like operator from OtherDerived.

static SOPHUS_FUNC Eigen::Matrix<Scalar, kDoF, kDoF> leftJacobian(
    Tangent const& omega,
    std::optional<Scalar> const& maybe_theta = std::nullopt
    )

Returns the left Jacobian on lie group. See 1st entry in rightmost column in: http://asrl.utias.utoronto.ca/~tdb/bib/barfoot_ser17_identities.pdf.

A precomputed theta can be optionally passed in

Warning: Not to be confused with dxExpX(), which is derivative of the internal quaternion representation of So3 wrt the tangent vector

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

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, 3, kDoF> dxExpXTimesPointAt0(Point const& point)

Returns derivative of exp(x) * p wrt. x 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 So3<Scalar> exp(Tangent const& omega)

Group exponential.

This functions takes in an element of tangent space (= rotation vector omega) and returns the corresponding element of the group SO(3).

To be more specific, this function computes expmat(hat(omega)) with expmat(.) being the matrix exponential and hat(.) being the hat() -operator of SO(3).

static SOPHUS_FUNC So3AndTheta expAndTheta(Tangent const& omega)

As above, but also returns theta = |omega| as out-parameter.

Precondition: theta must not be nullptr.

template <class TS = Scalar>
static SOPHUS_FUNC std::enable_if_t<std::is_floating_point<TS>::value, So3> fitToSo3(Transformation const& r)

Returns closest So3 given arbitrary 3x3 matrix.

static SOPHUS_FUNC Transformation generator(int i)

Returns the ith infinitesimal generators of SO(3).

The infinitesimal generators of SO(3) are:

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

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

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

Precondition: i must be 0, 1 or 2.

static SOPHUS_FUNC Transformation hat(Tangent const& omega)

hat-operator

It takes in the 3-vector representation omega (= rotation vector) and returns the corresponding matrix representation of Lie algebra element.

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

hat(.): R^3 -> R^{3x3}, hat(omega) = sum_i omega_i * G_i (for i=0,1,2)

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

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

static SOPHUS_FUNC Tangent lieBracket(Tangent const& omega1, Tangent const& omega2)

Lie bracket.

It computes the Lie bracket of SO(3). To be more specific, it computes

[omega_1, omega_2]_so3 := 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 So3.

For the Lie algebra so3, the Lie bracket is simply the cross product:

[omega_1, omega_2]_so3 = omega_1 x omega_2.

static SOPHUS_FUNC So3 rotX(Scalar const& x)

Construct x-axis rotation.

static SOPHUS_FUNC So3 rotY(Scalar const& y)

Construct y-axis rotation.

static SOPHUS_FUNC So3 rotZ(Scalar const& z)

Construct z-axis rotation.

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

Draw uniform sample from SO(3) manifold. Based on: http://planning.cs.uiuc.edu/node198.html.

static SOPHUS_FUNC Tangent vee(Transformation const& omega)

vee-operator

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

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

Precondition: Omega must have the following structure:

|  0 -c  b |
|  c  0 -a |
| -b  a  0 |
SOPHUS_FUNC QuaternionMember& mutUnitQuaternion()

Mutator of unit_quaternion is protected to ensure class invariant.