A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector , or alternatively, q = r + ix + jy + kz.
The elements of the quaternion can be accessed by either:
Users will usually employ the typedef "CQuaternionDouble" instead of this template.
For more information about quaternions, see:
Definition at line 46 of file CQuaternion.h.
#include <mrpt/math/CQuaternion.h>
Public Types | |
typedef T | value_type |
Public Member Functions | |
T | r () const |
Return r coordinate of the quaternion. More... | |
T | x () const |
Return x coordinate of the quaternion. More... | |
T | y () const |
Return y coordinate of the quaternion. More... | |
T | z () const |
Return z coordinate of the quaternion. More... | |
void | r (const T r) |
Set r coordinate of the quaternion. More... | |
void | x (const T x) |
Set x coordinate of the quaternion. More... | |
void | y (const T y) |
Set y coordinate of the quaternion. More... | |
void | z (const T z) |
Set z coordinate of the quaternion. More... | |
template<class ARRAYLIKE3 > | |
void | fromRodriguesVector (const ARRAYLIKE3 &v) |
Set this quaternion to the rotation described by a 3D (Rodrigues) rotation vector : If , then the quaternion is , otherwise:
where . More... | |
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). More... | |
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. More... | |
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. More... | |
double | normSqr () const |
Return the squared norm of the quaternion. More... | |
void | normalize () |
Normalize this quaternion, so its norm becomes the unitity. More... | |
template<class MATRIXLIKE > | |
void | normalizationJacobian (MATRIXLIKE &J) const |
Calculate the 4x4 Jacobian of the normalization operation of this quaternion. More... | |
template<class MATRIXLIKE > | |
void | rotationJacobian (MATRIXLIKE &J) const |
Compute the Jacobian of the rotation composition operation , that is the 4x4 matrix . More... | |
template<class MATRIXLIKE > | |
void | rotationMatrix (MATRIXLIKE &M) const |
Calculate the 3x3 rotation matrix associated to this quaternion:
. More... | |
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). More... | |
void | conj (CQuaternion &q_out) const |
Return the conjugate quaternion. More... | |
CQuaternion | conj () const |
Return the conjugate quaternion. More... | |
void | rpy (T &roll, T &pitch, T &yaw) const |
Return the yaw, pitch & roll angles associated to quaternion. More... | |
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. More... | |
CQuaternion | operator* (const T &factor) |
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). More... | |
CQuaternion () | |
Default constructor: construct a (1, (0,0,0) ) quaternion representing no rotation. More... | |
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. More... | |
Private Types | |
typedef CArrayNumeric< T, 4 > | Base |
Lie Algebra 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 ;-). More... | |
template<class ARRAYLIKE3 > | |
ARRAYLIKE3 | ln () const |
overload that returns by value More... | |
template<class ARRAYLIKE3 > | |
void | ln_noresize (ARRAYLIKE3 &out_ln) const |
Like ln() but does not try to resize the output vector. More... | |
template<class ARRAYLIKE3 > | |
static CQuaternion< T > | exp (const ARRAYLIKE3 &v) |
Exponential map from the SO(3) Lie Algebra to unit quaternions. More... | |
template<class ARRAYLIKE3 > | |
static void | exp (const ARRAYLIKE3 &v, CQuaternion< T > &out_quat) |
|
private |
Definition at line 48 of file CQuaternion.h.
|
inherited |
Definition at line 29 of file CArrayNumeric.h.
|
inline |
Can be used with UNINITIALIZED_QUATERNION as argument, does not initialize the 4 elements of the quaternion (use this constructor when speed is critical).
Definition at line 57 of file CQuaternion.h.
|
inline |
Default constructor: construct a (1, (0,0,0) ) quaternion representing no rotation.
Definition at line 60 of file CQuaternion.h.
|
inline |
Construct a quaternion from its parameters 'r', 'x', 'y', 'z', with q = r + ix + jy + kz.
Definition at line 70 of file CQuaternion.h.
References ASSERTMSG_, mrpt::mrpt::format(), mrpt::math::CQuaternion< T >::normSqr(), mrpt::math::CQuaternion< T >::r(), mrpt::math::CQuaternion< T >::x(), mrpt::math::CQuaternion< T >::y(), and mrpt::math::CQuaternion< T >::z().
|
inline |
Return the conjugate quaternion.
Definition at line 374 of file CQuaternion.h.
References mrpt::math::CQuaternion< T >::r(), mrpt::math::CQuaternion< T >::x(), mrpt::math::CQuaternion< T >::y(), and mrpt::math::CQuaternion< T >::z().
Referenced by mrpt::poses::CPose3DQuatPDFGaussianInf::inverse(), and mrpt::poses::CPose3DQuatPDFGaussian::inverse().
|
inline |
Return the conjugate quaternion.
Definition at line 383 of file CQuaternion.h.
|
inline |
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).
Definition at line 202 of file CQuaternion.h.
References mrpt::math::CQuaternion< T >::normalize(), mrpt::math::CQuaternion< T >::r(), mrpt::math::CQuaternion< T >::x(), mrpt::math::CQuaternion< T >::y(), and mrpt::math::CQuaternion< T >::z().
Referenced by mrpt::poses::CPose3DQuat::composeFrom(), mrpt::poses::CPose3DQuat::inverseComposeFrom(), mrpt::poses::CPose3DPDF::jacobiansPoseComposition(), and TEST_F().
|
inlinestatic |
Exponential map from the SO(3) Lie Algebra to unit quaternions.
Definition at line 182 of file CQuaternion.h.
References mrpt::math::UNINITIALIZED_QUATERNION.
|
inlinestatic |
This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.
Definition at line 190 of file CQuaternion.h.
References mrpt::math::CQuaternion< T >::fromRodriguesVector().
|
inline |
Set this quaternion to the rotation described by a 3D (Rodrigues) rotation vector : If , then the quaternion is , otherwise:
where .
Definition at line 117 of file CQuaternion.h.
References THROW_EXCEPTION, mrpt::math::CQuaternion< T >::x(), mrpt::math::CQuaternion< T >::y(), and mrpt::math::CQuaternion< T >::z().
Referenced by mrpt::math::CQuaternion< T >::exp().
|
inline |
Rotate a 3D point (lx,ly,lz) -> (gx,gy,gz) as described by the inverse (conjugate) of this quaternion.
Definition at line 245 of file CQuaternion.h.
References mrpt::math::CQuaternion< T >::r(), mrpt::math::CQuaternion< T >::x(), mrpt::math::CQuaternion< T >::y(), and mrpt::math::CQuaternion< T >::z().
Referenced by mrpt::poses::CPose3DQuat::inverseComposePoint().
|
inline |
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 ;-).
[out] | out_ln | The target vector, which can be: std::vector<>, or mrpt::math::CVectorDouble or any row or column Eigen::Matrix<>. |
Definition at line 154 of file CQuaternion.h.
References mrpt::math::CQuaternion< T >::ln_noresize().
Referenced by QuaternionTests::test_ExpAndLnMatches(), and QuaternionTests::test_lnAndExpMatches().
|
inline |
overload that returns by value
Definition at line 161 of file CQuaternion.h.
|
inline |
Like ln() but does not try to resize the output vector.
Definition at line 169 of file CQuaternion.h.
References mrpt::math::CQuaternion< T >::r(), mrpt::math::square(), mrpt::mrpt::math::square(), mrpt::math::CQuaternion< T >::x(), mrpt::math::CQuaternion< T >::y(), and mrpt::math::CQuaternion< T >::z().
Referenced by mrpt::math::CQuaternion< T >::ln().
|
inline |
Calculate the 4x4 Jacobian of the normalization operation of this quaternion.
The output matrix can be a dynamic or fixed size (4x4) matrix.
Definition at line 283 of file CQuaternion.h.
References mrpt::math::CQuaternion< T >::normSqr(), mrpt::math::CQuaternion< T >::r(), mrpt::math::CQuaternion< T >::x(), mrpt::math::CQuaternion< T >::y(), and mrpt::math::CQuaternion< T >::z().
Referenced by mrpt::poses::CPose3DQuat::composePoint(), mrpt::poses::CPose3DPDFGaussian::copyFrom(), mrpt::poses::CPose3DQuat::inverseComposePoint(), mrpt::poses::CPose3DQuatPDF::jacobiansPoseComposition(), mrpt::poses::CPose3DPDF::jacobiansPoseComposition(), and Pose3DQuatTests::test_normalizeJacob().
|
inline |
Normalize this quaternion, so its norm becomes the unitity.
Definition at line 272 of file CQuaternion.h.
References mrpt::math::CQuaternion< T >::normSqr().
Referenced by mrpt::poses::CPose3DQuat::CPose3DQuat(), and mrpt::math::CQuaternion< T >::crossProduct().
|
inline |
Return the squared norm of the quaternion.
Definition at line 264 of file CQuaternion.h.
References mrpt::math::CQuaternion< T >::r(), mrpt::math::square(), mrpt::mrpt::math::square(), mrpt::math::CQuaternion< T >::x(), mrpt::math::CQuaternion< T >::y(), and mrpt::math::CQuaternion< T >::z().
Referenced by mrpt::math::CQuaternion< T >::CQuaternion(), mrpt::math::CQuaternion< T >::normalizationJacobian(), and mrpt::math::CQuaternion< T >::normalize().
|
inline |
Definition at line 493 of file CQuaternion.h.
|
inline |
Return r coordinate of the quaternion.
Definition at line 87 of file CQuaternion.h.
Referenced by mrpt::poses::CPose3DQuat::composePoint(), mrpt::math::CQuaternion< T >::conj(), mrpt::math::CQuaternion< T >::CQuaternion(), mrpt::math::CQuaternion< T >::crossProduct(), mrpt::hwdrivers::CImageGrabber_FlyCapture2::getObservation(), mrpt::utils::internal::getPoseFromString(), mrpt::poses::CPose3DQuat::inverseComposeFrom(), mrpt::poses::CPose3DQuat::inverseComposePoint(), mrpt::math::CQuaternion< T >::inverseRotatePoint(), mrpt::poses::CPose3DQuatPDF::jacobiansPoseComposition(), mrpt::math::CQuaternion< T >::ln_noresize(), mrpt::math::CQuaternion< T >::normalizationJacobian(), mrpt::math::CQuaternion< T >::normSqr(), mrpt::topography::path_from_rtk_gps(), mrpt::math::CQuaternion< T >::r(), mrpt::graphslam::CGraphSlamEngine< GRAPH_T >::readGTFileRGBD_TUM(), mrpt::math::CQuaternion< T >::rotatePoint(), mrpt::math::CQuaternion< T >::rotationJacobian(), mrpt::math::CQuaternion< T >::rotationMatrixNoResize(), mrpt::math::CQuaternion< T >::rpy_and_jacobian(), and Pose3DQuatTests::test_invComposePointJacob().
|
inline |
Set r coordinate of the quaternion.
Definition at line 95 of file CQuaternion.h.
References mrpt::math::CQuaternion< T >::r().
|
inline |
Rotate a 3D point (lx,ly,lz) -> (gx,gy,gz) as described by this quaternion.
Definition at line 224 of file CQuaternion.h.
References mrpt::math::CQuaternion< T >::r(), mrpt::math::CQuaternion< T >::x(), mrpt::math::CQuaternion< T >::y(), and mrpt::math::CQuaternion< T >::z().
Referenced by mrpt::poses::CPose3DQuat::composeFrom(), mrpt::poses::CPose3DQuat::composePoint(), and mrpt::poses::CPose3DQuat::inverseComposeFrom().
|
inline |
Compute the Jacobian of the rotation composition operation , that is the 4x4 matrix .
The output matrix can be a dynamic or fixed size (4x4) matrix.
Definition at line 315 of file CQuaternion.h.
References mrpt::math::CQuaternion< T >::r(), mrpt::math::CQuaternion< T >::x(), mrpt::math::CQuaternion< T >::y(), and mrpt::math::CQuaternion< T >::z().
|
inline |
Calculate the 3x3 rotation matrix associated to this quaternion:
.
Definition at line 350 of file CQuaternion.h.
References mrpt::math::CQuaternion< T >::rotationMatrixNoResize().
|
inline |
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).
Definition at line 360 of file CQuaternion.h.
References mrpt::math::CQuaternion< T >::r(), mrpt::math::CQuaternion< T >::x(), mrpt::math::CQuaternion< T >::y(), and mrpt::math::CQuaternion< T >::z().
Referenced by mrpt::poses::CPose3DQuat::getHomogeneousMatrix(), and mrpt::math::CQuaternion< T >::rotationMatrix().
|
inline |
Return the yaw, pitch & roll angles associated to quaternion.
Definition at line 395 of file CQuaternion.h.
References mrpt::obs::gnss::pitch, mrpt::obs::gnss::roll, and mrpt::math::CQuaternion< T >::rpy_and_jacobian().
Referenced by mrpt::utils::internal::getPoseFromString(), and mrpt::graphslam::CGraphSlamEngine< GRAPH_T >::readGTFileRGBD_TUM().
|
inline |
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.
Definition at line 411 of file CQuaternion.h.
References M_PI, mrpt::obs::gnss::pitch, mrpt::math::CQuaternion< T >::r(), mrpt::obs::gnss::roll, mrpt::mrpt::math::square(), mrpt::math::square(), mrpt::math::CQuaternion< T >::x(), mrpt::math::CQuaternion< T >::y(), and mrpt::math::CQuaternion< T >::z().
Referenced by mrpt::poses::CPose3DPDFGaussian::copyFrom(), mrpt::poses::CPose3DPDF::jacobiansPoseComposition(), and mrpt::math::CQuaternion< T >::rpy().
|
inline |
Return x coordinate of the quaternion.
Definition at line 89 of file CQuaternion.h.
Referenced by mrpt::poses::CPose3DQuat::composePoint(), mrpt::math::CQuaternion< T >::conj(), mrpt::math::CQuaternion< T >::CQuaternion(), mrpt::math::CQuaternion< T >::crossProduct(), mrpt::math::CQuaternion< T >::fromRodriguesVector(), mrpt::hwdrivers::CImageGrabber_FlyCapture2::getObservation(), mrpt::utils::internal::getPoseFromString(), mrpt::poses::CPose3DQuat::inverseComposeFrom(), mrpt::poses::CPose3DQuat::inverseComposePoint(), mrpt::math::CQuaternion< T >::inverseRotatePoint(), mrpt::poses::CPose3DQuatPDF::jacobiansPoseComposition(), mrpt::math::CQuaternion< T >::ln_noresize(), mrpt::math::CQuaternion< T >::normalizationJacobian(), mrpt::math::CQuaternion< T >::normSqr(), mrpt::topography::path_from_rtk_gps(), mrpt::graphslam::CGraphSlamEngine< GRAPH_T >::readGTFileRGBD_TUM(), mrpt::math::CQuaternion< T >::rotatePoint(), mrpt::math::CQuaternion< T >::rotationJacobian(), mrpt::math::CQuaternion< T >::rotationMatrixNoResize(), mrpt::math::CQuaternion< T >::rpy_and_jacobian(), Pose3DQuatTests::test_invComposePointJacob(), and mrpt::math::CQuaternion< T >::x().
|
inline |
Set x coordinate of the quaternion.
Definition at line 97 of file CQuaternion.h.
References mrpt::math::CQuaternion< T >::x().
|
inline |
Return y coordinate of the quaternion.
Definition at line 91 of file CQuaternion.h.
Referenced by mrpt::poses::CPose3DQuat::composePoint(), mrpt::math::CQuaternion< T >::conj(), mrpt::math::CQuaternion< T >::CQuaternion(), mrpt::math::CQuaternion< T >::crossProduct(), mrpt::math::CQuaternion< T >::fromRodriguesVector(), mrpt::hwdrivers::CImageGrabber_FlyCapture2::getObservation(), mrpt::utils::internal::getPoseFromString(), mrpt::poses::CPose3DQuat::inverseComposeFrom(), mrpt::poses::CPose3DQuat::inverseComposePoint(), mrpt::math::CQuaternion< T >::inverseRotatePoint(), mrpt::poses::CPose3DQuatPDF::jacobiansPoseComposition(), mrpt::math::CQuaternion< T >::ln_noresize(), mrpt::math::CQuaternion< T >::normalizationJacobian(), mrpt::math::CQuaternion< T >::normSqr(), mrpt::topography::path_from_rtk_gps(), mrpt::graphslam::CGraphSlamEngine< GRAPH_T >::readGTFileRGBD_TUM(), mrpt::math::CQuaternion< T >::rotatePoint(), mrpt::math::CQuaternion< T >::rotationJacobian(), mrpt::math::CQuaternion< T >::rotationMatrixNoResize(), mrpt::math::CQuaternion< T >::rpy_and_jacobian(), Pose3DQuatTests::test_invComposePointJacob(), and mrpt::math::CQuaternion< T >::y().
|
inline |
Set y coordinate of the quaternion.
Definition at line 99 of file CQuaternion.h.
References mrpt::math::CQuaternion< T >::y().
|
inline |
Return z coordinate of the quaternion.
Definition at line 93 of file CQuaternion.h.
Referenced by mrpt::poses::CPose3DQuat::composePoint(), mrpt::math::CQuaternion< T >::conj(), mrpt::math::CQuaternion< T >::CQuaternion(), mrpt::math::CQuaternion< T >::crossProduct(), mrpt::math::CQuaternion< T >::fromRodriguesVector(), mrpt::hwdrivers::CImageGrabber_FlyCapture2::getObservation(), mrpt::utils::internal::getPoseFromString(), mrpt::poses::CPose3DQuat::inverseComposeFrom(), mrpt::poses::CPose3DQuat::inverseComposePoint(), mrpt::math::CQuaternion< T >::inverseRotatePoint(), mrpt::poses::CPose3DQuatPDF::jacobiansPoseComposition(), mrpt::math::CQuaternion< T >::ln_noresize(), mrpt::math::CQuaternion< T >::normalizationJacobian(), mrpt::math::CQuaternion< T >::normSqr(), mrpt::topography::path_from_rtk_gps(), mrpt::graphslam::CGraphSlamEngine< GRAPH_T >::readGTFileRGBD_TUM(), mrpt::math::CQuaternion< T >::rotatePoint(), mrpt::math::CQuaternion< T >::rotationJacobian(), mrpt::math::CQuaternion< T >::rotationMatrixNoResize(), mrpt::math::CQuaternion< T >::rpy_and_jacobian(), Pose3DQuatTests::test_invComposePointJacob(), and mrpt::math::CQuaternion< T >::z().
|
inline |
Set z coordinate of the quaternion.
Definition at line 101 of file CQuaternion.h.
References mrpt::math::CQuaternion< T >::z().
Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019 |