template class mrpt::math::CQuaternion

A quaternion, which can represent a 3D rotation as pair \((r,\mathbf{u}) *\), with a real part “r” and a 3D vector \(\mathbf{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:

• http://people.csail.mit.edu/bkph/articles/Quaternions.pdf

• http://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation

See also:

mrpt::poses::CPose3D

#include <mrpt/math/CQuaternion.h>

template <class T>
class CQuaternion: public mrpt::math::CMatrixFixed
{
public:
    // typedefs

    typedef typename vec_t::iterator iterator;
    typedef typename vec_t::const_iterator const_iterator;
    typedef T Scalar;
    typedef int Index;
    typedef T& reference;
    typedef const T& const_reference;
    typedef int size_type;
    typedef std::ptrdiff_t difference_type;
    typedef Eigen::Matrix<T, ROWS, COLS, StorageOrder, ROWS, COLS> eigen_t;

    //
fields

    static constexpr static int RowsAtCompileTime = ROWS;
    static constexpr static int ColsAtCompileTime = COLS;
    static constexpr static int SizeAtCompileTime = ROWS* COLS;
    static constexpr static int is_mrpt_type = 1;
    static constexpr static int StorageOrder =(ROWS != 1&& COLS == 1) ? 0  : 1;

    // construction

    CQuaternion(TConstructorFlags_Quaternions);
    CQuaternion();
    CQuaternion(const T R, const T X, const T Y, const T Z);

    //
methods

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

    template <class ARRAYLIKE3>
    ARRAYLIKE3 ln() const;

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

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

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

    iterator begin();
    const_iterator begin() const;
    iterator end();
    const_iterator end() const;
    const_iterator cbegin() const;
    const_iterator cend() const;

    template <class MAT>
    void setFromMatrixLike(const MAT& m);

    template <typename VECTOR>
    void loadFromArray(const VECTOR& vals);

    void swap(CMatrixFixed& o);
    CMatrixFixed& derived();
    const CMatrixFixed& derived() const;

    void conservativeResize(
        size_t row,
        size_t col
        );

    void resize(size_t n);

    void resize(
        size_t row,
        size_t col
        );

    template <
        typename EIGEN_MATRIX = eigen_t,
        typename EIGEN_MAP = Eigen::Map<const EIGEN_MATRIX, MRPT_MAX_STATIC_ALIGN_BYTES,            Eigen::InnerStride<1>>
        >
    EIGEN_MAP asEigen() const;

    const T& operator () (
        int row,
        int col
        ) const;

    const T& operator () (int i) const;
    const T& operator [] (int i) const;
    CMatrixFixed<float, ROWS, COLS> cast_float() const;
    CMatrixFixed<double, ROWS, COLS> cast_double() const;
    void ensurePositiveRealPart();
    T r() const;
    T w() const;
    T x() const;
    T y() const;
    T z() const;
    void r(const T r);
    void w(const T w);
    void x(const T x);
    void y(const T y);
    void z(const T z);
    T& r();
    T& x();
    T& y();
    T& z();

    template <class ARRAYLIKE3>
    void fromRodriguesVector(const ARRAYLIKE3& v);

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

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

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

    double normSqr() const;
    void normalize();

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

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

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

    template <class MATRIXLIKE>
    MATRIXLIKE rotationMatrix() const;

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

    void conj(CQuaternion& q_out) const;
    CQuaternion conj() const;
    void rpy(T& roll, T& pitch, T& yaw) const;

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

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

Inherited Members

public:
    // typedefs

    typedef T value_type;

    //
methods

    void setConstant(const Scalar value);
    void setConstant(size_t nrows, size_t ncols, const Scalar value);
    void setConstant(size_t nrows, const Scalar value);
    void assign(const std::size_t N, const Scalar value);
    void setZero();
    void setZero(size_t nrows, size_t ncols);
    void setZero(size_t nrows);
    static Derived Constant(const Scalar value);
    static Derived Constant(size_t nrows, size_t ncols, const Scalar value);
    static Derived Zero();
    static Derived Zero(size_t nrows, size_t ncols);
    auto block(int start_row, int start_col, int BLOCK_ROWS, int BLOCK_COLS);
    auto block(int start_row, int start_col, int BLOCK_ROWS, int BLOCK_COLS) const;
    auto transpose();
    auto transpose() const;
    auto array();
    auto array() const;
    auto operator - () const;

    template <typename S2, class D2>
    auto operator + (const MatrixVectorBase<S2, D2>& m2) const;

    template <typename S2, class D2>
    void operator += (const MatrixVectorBase<S2, D2>& m2);

    template <typename S2, class D2>
    auto operator - (const MatrixVectorBase<S2, D2>& m2) const;

    template <typename S2, class D2>
    void operator -= (const MatrixVectorBase<S2, D2>& m2);

    template <typename S2, class D2>
    auto operator * (const MatrixVectorBase<S2, D2>& m2) const;

    auto operator * (const Scalar s) const;

    template <int N>
    CMatrixFixed<Scalar, N, 1> tail() const;

    template <int N>
    CMatrixFixed<Scalar, N, 1> head() const;

    Scalar& coeffRef(int r, int c);
    const Scalar& coeff(int r, int c) const;
    Scalar minCoeff(std::size_t& outIndexOfMin) const;
    Scalar minCoeff(std::size_t& rowIdx, std::size_t& colIdx) const;
    Scalar maxCoeff(std::size_t& outIndexOfMax) const;
    Scalar maxCoeff(std::size_t& rowIdx, std::size_t& colIdx) const;
    void operator += (Scalar s);
    void operator -= (Scalar s);
    void operator *= (Scalar s);
    CMatrixDynamic<Scalar> operator * (const CMatrixDynamic<Scalar>& v);
    Derived operator + (const Derived& m2) const;
    void operator += (const Derived& m2);
    Derived operator - (const Derived& m2) const;
    void operator -= (const Derived& m2);
    Derived operator * (const Derived& m2) const;
    Scalar dot(const MatrixVectorBase<Scalar, Derived>& v) const;

    template <typename OTHERMATVEC>
    bool operator == (const OTHERMATVEC& o) const;

    template <typename OTHERMATVEC>
    bool operator != (const OTHERMATVEC& o) const;

    Derived& mvbDerived();
    const Derived& mvbDerived() const;
    auto col(int colIdx);
    auto col(int colIdx) const;
    auto row(int rowIdx);
    auto row(int rowIdx) const;

    template <typename VECTOR_LIKE>
    void extractRow(int rowIdx, VECTOR_LIKE& v) const;

    template <typename VECTOR_LIKE>
    VECTOR_LIKE extractRow(int rowIdx) const;

    template <typename VECTOR_LIKE>
    void extractColumn(int colIdx, VECTOR_LIKE& v) const;

    template <typename VECTOR_LIKE>
    VECTOR_LIKE extractColumn(int colIdx) const;

    template <int BLOCK_ROWS, int BLOCK_COLS>
    CMatrixFixed<Scalar, BLOCK_ROWS, BLOCK_COLS> extractMatrix(
        const int start_row = 0,
        const int start_col = 0
        ) const;

    CMatrixDynamic<Scalar> extractMatrix(
        const int BLOCK_ROWS,
        const int BLOCK_COLS,
        const int start_row,
        const int start_col
        ) const;

    Derived& mbDerived();
    const Derived& mbDerived() const;
    void setIdentity();
    void setIdentity(const std::size_t N);
    static Derived Identity();
    static Derived Identity(const std::size_t N);

    template <class Derived>
    CMatrixFixed& operator = (const Eigen::MatrixBase<Derived>& m);

    template <typename VectorType, int Size>
    CMatrixFixed& operator = (const Eigen::VectorBlock<VectorType, Size>& m);

    template <typename U>
    CMatrixFixed& operator = (const CMatrixDynamic<U>& m);

    void loadFromRawPointer(const T* data);
    void setSize(size_t row, size_t col, ] bool zeroNewElements = false);
    void resize(const matrix_size_t& siz, ] bool zeroNewElements = false);
    constexpr size_type rows() const;
    constexpr size_type cols() const;
    constexpr matrix_size_t size() const;

    template <
        typename EIGEN_MATRIX = eigen_t,
        typename EIGEN_MAP = Eigen::Map<EIGEN_MATRIX, MRPT_MAX_STATIC_ALIGN_BYTES, Eigen::InnerStride<1>>
        >
    EIGEN_MAP asEigen();

    const T* data() const;
    T* data();
    T& operator () (int row, int col);
    T& operator () (int i);
    T& operator [] (int i);
    CMatrixFixed<T, ROWS, 1> llt_solve(const CMatrixFixed<T, ROWS, 1>& b) const;
    CMatrixFixed<T, ROWS, 1> lu_solve(const CMatrixFixed<T, ROWS, 1>& b) const;
    void sum_At(const CMatrixFixed<Scalar, ROWS, COLS>& A);

Construction

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).

CQuaternion()

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

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.

Methods

template <class ARRAYLIKE3>
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 ;-).

Parameters:

out_ln

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

See also:

exp, mrpt::poses::SE_traits

template <class ARRAYLIKE3>
ARRAYLIKE3 ln() const

overload that returns by value

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

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

template <class ARRAYLIKE3>
static 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 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.

void ensurePositiveRealPart()

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

T r() const

Return r (real part) coordinate of the quaternion.

T w() const

Return w (real part) coordinate of the quaternion.

Alias of r()

T x() const

Return x coordinate of the quaternion.

T y() const

Return y coordinate of the quaternion.

T z() const

Return z coordinate of the quaternion.

void r(const T r)

Set r (real part) coordinate of the quaternion.

void w(const T w)

Set w (real part) coordinate of the quaternion.

Alias of r()

void x(const T x)

Set x coordinate of the quaternion.

void y(const T y)

Set y coordinate of the quaternion.

void z(const T z)

Set z coordinate of the quaternion.

template <class ARRAYLIKE3>
void fromRodriguesVector(const ARRAYLIKE3& v)

Set this quaternion to the rotation described by a 3D (Rodrigues) rotation vector \(\mathbf{v}\) : If \(\mathbf{v}=0\), then the quaternion is \(\mathbf{q} = [1 ~ 0 ~ 0 ~ 0]^\top\), otherwise:

\[\begin{split}\mathbf{q} = \left[ \begin{array}{c} \cos(\frac{\theta}{2}) \\ v_x \frac{\sin(\frac{\theta}{2})}{\theta} \\ v_y \frac{\sin(\frac{\theta}{2})}{\theta} \\ v_z \frac{\sin(\frac{\theta}{2})}{\theta} \end{array} \right]\end{split}\]

where \(\theta = |\mathbf{v}| = \sqrt{v_x^2+v_y^2+v_z^2}\).

See also:

“Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors (2006)”, James Diebel.

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).

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.

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.

double normSqr() const

Return the squared norm of the quaternion.

void normalize()

Normalize this quaternion, so its norm becomes the unitity.

template <class MATRIXLIKE>
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>
void rotationJacobian(MATRIXLIKE& J) const

Compute the Jacobian of the rotation composition operation \(p = f(\cdot) = q_{this} \times r\), that is the 4x4 matrix \(\frac{\partial f}{\partial q_{this} }\).

The output matrix can be a dynamic or fixed size (4x4) matrix.

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

Calculate the 3x3 rotation matrix associated to this quaternion:

\[\begin{split}\mathbf{R} = \left( \begin{array}{ccc} q_r^2+q_x^2-q_y^2-q_z^2 & 2(q_x q_y - q_r q_z) & 2(q_z q_x+q_r q_y) \\ 2(q_x q_y+q_r q_z) & q_r^2-q_x^2+q_y^2-q_z^2 & 2(q_y q_z-q_r q_x) \\ 2(q_z q_x-q_r q_y) & 2(q_y q_z+q_r q_x) & q_r^2- q_x^2 - q_y^2 + q_z^2 \end{array} \right)\end{split}\]

template <class MATRIXLIKE>
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).

void conj(CQuaternion& q_out) const

Return the conjugate quaternion

CQuaternion conj() const

Return the conjugate quaternion

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

rpy_and_jacobian

template <class MATRIXLIKE>
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

rpy

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{2arccos{q_r}}{|mathbf{q}_v|}mathbf{q}_v$.