Template Class CQuaternion

Inheritance Relationships

Base Type

Class Documentation

template<class T>
class CQuaternion : public mrpt::math::CMatrixFixed<T, 4>

A quaternion, which can represent a 3D rotation as pair (r, u) with a real part “r” and a 3D vector u = (x,y,z), or alternatively, q = r + ix + jy + kz.

The elements of the quaternion can be accessed by either:

  • r()(equivalent to w()), x(), y(), z(), or

  • the operator [] with indices: [0]=w, [1]:x, [2]:y, [3]:z

Users will usually employ the type CQuaternionDouble instead of this template.

For more information about quaternions, see:

See also

mrpt::poses::CPose3D

Lie Algebra methods

template<class ARRAYLIKE3>
inline void ln(ARRAYLIKE3 &out_ln) const

Logarithm of the 3x3 matrix defined by this pose, generating the corresponding vector in the SO(3) Lie Algebra, which coincides with the so-called “rotation vector” (I don’t have space here for the proof ;-).

See also

exp, mrpt::poses::SE_traits

Parameters:

out_ln – [out] The target vector, which can be: std::vector<>, or mrpt::math::CVectorDouble or any row or column Eigen::Matrix<>.

template<class ARRAYLIKE3>
inline ARRAYLIKE3 ln() const

overload that returns by value

template<class ARRAYLIKE3>
inline void ln_noresize(ARRAYLIKE3 &out_ln) const

Like ln() but does not try to resize the output vector.

template<class ARRAYLIKE3>
static inline CQuaternion<T> exp(const ARRAYLIKE3 &v)

Exponential map from the SO(3) Lie Algebra to unit quaternions.

See also

ln, mrpt::poses::SE_traits

template<class ARRAYLIKE3>
static inline void exp(const ARRAYLIKE3 &v, CQuaternion<T> &out_quat)

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

Public Functions

inline CQuaternion(TConstructorFlags_Quaternions)

Can be used with UNINITIALIZED_QUATERNION as argument, does not initialize the 4 elements of the quaternion (use this constructor when speed is critical).

inline CQuaternion()

Default constructor: construct a (1, (0,0,0) ) quaternion representing no rotation.

inline CQuaternion(const T R, const T X, const T Y, const T Z)

Construct a quaternion from its parameters ‘r’, ‘x’, ‘y’, ‘z’, with q = r + ix + jy + kz.

inline void ensurePositiveRealPart()

Adhere to the convention of w>=0 to avoid ambiguity of quaternion double cover of SO(3)

inline T r() const

Return r (real part) coordinate of the quaternion

inline T w() const

Return w (real part) coordinate of the quaternion. Alias of r()

inline T x() const

Return x coordinate of the quaternion

inline T y() const

Return y coordinate of the quaternion

inline T z() const

Return z coordinate of the quaternion

inline void r(const T r)

Set r (real part) coordinate of the quaternion

inline void w(const T w)

Set w (real part) coordinate of the quaternion. Alias of r()

inline void x(const T x)

Set x coordinate of the quaternion

inline void y(const T y)

Set y coordinate of the quaternion

inline void z(const T z)

Set z coordinate of the quaternion

inline T &r()
inline T &x()
inline T &y()
inline T &z()
template<class ARRAYLIKE3>
inline void fromRodriguesVector(const ARRAYLIKE3 &v)

Set this quaternion to the rotation described by a 3D (Rodrigues) rotation vector v.

If v = 0, then q = [1 0 0 0]. Otherwise: 

*    theta = |v| = sqrt(vx^2 + vy^2 + vz^2)
*
*    q = [ cos(theta/2)            ]   (qr)
*        [ vx * sin(theta/2)/theta ]   (qx)
*        [ vy * sin(theta/2)/theta ]   (qy)
*        [ vz * sin(theta/2)/theta ]   (qz)
*

See also

“Representing Attitude: Euler Angles, Unit Quaternions, and

Rotation Vectors (2006)”, James Diebel.

inline void crossProduct(const CQuaternion &q1, const CQuaternion &q2)

Calculate the “cross” product (or “composed rotation”) of two quaternion: this = q1 x q2 After the operation, “this” will represent the composed rotations of q1 and q2 (q2 applied “after” q1).

inline void rotatePoint(const double lx, const double ly, const double lz, double &gx, double &gy, double &gz) const

Rotate a 3D point (lx,ly,lz) -> (gx,gy,gz) as described by this quaternion

inline void inverseRotatePoint(const double lx, const double ly, const double lz, double &gx, double &gy, double &gz) const

Rotate a 3D point (lx,ly,lz) -> (gx,gy,gz) as described by the inverse (conjugate) of this quaternion

inline double normSqr() const

Return the squared norm of the quaternion

inline void normalize()

Normalize this quaternion, so its norm becomes the unitity.

template<class MATRIXLIKE>
inline void normalizationJacobian(MATRIXLIKE &J) const

Calculate the 4x4 Jacobian of the normalization operation of this quaternion. The output matrix can be a dynamic or fixed size (4x4) matrix.

template<class MATRIXLIKE>
inline void rotationJacobian(MATRIXLIKE &J) const

Compute the Jacobian of the rotation composition operation p = f(.) = q_this x r, that is the 4x4 matrix df/dq_this. The output matrix can be a dynamic or fixed size (4x4) matrix.

template<class MATRIXLIKE>
inline void rotationMatrix(MATRIXLIKE &M) const

Calculate the 3x3 rotation matrix associated to this quaternion.

Let r=qr, x=qx, y=qy, z=qz. Then: 

*   R = | r^2+x^2-y^2-z^2   2(xy - rz)        2(zx + ry)       |
*       | 2(xy + rz)         r^2-x^2+y^2-z^2   2(yz - rx)       |
*       | 2(zx - ry)         2(yz + rx)         r^2-x^2-y^2+z^2 |
*

template<class MATRIXLIKE>
inline MATRIXLIKE rotationMatrix() const
template<class MATRIXLIKE>
inline void rotationMatrixNoResize(MATRIXLIKE &M) const

Fill out the top-left 3x3 block of the given matrix with the rotation matrix associated to this quaternion (does not resize the matrix, for that, see rotationMatrix).

inline void conj(CQuaternion &q_out) const

Return the conjugate quaternion

inline CQuaternion conj() const

Return the conjugate quaternion

inline void rpy(T &roll, T &pitch, T &yaw) const

Return the yaw, pitch & roll angles associated to quaternion

See also

For the equations, see The MRPT Book, or see http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/Quaternions.pdf

See also

rpy_and_jacobian

template<class MATRIXLIKE>
inline void rpy_and_jacobian(T &roll, T &pitch, T &yaw, MATRIXLIKE *out_dr_dq = nullptr, bool resize_out_dr_dq_to3x4 = true) const

Return the yaw, pitch & roll angles associated to quaternion, and (optionally) the 3x4 Jacobian of the transformation. Note that both the angles and the Jacobian have one set of normal equations, plus other special formulas for the degenerated cases of |pitch|=90 degrees.

See also

For the equations, see The MRPT Book, or http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/Quaternions.pdf

See also

rpy

inline CQuaternion operator*(const T &factor)
inline mrpt::math::CMatrixFixed<double, 3, 4> jacobian_rodrigues_from_quat() const

Computes the 3x4 rotation Jacobian that maps quaternion to SE(3) It is obtained by partial differentiation of the following equation wrt the quaternion components (quat=$[q_r, \bm{q}_v]^T): $\boldsymbol{\omega} = \frac{2\arccos{q_r}}{|\mathbf{q}_v|}\mathbf{q}_v$