template class mrpt::math::CQuaternion
Overview
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:
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:
#include <mrpt/math/CQuaternion.h> template <class T> class CQuaternion: public mrpt::math::CMatrixFixed { public: // 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 ); 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 matrix_index_t Index_t; typedef matrix_dim_t size_type_t; typedef T value_type; typedef T Scalar; typedef matrix_index_t Index; typedef T& reference; typedef const T& const_reference; typedef matrix_dim_t size_type; typedef std::ptrdiff_t difference_type; typedef Eigen::Matrix<T, ROWS, COLS, StorageOrder, ROWS, COLS> eigen_t; typedef typename vec_t::iterator iterator; typedef typename vec_t::const_iterator const_iterator; // 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; // methods void fill(const Scalar& val); void setConstant(const Scalar value); void setConstant(matrix_dim_t nrows, matrix_dim_t ncols, const Scalar value); void setConstant(matrix_dim_t nrows, const Scalar value); void assign(const matrix_dim_t N, const Scalar value); void setZero(); void setZero(matrix_dim_t nrows, matrix_dim_t ncols); void setZero(matrix_dim_t nrows); static Derived Constant(const Scalar value); static Derived Constant(matrix_dim_t nrows, matrix_dim_t ncols, const Scalar value); static Derived Zero(); static Derived Zero(matrix_dim_t nrows, matrix_dim_t ncols); template <matrix_dim_t BLOCK_ROWS, matrix_dim_t BLOCK_COLS> auto block( matrix_index_t start_row, matrix_index_t start_col ); auto block(matrix_index_t start_row, matrix_index_t start_col, matrix_dim_t BLOCK_ROWS, matrix_dim_t BLOCK_COLS); auto block(matrix_index_t start_row, matrix_index_t start_col, matrix_dim_t BLOCK_ROWS, matrix_dim_t 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 <matrix_dim_t N> CMatrixFixed<Scalar, N, 1> tail() const; template <matrix_dim_t N> CMatrixFixed<Scalar, N, 1> head() const; Scalar& coeffRef(matrix_index_t r, matrix_index_t c); const Scalar& coeff(matrix_index_t r, matrix_index_t c) const; Scalar minCoeff() const; Scalar minCoeff(matrix_index_t& outIndexOfMin) const; Scalar minCoeff(matrix_index_t& rowIdx, matrix_index_t& colIdx) const; Scalar maxCoeff() const; Scalar maxCoeff(matrix_index_t& outIndexOfMax) const; Scalar maxCoeff(matrix_index_t& rowIdx, matrix_index_t& colIdx) const; bool isSquare() const; bool empty() const; Scalar norm_inf() const; Scalar norm() 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 CVectorDynamic<Scalar>& v) const; Scalar dot(const MatrixVectorBase<Scalar, Derived>& v) const; void matProductOf_Ab(const CMatrixDynamic<Scalar>& A, const CVectorDynamic<Scalar>& b); void matProductOf_Atb(const CMatrixDynamic<Scalar>& A, const CVectorDynamic<Scalar>& b); Scalar sum() const; Scalar sum_abs() const; std::string asString() const; bool fromMatlabStringFormat(const std::string& s, mrpt::optional_ref<std::ostream> dump_errors_here = std::nullopt); std::string inMatlabFormat(const std::size_t decimal_digits = 6) const; void saveToTextFile( const std::string& file, mrpt::math::TMatrixTextFileFormat fileFormat = mrpt::math::MATRIX_FORMAT_ENG, bool appendMRPTHeader = false, const std::string& userHeader = std::string() ) const; void loadFromTextFile(std::istream& f); void loadFromTextFile(const std::string& file); 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(Index_t colIdx); auto col(Index_t colIdx) const; auto row(Index_t rowIdx); auto row(Index_t rowIdx) const; template <typename VectorLike> void extractRow(Index_t rowIdx, VectorLike& v) const; template <typename VectorLike> VectorLike extractRow(Index_t rowIdx) const; template <typename VectorLike> void extractColumn(Index_t colIdx, VectorLike& v) const; template <typename VectorLike> VectorLike extractColumn(Index_t colIdx) const; Scalar det() const; Derived inverse() const; Derived inverse_LLt() const; matrix_dim_t rank(Scalar threshold = 0) const; bool chol(Derived& U) const; bool eig(Derived& eVecs, std::vector<Scalar>& eVals, bool sorted = true) const; bool eig_symmetric(Derived& eVecs, std::vector<Scalar>& eVals, bool sorted = true) const; Scalar maximumDiagonal() const; Scalar minimumDiagonal() const; Scalar trace() const; void unsafeRemoveColumns(const std::vector<std::size_t>& idxs); void removeColumns(const std::vector<std::size_t>& idxsToRemove); void unsafeRemoveRows(const std::vector<std::size_t>& idxs); void removeRows(const std::vector<std::size_t>& idxsToRemove); template <typename OtherMatrixOrVector> void insertMatrix( const Index_t row_start, const Index_t col_start, const OtherMatrixOrVector& submat ); template <typename OtherMatrixOrVector> void insertMatrixTransposed( const Index_t row_start, const Index_t col_start, const OtherMatrixOrVector& submat ); template <size_type_t BLOCK_ROWS, size_type_t BLOCK_COLS> CMatrixFixed<Scalar, BLOCK_ROWS, BLOCK_COLS> blockCopy( Index_t start_row = 0, Index_t start_col = 0 ) const; CMatrixDynamic<Scalar> blockCopy(Index_t start_row, Index_t start_col, size_type_t BLOCK_ROWS, size_type_t BLOCK_COLS) const; template <size_type_t BLOCK_ROWS, size_type_t BLOCK_COLS> CMatrixFixed<Scalar, BLOCK_ROWS, BLOCK_COLS> extractMatrix( const Index_t start_row = 0, const Index_t start_col = 0 ) const; CMatrixDynamic<Scalar> extractMatrix( const size_type_t BLOCK_ROWS, const size_type_t BLOCK_COLS, const Index_t start_row, const Index_t start_col ) const; template <typename MAT_A> void matProductOf_AAt(const MAT_A& A); template <typename MAT_A> void matProductOf_AtA(const MAT_A& A); Derived& mbDerived(); const Derived& mbDerived() const; void setDiagonal(const size_type_t N, const Scalar value); void setDiagonal(const Scalar value); void setDiagonal(const std::vector<Scalar>& diags); void setIdentity(); void setIdentity(const size_type_t N); void matProductOf_AB(const Derived& A, const Derived& B); static Derived Identity(); static Derived Identity(const size_type_t N); iterator begin(); iterator end(); const_iterator begin() const; const_iterator end() const; const_iterator cbegin() const; const_iterator cend() const; template <class MAT> void setFromMatrixLike(const MAT& m); 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); template <typename VECTOR> void loadFromArray(const VECTOR& vals); void loadFromRawPointer(const T* data); void setSize(size_type row, size_type col, ] bool zeroNewElements = false); void swap(CMatrixFixed& o); CMatrixFixed& derived(); const CMatrixFixed& derived() const; void conservativeResize(size_type row, size_type col); void resize(size_type n); void resize(const matrix_size_t& siz, ] bool zeroNewElements = false); void resize(size_type row, size_type col); 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, EIGEN_MAP_ALIGN_BYTES, Eigen::InnerStride<1>> > EIGEN_MAP asEigen(); template < typename EIGEN_MATRIX = eigen_t, typename EIGEN_MAP = Eigen::Map<const EIGEN_MATRIX, EIGEN_MAP_ALIGN_BYTES, Eigen::InnerStride<1>> > EIGEN_MAP asEigen() const; const T* data() const; T* data(); T& operator () (Index row, Index col); const T& operator () (Index row, Index col) const; T& operator () (Index i); const T& operator () (Index i) const; T& operator [] (Index i); const T& operator [] (Index i) const; CMatrixFixed<float, ROWS, COLS> cast_float() const; CMatrixFixed<double, ROWS, COLS> cast_double() const; 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 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.
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(.) = 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> 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> 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
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
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$.