Template Class SO3

Inheritance Relationships

Base Type

Class Documentation

template<class Scalar_, int Options>
class SO3 : public Sophus::SO3Base<SO3<Scalar_, Options>>

SO3 using default storage; derived from SO3Base.

Public Types

using Base = SO3Base<SO3<Scalar_, Options>>
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 QuaternionMember = Eigen::Quaternion<Scalar, Options>

Public Functions

SOPHUS_FUNC SO3 & operator= (SO3 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).

inline EIGEN_MAKE_ALIGNED_OPERATOR_NEW SOPHUS_FUNC SO3 ()

Default constructor initializes unit quaternion to identity rotation.

SOPHUS_FUNC SO3(SO3 const &other) = default

Copy constructor

template<class OtherDerived>
inline SOPHUS_FUNC SO3(SO3Base<OtherDerived> const &other)

Copy-like constructor from OtherDerived.

inline SOPHUS_FUNC SO3(Transformation const &R)

Constructor from rotation matrix

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

template<class D>
inline explicit SOPHUS_FUNC SO3(Eigen::QuaternionBase<D> const &quat)

Constructor from quaternion

Precondition: quaternion must not be close to zero.

inline SOPHUS_FUNC QuaternionMember const  & unit_quaternion () const

Accessor of unit quaternion.

Public Static Functions

static inline SOPHUS_FUNC Sophus::Matrix< Scalar, DoF, DoF > leftJacobian (Tangent const &omega, optional< Scalar > const &theta_o=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 Dx_exp_x(), which is derivative of the internal quaternion representation of SO3 wrt the tangent vector

static inline SOPHUS_FUNC Sophus::Matrix< Scalar, DoF, DoF > leftJacobianInverse (Tangent const &omega, optional< Scalar > const &theta_o=nullopt)
static inline SOPHUS_FUNC Sophus::Matrix< Scalar, num_parameters, DoF > Dx_exp_x (Tangent const &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 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 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 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 inline SOPHUS_FUNC SO3< Scalar > expAndTheta (Tangent const &omega, Scalar *theta)

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

Precondition: theta must not be nullptr.

template<class S = Scalar> static inline SOPHUS_FUNC enable_if_t< std::is_floating_point< S >::value, SO3 > fitToSO3 (Transformation const &R)

Returns closest SO3 given arbitrary 3x3 matrix.

static inline 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 inline 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 inline 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 inline SOPHUS_FUNC SO3 rotX (Scalar const &x)

Construct x-axis rotation.

static inline SOPHUS_FUNC SO3 rotY (Scalar const &y)

Construct y-axis rotation.

static inline SOPHUS_FUNC SO3 rotZ (Scalar const &z)

Construct z-axis rotation.

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

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

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

Public Static Attributes

static constexpr int DoF = Base::DoF
static constexpr int num_parameters = Base::num_parameters

Protected Functions

inline SOPHUS_FUNC QuaternionMember & unit_quaternion_nonconst ()

Mutator of unit_quaternion is protected to ensure class invariant.

Protected Attributes

QuaternionMember unit_quaternion_

Friends

friend class SO3Base< SO3< Scalar, Options > >