Common

template<typename _Scalar> void motion3d::normalizeAngle_(_Scalar &x): Inplace variant of normalizeAngle().

template<typename _Scalar> _Scalar motion3d::normalizeAngle(_Scalar x): Normalizes angle x to range $[-\pi,\pi)$ .

template<typename Derived> Derived::Scalar motion3d::getAxisNormalizationFactorInverse(const Eigen::MatrixBase<Derived> &v, bool unit_norm)

Returns:: the inverse factor for normalizing an axis to the upper hemisphere.

template<typename Derived> void motion3d::normalizeRotationMatrix_(Eigen::MatrixBase<Derived> &m): Inplace variant of normalizeRotationMatrix().

template<typename Derived> Eigen::Matrix<typename Derived::Scalar, 3, 3> motion3d::normalizeRotationMatrix(const Eigen::MatrixBase<Derived> &m): Normalizes matrix m to a valid $\mathrm{SO(3)}$ rotation matrix.

template<typename Derived> std::tuple<Eigen::Matrix<typename Derived::Scalar, 3, 3>, Eigen::Matrix<typename Derived::Scalar, 3, 1>, Eigen::Matrix<typename Derived::Scalar, 3, 1>> motion3d::decomposeRZS(const Eigen::MatrixBase<Derived> &m): Decomposes a 3x3 matrix into rotations, zooms and shears.

Note

The implementation is based on transforms3d.affines.decomposeA44: https://github.com/matthew-brett/transforms3d/blob/master/transforms3d/affines.py#L10

class Time

A class for storing, converting and comparing timestamps.

Public Functions

Time() = default: Default constructor leaving the time uninitialized.

explicit Time(double t_sec): Constructs and initializes from time in seconds.

explicit Time(std::uint64_t t_nsec): Constructs and initializes from time in nanoseconds.

template<class Clock, class Duration> explicit Time(const std::chrono::time_point<Clock, Duration> &t): Constructs and initializes from std::chrono::time_point.

explicit Time(const std::string &t_nsec): Constructs and initializes from a string containing the time in nanoseconds.

explicit Time(const std::vector<std::uint8_t> &binary): Constructs and initializes from a vector with binary data.

Time(std::uint32_t t_sec, std::uint32_t t_nsec): Constructs and initializes from time separated into seconds and nanoseconds.

double toSec() const

Returns:: time in seconds.

std::uint64_t toNSec() const

Returns:: time in nanoseconds.

std::chrono::nanoseconds toChrono() const

Returns:: time as std::chrono duration.

std::pair<std::uint32_t, std::uint32_t> toSecNSec() const

Returns:: time separated into seconds and nanoseconds.

std::vector<std::uint8_t> toBinary() const

Returns:: time as binary data.

inline std::string desc() const

Returns:: a description of *this.

Public Static Functions

static Time FromSec(double t_sec)

See also

Time(double)

static Time FromNSec(std::uint64_t t_nsec)

See also

Time(std::uint64_t)

static Time FromSecNSec(std::uint32_t t_sec, std::uint32_t t_nsec)

See also

Time(std::uint32_t, std::uint32_t)

Public Static Attributes

static constexpr std::uint32_t kBinarySize = 8

Private Members

std::uint64_t nsec_ = {0}: Time in nanoseconds.

Friends

inline friend bool operator==(const Time &lhs, const Time &rhs)

inline friend bool operator!=(const Time &lhs, const Time &rhs)

inline friend bool operator<(const Time &lhs, const Time &rhs)

inline friend bool operator<=(const Time &lhs, const Time &rhs)

inline friend bool operator>(const Time &lhs, const Time &rhs)

inline friend bool operator>=(const Time &lhs, const Time &rhs)

inline friend std::ostream &operator<<(std::ostream &os, const Time &t): Inserts the time in nanoseconds.

friend Time timeDiffAbs(const Time &ta, const Time &tb)

Returns:: the absolute difference between Time ta and Time tb.

template<typename _Scalar> class Quaternion

The quaternion class used for performing computations involving quaternions, including but not limited to 3D rotations.

This class represents a quaternion $\mathrm{q}=w+xi+yj+zk$ , extending the implementation of Eigen::Quaternion. It can also denoted vectorized as $\mathrm{vec(q)}=[w \ x \ y \ z]^\mathrm{T}$ or in scalar-vector notation as $\mathrm{q}=\{ w,\boldsymbol{v} \}$ with $\boldsymbol{v}=[x \ y \ z]^\mathrm{T}$ .

A unit quaternion satifies $||\mathrm{r}|| = 1$ . Unit quaternions can be used to represent spatial rotations.

Warning

Operations interpreting the quaternion as rotation have undefined behavior if the quaternion is not normalized.

Warning

The original coeffs() implemented in Eigen::Quaternion are in order $[x \ y \ z \ w]^\mathrm{T}$ .

Template Parameters:: _Scalar – the scalar type, i.e., the type of the coefficients

Public Types

using AngleAxisType = typename Eigen::Quaternion<_Scalar>::AngleAxisType

using Matrix3 = typename Eigen::Quaternion<_Scalar>::Matrix3

using Vector3 = typename Eigen::Quaternion<_Scalar>::Vector3

Public Functions

inline Quaternion(): Default constructor leaving the quaternion uninitialized.

inline explicit Quaternion(const AngleAxisType &aa): Constructs and initializes a quaternion from the angle-axis aa.

template<class Derived> inline Quaternion(const _Scalar &angle, const Eigen::MatrixBase<Derived> &axis): Constructs and initializes a quaternion from the axis-angle representation with the angle in rad.

inline Quaternion(const _Scalar &w, const _Scalar &x, const _Scalar &y, const _Scalar &z): Constructs and initializes the quaternion from its four coefficients w, x, y and z.

template<typename OtherScalar, int OtherOptions> inline explicit Quaternion(const Eigen::Quaternion<OtherScalar, OtherOptions> &other): Explicit copy constructor from Eigen quaternion with scalar conversion.

template<class Derived> inline Quaternion(const Eigen::QuaternionBase<Derived> &other): Copy constructor from Eigen quaternion.

inline Quaternion(Eigen::Quaternion<_Scalar> &&other): Move constructor from Eigen quaternion.

inline Quaternion<_Scalar> &operator=(Eigen::Quaternion<_Scalar> &&other): Move assignment operator from Eigen quaternion.

std::vector<_Scalar> toVector() const: Converts *this to std vector with the order $[w \ x \ y \ z]^\mathrm{T}$ .

Eigen::Matrix<_Scalar, kQuaternionDim, 1> toEigenVector() const: Converts *this to Eigen vector with the order $[w \ x \ y \ z]^\mathrm{T}$ .

Eigen::Matrix<_Scalar, kQuaternionDim, kQuaternionDim> toPositiveMatrix() const

Converts the left quaternion of a quaternion multiplication to a 4x4 matrix for representing the multiplication as matrix-vector product $\text{vec}(\mathrm{q}_a \mathrm{q}_b) = \boldsymbol{Q}_a^{+} \text{vec}(\mathrm{q}_b)$ .

The following two lines yield identical results for Quaterniond q1 and q2:

Eigen::Matrix<double, 4, 1> v1 = (q1 * q2).toEigenVector();
Eigen::Matrix<double, 4, 1> v2 = q1.toPositiveMatrix() * q2.toEigenVector();

Eigen::Matrix<_Scalar, kQuaternionDim, kQuaternionDim> toNegativeMatrix() const

Converts the right quaternion of a quaternion multiplication to a 4x4 matrix for representing the multiplication as matrix-vector product $\text{vec}(\mathrm{q}_a \mathrm{q}_b) = \boldsymbol{Q}_b^{-} \text{vec}(\mathrm{q}_a)$ .

The following two lines yield identical results for Quaterniond q1 and q2:

Eigen::Matrix<double, 4, 1> v1 = (q1 * q2).toEigenVector();
Eigen::Matrix<double, 4, 1> v2 = q2.toNegativeMatrix() * q1.toEigenVector();

inline AngleAxisType toAxisAngle() const: Converts *this to axis-angle representation.

_Scalar rotationNorm() const

Returns:: the rotation norm in rad.

Quaternion<_Scalar> &conjugate_(): Inplace variant of conjugate().

Quaternion<_Scalar> conjugate() const

Returns:: the conjugate quaternion $\mathrm{q}^{*} = w-xi-yj-zk$ .

Quaternion<_Scalar> &inverse_(): Inplace variant of inverse().

Quaternion<_Scalar> inverse() const

Throws:: MathException – if the norm of the quaternion is 0.
Returns:: the inverse quaternion so $\mathrm{q} \mathrm{q}^{-1} = \{ 1, \boldsymbol{0} \}$ .

Quaternion<_Scalar> &normalized_(): Inplace variant of normalized().

Quaternion<_Scalar> normalized() const

Returns:: the normalized quaternion with norm 1.

Quaternion<_Scalar> slerp(const _Scalar &t, const Quaternion<_Scalar> &other) const

Returns:: the spherical linear interpolation between *this and other for $t \in [0,1]$ .

bool isEqual(const Quaternion<_Scalar> &other, const _Scalar &eps = kDefaultEps) const

Returns:: true if *this is approximately equal to other, within the precision determined by eps.

_Scalar angularDistance(const Quaternion<_Scalar> &other) const

Returns:: the angular distance between *this and other in rad.

Quaternion<_Scalar> &setZero(): Fills *this with zeros.

Quaternion<_Scalar> &setIdentity()

See also

Identity()

template<typename Derived> Vector3 transformPoint(const Eigen::MatrixBase<Derived> &point) const: Rotates point $\boldsymbol{p}$ using $\mathrm{q} \mathrm{p} \mathrm{q}^{*}$ with $\mathrm{p} = \{ 0, \boldsymbol{p} \}$ .

inline std::string desc() const

Returns:: a description of *this.

Quaternion<_Scalar> operator*(const _Scalar &v) const: Coefficient-wise multiplication of *this and v.

Quaternion<_Scalar> &operator*=(const _Scalar &v)

See also

operator*(const _Scalar&) const

Quaternion<_Scalar> operator*(const Quaternion<_Scalar> &other) const: Quaternion product of *this and other.

Quaternion<_Scalar> operator*=(const Quaternion<_Scalar> &other)

Quaternion<_Scalar> operator/(const _Scalar &v) const: Coefficient-wise division of *this by v.

Quaternion<_Scalar> &operator/=(const _Scalar &v)

See also

operator/(const _Scalar&) const

Quaternion<_Scalar> operator+(const Quaternion<_Scalar> &other) const: Coefficient-wise summation of *this and other.

Quaternion<_Scalar> &operator+=(const Quaternion<_Scalar> &other)

Quaternion<_Scalar> operator-() const: Coefficient-wise negative of *this.

Quaternion<_Scalar> operator-(const Quaternion<_Scalar> &other) const: Coefficient-wise subtraction of *this and other.

Quaternion<_Scalar> &operator-=(const Quaternion<_Scalar> &other)

Public Static Functions

static Quaternion<_Scalar> Zero()

Returns:: a quaternion filled with zeros.

static Quaternion<_Scalar> Identity()

Returns:: an identity quaternion $\{ 1, \boldsymbol{0} \}$ .

static Quaternion<_Scalar> FromVector(const std::vector<_Scalar> &data)

Returns:: a quaternion from an std vector with the order $[w \ x \ y \ z]^\mathrm{T}$ .

template<typename Derived> static Quaternion<_Scalar> FromVector(const Eigen::DenseBase<Derived> &data)

Returns:: a quaternion from an Eigen vector with the order $[w \ x \ y \ z]^\mathrm{T}$ .

template<class Derived> static Quaternion<_Scalar> FromRotationMatrix(const Eigen::MatrixBase<Derived> &matrix)

Returns:: a quaternion converted from a rotation matrix expression.

Private Functions

template<typename Derived> inline explicit Quaternion(const Eigen::MatrixBase<Derived> &other)

Constructs and initializes a quaternion from either:

a rotation matrix expression,
a 4D vector expression representing quaternion coefficients in the order $[x \ y \ z \ w]^\mathrm{T}$ .

Warning

The coefficient order in this method is different from the usual order $[w \ x \ y \ z]^\mathrm{T}$ .

template<typename Derived> inline explicit Quaternion(const _Scalar *data): Constructs and initialize a quaternion from the array data in the order $[x \ y \ z \ w]^\mathrm{T}$ .

Warning

The coefficient order in this method is different from the usual order $[w \ x \ y \ z]^\mathrm{T}$ .

Friends

inline friend Quaternion<_Scalar> operator*(const _Scalar &v, const Quaternion<_Scalar> &q)

See also

operator*(const _Scalar&) const

template<typename _OtherScalar> friend std::ostream &operator<<(std::ostream &os, const Quaternion<_OtherScalar> &q): Inserts a description of *this.

template<typename _Scalar> class DualQuaternion

The dual quaternion class used for performing computations involving dual quaternions, including but not limited to 3D transformations.

This class represents a dual quaternion $\mathcal{Q}=\mathrm{r} + \epsilon \, \mathrm{d}$ with real part $\mathrm{r}$ and dual part $\mathrm{r}$ , which are both a Quaternion. It can also denoted vectorized as $\mathrm{vec}(\mathcal{Q})=[\boldsymbol{r}^\mathrm{T} \ \boldsymbol{d}^\mathrm{T}]^\mathrm{T}$ .

A unit dual quaternion satifies $||\mathrm{r}|| = 1$ and $\mathrm{r}^{\ast} \, \mathrm{d} + \mathrm{d}^{\ast} \, \mathrm{r} = 0$ . Unit dual quaternions can be used to represent spatial transformations.

Warning

Operations interpreting the dual quaternion as transformation have undefined behavior if the dual quaternion is not normalized.

Template Parameters:: _Scalar – the scalar type, i.e., the type of the coefficients

Public Types

using QuaternionType = Quaternion<_Scalar>

using Vector3 = typename Quaternion<_Scalar>::Vector3

Public Functions

DualQuaternion() = default: Default constructor leaving the dual quaternion uninitialized.

DualQuaternion(QuaternionType real, QuaternionType dual): Constructs and initializes a dual quaternion with the given real and dual part.

template<class Derived> DualQuaternion(const Eigen::DenseBase<Derived> &translation, const QuaternionType &rotation): Constructs and initializes a dual quaternion representing a transformation with the given translation and rotation.

DualQuaternion(const _Scalar &real_w, const _Scalar &real_x, const _Scalar &real_y, const _Scalar &real_z, const _Scalar &dual_w, const _Scalar &dual_x, const _Scalar &dual_y, const _Scalar &dual_z): Constructs and initializes a dual quaternion with the given coefficients.

std::vector<_Scalar> toVector() const: Converts *this to std vector with the order $[\boldsymbol{r}^\mathrm{T} \ \boldsymbol{d}^\mathrm{T}]^\mathrm{T}$ .

Eigen::Matrix<_Scalar, kDualQuaternionDim, 1> toEigenVector() const: Converts *this to Eigen vector with the order $[\boldsymbol{r}^\mathrm{T} \ \boldsymbol{d}^\mathrm{T}]^\mathrm{T}$ .

inline QuaternionType &real()

Returns:: a reference to the real part $\mathrm{r}$ .

inline const QuaternionType &real() const

Returns:: a const reference to the real part $\mathrm{r}$ .

inline QuaternionType &dual()

Returns:: a reference to the dual part $\mathrm{d}$ .

inline const QuaternionType &dual() const

Returns:: a const reference to the dual part $\mathrm{d}$ .

_Scalar squaredNorm() const

Returns:: the squared norm of all coefficients.

_Scalar norm() const

Returns:: the norm of all coefficients.

_Scalar translationNorm() const

Returns:: the translation norm.

_Scalar rotationNorm() const

Returns:: the rotation norm in rad.

DualQuaternion<_Scalar> &quatConjugate_(): Inplace variant of quatConjugate().

DualQuaternion<_Scalar> quatConjugate() const

Returns:: the quaternion conjugate $\mathcal{Q}^{*} = \mathrm{r}^{*} + \epsilon \, \mathrm{d}^{*}$ .

DualQuaternion<_Scalar> &dualConjugate_(): Inplace variant of dualConjugate().

DualQuaternion<_Scalar> dualConjugate() const

Returns:: the dual conjugate $\bar{\mathcal{Q}} = \mathrm{r} - \epsilon \, \mathrm{d}$ .

DualQuaternion<_Scalar> &combConjugate_(): Inplace variant of combConjugate().

DualQuaternion<_Scalar> combConjugate() const

Returns:: the combined conjugate $\bar{\mathcal{Q}}^{*} = \mathrm{r}^{*} - \epsilon \, \mathrm{d}^{*}$ .

DualQuaternion<_Scalar> &inverse_(): Inplace variant of inverse().

DualQuaternion<_Scalar> inverse() const

Returns:: the inverted dual quaternion.

DualQuaternion<_Scalar> &normalized_(): Inplace variant of normalized().

DualQuaternion<_Scalar> normalized() const

Returns:: the normalized dual quaternion with real part norm $\boldsymbol{r}^\mathrm{T} \boldsymbol{r} = 1$ and $\boldsymbol{r}^\mathrm{T} \boldsymbol{d} = 0$ .

bool isEqual(const DualQuaternion<_Scalar> &other, const _Scalar &eps = kDefaultEps) const

Returns:: true if *this is approximately equal to other, within the precision determined by eps.

DualQuaternion<_Scalar> &setZero(): Fills *this with zeros.

DualQuaternion<_Scalar> &setIdentity()

See also

Identity()

Eigen::Matrix<_Scalar, kDualQuaternionDim, kDualQuaternionDim> toPositiveMatrix() const

Converts the left dual quaternion of a dual quaternion multiplication to a 8x8 matrix for representing the multiplication as matrix-vector product $\text{vec}(\mathcal{Q}_a \mathcal{Q}_b) = \boldsymbol{Q}_a^{+} \text{vec}(\mathcal{Q}_b)$ .

The following two lines yield identical results for Quaterniond q1 and q2:

Eigen::Matrix<double, 8, 1> v1 = (q1 * q2).toEigenVector();
Eigen::Matrix<double, 8, 1> v2 = q1.toPositiveMatrix() * q2.toEigenVector();

Eigen::Matrix<_Scalar, kDualQuaternionDim, kDualQuaternionDim> toNegativeMatrix() const

Converts the right dual quaternion of a dual quaternion multiplication to a 8x8 matrix for representing the multiplication as matrix-vector product $\text{vec}(\mathcal{Q}_a \mathcal{Q}_b) = \boldsymbol{Q}_b^{-} \text{vec}(\mathcal{Q}_a)$ .

The following two lines yield identical results for DualQuaterniond q1 and q2:

Eigen::Matrix<double, 8, 1> v1 = (q1 * q2).toEigenVector();
Eigen::Matrix<double, 8, 1> v2 = q2.toNegativeMatrix() * q1.toEigenVector();

QuaternionType getTranslationQuaternion() const

Returns:: the translation quaternion $\mathrm{t} = 2 \mathrm{d} \mathrm{r}^{*}$ .

Vector3 getTranslationVector() const

Returns:: the vector part of $\mathrm{t}$ from getTranslationQuaternion().

template<typename Derived> Vector3 transformPoint(const Eigen::MatrixBase<Derived> &point) const: Transforms point $\boldsymbol{p}$ using $\mathcal{Q} \mathcal{P} \bar{\mathcal{Q}}^{*}$ with $\mathcal{P}$ from FromPoint().

inline std::string desc() const

Returns:: a description of *this.

DualQuaternion<_Scalar> operator*(const _Scalar &v) const: Coefficient-wise multiplication of *this and v.

DualQuaternion<_Scalar> &operator*=(const _Scalar &v)

See also

operator*(const _Scalar&) const

DualQuaternion<_Scalar> operator*(const DualQuaternion<_Scalar> &other) const: Dual quaternion product of *this and other.

DualQuaternion<_Scalar> &operator*=(const DualQuaternion<_Scalar> &other)

DualQuaternion<_Scalar> operator/(const _Scalar &v) const: Coefficient-wise division of *this by v.

DualQuaternion<_Scalar> &operator/=(const _Scalar &v)

See also

operator/(const _Scalar&) const

DualQuaternion<_Scalar> operator+(const DualQuaternion<_Scalar> &other) const: Coefficient-wise summation of *this and other.

DualQuaternion<_Scalar> &operator+=(const DualQuaternion<_Scalar> &other)

DualQuaternion<_Scalar> operator-() const: Coefficient-wise negative of *this.

DualQuaternion<_Scalar> operator-(const DualQuaternion<_Scalar> &other) const: Coefficient-wise subtraction of *this and other.

DualQuaternion<_Scalar> &operator-=(const DualQuaternion<_Scalar> &other)

template<typename Derived> Quaternion<_Scalar> createDualPart(const Eigen::DenseBase<Derived> &translation, const QuaternionType &rotation)

Public Static Functions

static DualQuaternion<_Scalar> Zero()

Returns:: a dual quaternion filled with zeros.

static DualQuaternion<_Scalar> Identity()

Returns:: an identity dual quaternion $\mathcal{Q} = \{1 , \boldsymbol{0} \} + \epsilon \, \{ 0 , \boldsymbol{0} \}$ .

static DualQuaternion<_Scalar> FromVector(const std::vector<_Scalar> &data)

Returns:: a dual quaternion from an std vector with the order $[\boldsymbol{r}^\mathrm{T} \ \boldsymbol{d}^\mathrm{T}]^\mathrm{T}$ .

template<typename Derived> static DualQuaternion<_Scalar> FromVector(const Eigen::DenseBase<Derived> &data)

Returns:: a dual quaternion from an Eigen vector with the order $[\boldsymbol{r}^\mathrm{T} \ \boldsymbol{d}^\mathrm{T}]^\mathrm{T}$ .

static DualQuaternion<_Scalar> FromVector(const std::vector<_Scalar> &real, const std::vector<_Scalar> &dual)

Returns:: a dual quaternion from the real and dual part as std vector.

template<typename DerivedReal, typename DerivedDual> static DualQuaternion<_Scalar> FromVector(const Eigen::DenseBase<DerivedReal> &real, const Eigen::DenseBase<DerivedDual> &dual)

Returns:: a dual quaternion from the real and dual part as Eigen vector.

template<class Derived> static DualQuaternion<_Scalar> FromPoint(const Eigen::DenseBase<Derived> &point)

Returns:: the dual quaternion $\mathcal{P} = \{1, \boldsymbol{0}\} + \epsilon \, \{0, \boldsymbol{p}\}$ representing the point $\boldsymbol{p}$ .

Private Members

QuaternionType real_: Real part.

QuaternionType dual_: Dual part.

Private Static Functions

template<class Derived> static QuaternionType createDualPart(const Eigen::DenseBase<Derived> &translation, const QuaternionType &rotation)

Returns:

the dual part $\mathrm{d}$ calculated from translation vector $\boldsymbol{t}$ and rotation quaternion $\mathrm{r}$ .

$\mathrm{d} = \frac{1}{2} \mathrm{t} \mathrm{r} \quad \text{with} \ \mathrm{t} = \{ 0, \boldsymbol{t} \}$

Friends

inline friend DualQuaternion<_Scalar> operator*(const _Scalar &v, const DualQuaternion<_Scalar> &dq)

See also

operator*(const _Scalar&) const

template<typename _OtherScalar> friend std::ostream &operator<<(std::ostream &os, const DualQuaternion<_OtherScalar> &dq): Inserts a description of *this.