Chiebot-Mirror
/
opencv
mirror of https://github.com/opencv/opencv.git
8
0
Fork 0
Code Issues Projects Releases Wiki Activity

Merge pull request #19098 from chargerKong:EulerAngle

Browse Source 
* add to/from Euler Angles

* restruct codes

* quat: optimize implementation

* cleanup debug code

* correct spelling errors

* create QuatEnum for enum EulerAnglesType

* use for loop for test_quaternion

* drop template from isIntAngleType & add minimal error information in test_quaternion.cpp

Co-authored-by: ShanChenqi <shanchenqi@huawei.com>
Co-authored-by: Alexander Alekhin <alexander.a.alekhin@gmail.com>
pull/19247/head
Liangqian 4 years ago committed by GitHub
parent bec7b297ed
commit e4c7fca755
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 439 additions and 5 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 191
      modules/core/include/opencv2/core/quaternion.hpp
  2. 191
      modules/core/include/opencv2/core/quaternion.inl.hpp
  3. 62
      modules/core/test/test_quaternion.cpp

191
modules/core/include/opencv2/core/quaternion.hpp
Unescape View File

@ -27,6 +27,7 @@
#define OPENCV_CORE_QUATERNION_HPP #define OPENCV_CORE_QUATERNION_HPP
#include <opencv2/core.hpp> #include <opencv2/core.hpp>
#include <opencv2/core/utils/logger.hpp>
#include <iostream> #include <iostream>
namespace cv namespace cv
{ {
@ -51,6 +52,83 @@ enum QuatAssumeType
QUAT_ASSUME_UNIT QUAT_ASSUME_UNIT
}; };
class QuatEnum
{
public:
/** @brief Enum of Euler angles type.
*
* Without considering the possibility of using two different convertions for the definition of the rotation axes ,
* there exists twelve possible sequences of rotation axes, divided into two groups:
* - Proper Euler angles (Z-X-Z, X-Y-X, Y-Z-Y, Z-Y-Z, X-Z-X, Y-X-Y)
* - Tait–Bryan angles (X-Y-Z, Y-Z-X, Z-X-Y, X-Z-Y, Z-Y-X, Y-X-Z).
*
* The three elemental rotations may be [extrinsic](https://en.wikipedia.org/wiki/Euler_angles#Definition_by_extrinsic_rotations)
* (rotations about the axes *xyz* of the original coordinate system, which is assumed to remain motionless),
* or [intrinsic](https://en.wikipedia.org/wiki/Euler_angles#Definition_by_intrinsic_rotations)(rotations about the axes of the rotating coordinate system *XYZ*, solidary with the moving body, which changes its orientation after each elemental rotation).
*
*
* Extrinsic and intrinsic rotations are relevant.
*
* The definition of the Euler angles is as following,
* - \f$\theta_1 \f$ represents the first rotation angle,
* - \f$\theta_2 \f$ represents the second rotation angle,
* - \f$\theta_3 \f$ represents the third rotation angle.
*
* For intrinsic rotations in the order of X-Y-Z, the rotation matrix R can be calculated by:\f[R =X(\theta_1) Y(\theta_2) Z(\theta_3) \f]
* For extrinsic rotations in the order of X-Y-Z, the rotation matrix R can be calculated by:\f[R =Z({\theta_3}) Y({\theta_2}) X({\theta_1})\f]
* where
* \f[X({\theta})={\begin{bmatrix}1&0&0\\0&\cos {\theta_1} &-\sin {\theta_1} \\0&\sin {\theta_1} &\cos {\theta_1} \\\end{bmatrix}},
* Y({\theta})={\begin{bmatrix}\cos \theta_{2}&0&\sin \theta_{2}\\0&1 &0 \\\ -sin \theta_2& 0&\cos \theta_{2} \\\end{bmatrix}},
* Z({\theta})={\begin{bmatrix}\cos\theta_{3} &-\sin \theta_3&0\\\sin \theta_3 &\cos \theta_3 &0\\0&0&1\\\end{bmatrix}}.
* \f]
*
* The function is designed according to this set of conventions:
* - [Right handed](https://en.wikipedia.org/wiki/Right_hand_rule) reference frames are adopted, and the [right hand rule](https://en.wikipedia.org/wiki/Right_hand_rule) is used to determine the sign of angles.
* - Each matrix is meant to represent an [active rotation](https://en.wikipedia.org/wiki/Active_and_passive_transformation) (the composing and composed matrices
* are supposed to act on the coordinates of vectors defined in the initial fixed reference frame and give as a result the coordinates of a rotated vector defined in the same reference frame).
* - For \f$\theta_1\f$ and \f$\theta_3\f$, the valid range is (−π, π].
*
* For \f$\theta_2\f$, the valid range is [−π/2, π/2] or [0, π].
*
* For Tait–Bryan angles, the valid range of \f$\theta_2\f$ is [−π/2, π/2]. When transforming a quaternion to Euler angles, the solution of Euler angles is unique in condition of \f$ \theta_2 \in (−π/2, π/2)\f$ .
* If \f$\theta_2 = −π/2 \f$ or \f$ \theta_2 = π/2\f$, there are infinite solutions. The common name for this situation is gimbal lock.
* For Proper Euler angles,the valid range of \f$\theta_2\f$ is in [0, π]. The solutions of Euler angles are unique in condition of \f$ \theta_2 \in (0, π)\f$ . If \f$\theta_2 =0 \f$ or \f$\theta_2 =π \f$,
* there are infinite solutions and gimbal lock will occur.
*/
enum EulerAnglesType
{
INT_XYZ, ///< Intrinsic rotations with the Euler angles type X-Y-Z
INT_XZY, ///< Intrinsic rotations with the Euler angles type X-Z-Y
INT_YXZ, ///< Intrinsic rotations with the Euler angles type Y-X-Z
INT_YZX, ///< Intrinsic rotations with the Euler angles type Y-Z-X
INT_ZXY, ///< Intrinsic rotations with the Euler angles type Z-X-Y
INT_ZYX, ///< Intrinsic rotations with the Euler angles type Z-Y-X
INT_XYX, ///< Intrinsic rotations with the Euler angles type X-Y-X
INT_XZX, ///< Intrinsic rotations with the Euler angles type X-Z-X
INT_YXY, ///< Intrinsic rotations with the Euler angles type Y-X-Y
INT_YZY, ///< Intrinsic rotations with the Euler angles type Y-Z-Y
INT_ZXZ, ///< Intrinsic rotations with the Euler angles type Z-X-Z
INT_ZYZ, ///< Intrinsic rotations with the Euler angles type Z-Y-Z
EXT_XYZ, ///< Extrinsic rotations with the Euler angles type X-Y-Z
EXT_XZY, ///< Extrinsic rotations with the Euler angles type X-Z-Y
EXT_YXZ, ///< Extrinsic rotations with the Euler angles type Y-X-Z
EXT_YZX, ///< Extrinsic rotations with the Euler angles type Y-Z-X
EXT_ZXY, ///< Extrinsic rotations with the Euler angles type Z-X-Y
EXT_ZYX, ///< Extrinsic rotations with the Euler angles type Z-Y-X
EXT_XYX, ///< Extrinsic rotations with the Euler angles type X-Y-X
EXT_XZX, ///< Extrinsic rotations with the Euler angles type X-Z-X
EXT_YXY, ///< Extrinsic rotations with the Euler angles type Y-X-Y
EXT_YZY, ///< Extrinsic rotations with the Euler angles type Y-Z-Y
EXT_ZXZ, ///< Extrinsic rotations with the Euler angles type Z-X-Z
EXT_ZYZ, ///< Extrinsic rotations with the Euler angles type Z-Y-Z
#ifndef CV_DOXYGEN
EULER_ANGLES_MAX_VALUE
#endif
};
};
template <typename _Tp> class Quat; template <typename _Tp> class Quat;
template <typename _Tp> std::ostream& operator<<(std::ostream&, const Quat<_Tp>&); template <typename _Tp> std::ostream& operator<<(std::ostream&, const Quat<_Tp>&);
@ -133,9 +211,9 @@ class Quat
{ {
static_assert(std::is_floating_point<_Tp>::value, "Quaternion only make sense with type of float or double"); static_assert(std::is_floating_point<_Tp>::value, "Quaternion only make sense with type of float or double");
using value_type = _Tp; using value_type = _Tp;
public: public:
static constexpr _Tp CV_QUAT_EPS = (_Tp)1.e-6; static constexpr _Tp CV_QUAT_EPS = (_Tp)1.e-6;
static constexpr _Tp CV_QUAT_CONVERT_THRESHOLD = (_Tp)1.e-6;
Quat(); Quat();
@ -182,6 +260,41 @@ public:
*/ */
static Quat<_Tp> createFromRvec(InputArray rvec); static Quat<_Tp> createFromRvec(InputArray rvec);
/**
* @brief
* from Euler angles
*
* A quaternion can be generated from Euler angles by combining the quaternion representations of the Euler rotations.
*
* For example, if we use intrinsic rotations in the order of X-Y-Z,\f$\theta_1 \f$ is rotation around the X-axis, \f$\theta_2 \f$ is rotation around the Y-axis,
* \f$\theta_3 \f$ is rotation around the Z-axis. The final quaternion q can be calculated by
*
* \f[ {q} = q_{X, \theta_1} q_{Y, \theta_2} q_{Z, \theta_3}\f]
* where \f$ q_{X, \theta_1} \f$ is created from @ref createFromXRot, \f$ q_{Y, \theta_2} \f$ is created from @ref createFromYRot,
* \f$ q_{Z, \theta_3} \f$ is created from @ref createFromZRot.
* @param angles the Euler angles in a vector of length 3
* @param eulerAnglesType the convertion Euler angles type
*/
static Quat<_Tp> createFromEulerAngles(const Vec<_Tp, 3> &angles, QuatEnum::EulerAnglesType eulerAnglesType);
/**
* @brief get a quaternion from a rotation about the Y-axis by \f$\theta\f$ .
* \f[q = \cos(\theta/2)+0 i+ sin(\theta/2) j +0k \f]
*/
static Quat<_Tp> createFromYRot(const _Tp theta);
/**
* @brief get a quaternion from a rotation about the X-axis by \f$\theta\f$ .
* \f[q = \cos(\theta/2)+sin(\theta/2) i +0 j +0 k \f]
*/
static Quat<_Tp> createFromXRot(const _Tp theta);
/**
* @brief get a quaternion from a rotation about the Z-axis by \f$\theta\f$.
* \f[q = \cos(\theta/2)+0 i +0 j +sin(\theta/2) k \f]
*/
static Quat<_Tp> createFromZRot(const _Tp theta);
/** /**
* @brief a way to get element. * @brief a way to get element.
* @param index over a range [0, 3]. * @param index over a range [0, 3].
@ -811,8 +924,8 @@ public:
/** /**
* @brief transform a quaternion to a 3x3 rotation matrix. * @brief transform a quaternion to a 3x3 rotation matrix.
* @param assumeUnit if QUAT_ASSUME_UNIT, this quaternion assume to be a unit quaternion and * @param assumeUnit if QUAT_ASSUME_UNIT, this quaternion assume to be a unit quaternion and
* this function will save some computations. Otherwise, this function will normalized this * this function will save some computations. Otherwise, this function will normalize this
* quaternion at first then to do the transformation. * quaternion at first then do the transformation.
* *
* @note Matrix A which is to be rotated should have the form * @note Matrix A which is to be rotated should have the form
* \f[\begin{bmatrix} * \f[\begin{bmatrix}
@ -845,8 +958,8 @@ public:
/** /**
* @brief transform a quaternion to a 4x4 rotation matrix. * @brief transform a quaternion to a 4x4 rotation matrix.
* @param assumeUnit if QUAT_ASSUME_UNIT, this quaternion assume to be a unit quaternion and * @param assumeUnit if QUAT_ASSUME_UNIT, this quaternion assume to be a unit quaternion and
* this function will save some computations. Otherwise, this function will normalized this * this function will save some computations. Otherwise, this function will normalize this
* quaternion at first then to do the transformation. * quaternion at first then do the transformation.
* *
* The operations is similar as toRotMat3x3 * The operations is similar as toRotMat3x3
* except that the points matrix should have the form * except that the points matrix should have the form
@ -1434,6 +1547,74 @@ public:
template <typename S> template <typename S>
friend std::ostream& cv::operator<<(std::ostream&, const Quat<S>&); friend std::ostream& cv::operator<<(std::ostream&, const Quat<S>&);
/**
* @brief Transform a quaternion q to Euler angles.
*
*
* When transforming a quaternion \f$q = w + x\boldsymbol{i} + y\boldsymbol{j} + z\boldsymbol{k}\f$ to Euler angles, rotation matrix M can be calculated by:
* \f[ \begin{aligned} {M} &={\begin{bmatrix}1-2(y^{2}+z^{2})&2(xy-zx)&2(xz+yw)\\2(xy+zw)&1-2(x^{2}+z^{2})&2(yz-xw)\\2(xz-yw)&2(yz+xw)&1-2(x^{2}+y^{2})\end{bmatrix}}\end{aligned}.\f]
* On the other hand, the rotation matrix can be obtained from Euler angles.
* Using intrinsic rotations with Euler angles type XYZ as an example,
* \f$\theta_1 \f$, \f$\theta_2 \f$, \f$\theta_3 \f$ are three angles for Euler angles, the rotation matrix R can be calculated by:\f[R =X(\theta_1)Y(\theta_2)Z(\theta_3)
* ={\begin{bmatrix}\cos\theta_{2}\cos\theta_{3}&-\cos\theta_{2}\sin\theta_{3}&\sin\theta_{2}\\\cos\theta_{1}\sin\theta_{3}+\cos\theta_{3}\sin\theta_{1}\sin\theta_{2}&\cos\theta_{1}\cos\theta_{3}-\sin\theta_{1}\sin\theta_{2}\sin\theta_{3}&-\cos\theta_{2}\sin\theta_{1}\\\sin\theta_{1}\sin\theta_{3}-\cos\theta_{1}\cos\theta_{3}\sin\theta_{2}&\cos\theta_{3}\sin\theta_{1}+\cos\theta_{1}\sin\theta_{2}\sin\theta_{3}&\cos\theta_{1}\cos_{2}\end{bmatrix}}\f]
* Rotation matrix M and R are equal. As long as \f$ s_{2} \neq 1 \f$, by comparing each element of two matrices ,the solution is\f$\begin{cases} \theta_1 = \arctan2(-m_{23},m_{33})\\\theta_2 = arcsin(m_{13}) \\\theta_3 = \arctan2(-m_{12},m_{11}) \end{cases}\f$.
*
* When \f$ s_{2}=1\f$ or \f$ s_{2}=-1\f$, the gimbal lock occurs. The function will prompt "WARNING: Gimbal Lock will occur. Euler angles is non-unique. For intrinsic rotations, we set the third angle to 0, and for external rotation, we set the first angle to 0.".
*
* When \f$ s_{2}=1\f$ ,
* The rotation matrix R is \f$R = {\begin{bmatrix}0&0&1\\\sin(\theta_1+\theta_3)&\cos(\theta_1+\theta_3)&0\\-\cos(\theta_1+\theta_3)&\sin(\theta_1+\theta_3)&0\end{bmatrix}}\f$.
*
* The number of solutions is infinite with the condition \f$\begin{cases} \theta_1+\theta_3 = \arctan2(m_{21},m_{22})\\ \theta_2=\pi/2 \end{cases}\ \f$.
*
* We set \f$ \theta_3 = 0\f$, the solution is \f$\begin{cases} \theta_1=\arctan2(m_{21},m_{22})\\ \theta_2=\pi/2\\ \theta_3=0 \end{cases}\f$.
*
* When \f$ s_{2}=-1\f$,
* The rotation matrix R is \f$X_{1}Y_{2}Z_{3}={\begin{bmatrix}0&0&-1\\-\sin(\theta_1-\theta_3)&\cos(\theta_1-\theta_3)&0\\\cos(\theta_1-\theta_3)&\sin(\theta_1-\theta_3)&0\end{bmatrix}}\f$.
*
* The number of solutions is infinite with the condition \f$\begin{cases} \theta_1+\theta_3 = \arctan2(m_{32},m_{22})\\ \theta_2=\pi/2 \end{cases}\ \f$.
*
* We set \f$ \theta_3 = 0\f$, the solution is \f$ \begin{cases}\theta_1=\arctan2(m_{32},m_{22}) \\ \theta_2=-\pi/2\\ \theta_3=0\end{cases}\f$.
*
* Since \f$ sin \theta\in [-1,1] \f$ and \f$ cos \theta \in [-1,1] \f$, the unnormalized quaternion will cause computational troubles. For this reason, this function will normalize the quaternion at first and @ref QuatAssumeType is not needed.
*
* When the gimbal lock occurs, we set \f$\theta_3 = 0\f$ for intrinsic rotations or \f$\theta_1 = 0\f$ for extrinsic rotations.
*
* As a result, for every Euler angles type, we can get solution as shown in the following table.
* EulerAnglesType | Ordinary | \f$\theta_2 = π/2\f$ | \f$\theta_2 = -π/2\f$
* ------------- | -------------| -------------| -------------
* INT_XYZ|\f$ \theta_1 = \arctan2(-m_{23},m_{33})\\\theta_2 = \arcsin(m_{13}) \\\theta_3= \arctan2(-m_{12},m_{11}) \f$|\f$ \theta_1=\arctan2(m_{21},m_{22})\\ \theta_2=\pi/2\\ \theta_3=0 \f$|\f$ \theta_1=\arctan2(m_{32},m_{22})\\ \theta_2=-\pi/2\\ \theta_3=0 \f$
* INT_XZY|\f$ \theta_1 = \arctan2(m_{32},m_{22})\\\theta_2 = -\arcsin(m_{12}) \\\theta_3= \arctan2(m_{13},m_{11}) \f$|\f$ \theta_1=\arctan2(m_{31},m_{33})\\ \theta_2=\pi/2\\ \theta_3=0 \f$|\f$ \theta_1=\arctan2(-m_{23},m_{33})\\ \theta_2=-\pi/2\\ \theta_3=0 \f$
* INT_YXZ|\f$ \theta_1 = \arctan2(m_{13},m_{33})\\\theta_2 = -\arcsin(m_{23}) \\\theta_3= \arctan2(m_{21},m_{22}) \f$|\f$ \theta_1=\arctan2(m_{12},m_{11})\\ \theta_2=\pi/2\\ \theta_3=0 \f$|\f$ \theta_1=\arctan2(-m_{12},m_{11})\\ \theta_2=-\pi/2\\ \theta_3=0 \f$
* INT_YZX|\f$ \theta_1 = \arctan2(-m_{31},m_{11})\\\theta_2 = \arcsin(m_{21}) \\\theta_3= \arctan2(-m_{23},m_{22}) \f$|\f$ \theta_1=\arctan2(m_{13},m_{33})\\ \theta_2=\pi/2\\ \theta_3=0 \f$|\f$ \theta_1=\arctan2(m_{13},m_{12})\\ \theta_2=-\pi/2\\ \theta_3=0 \f$
* INT_ZXY|\f$ \theta_1 = \arctan2(-m_{12},m_{22})\\\theta_2 = \arcsin(m_{32}) \\\theta_3= \arctan2(-m_{31},m_{33}) \f$|\f$ \theta_1=\arctan2(m_{21},m_{11})\\ \theta_2=\pi/2\\ \theta_3=0 \f$|\f$ \theta_1=\arctan2(m_{21},m_{11})\\ \theta_2=-\pi/2\\ \theta_3=0 \f$
* INT_ZYX|\f$ \theta_1 = \arctan2(m_{21},m_{11})\\\theta_2 = \arcsin(-m_{31}) \\\theta_3= \arctan2(m_{32},m_{33}) \f$|\f$ \theta_1=\arctan2(m_{23},m_{22})\\ \theta_2=\pi/2\\ \theta_3=0 \f$|\f$ \theta_1=\arctan2(-m_{12},m_{22})\\ \theta_2=-\pi/2\\ \theta_3=0 \f$
* EXT_XYZ|\f$ \theta_1 = \arctan2(m_{32},m_{33})\\\theta_2 = \arcsin(-m_{31}) \\\ \theta_3 = \arctan2(m_{21},m_{11})\f$|\f$ \theta_1= 0\\ \theta_2=\pi/2\\ \theta_3=\arctan2(m_{23},m_{22}) \f$|\f$ \theta_1=0\\ \theta_2=-\pi/2\\ \theta_3=\arctan2(-m_{12},m_{22}) \f$
* EXT_XZY|\f$ \theta_1 = \arctan2(-m_{23},m_{22})\\\theta_2 = \arcsin(m_{21}) \\\theta_3= \arctan2(-m_{31},m_{11})\f$|\f$ \theta_1= 0\\ \theta_2=\pi/2\\ \theta_3=\arctan2(m_{13},m_{33}) \f$|\f$ \theta_1=0\\ \theta_2=-\pi/2\\ \theta_3=\arctan2(m_{13},m_{12}) \f$
* EXT_YXZ|\f$ \theta_1 = \arctan2(-m_{31},m_{33}) \\\theta_2 = \arcsin(m_{32}) \\\theta_3= \arctan2(-m_{12},m_{22})\f$|\f$ \theta_1= 0\\ \theta_2=\pi/2\\ \theta_3=\arctan2(m_{21},m_{11}) \f$|\f$ \theta_1=0\\ \theta_2=-\pi/2\\ \theta_3=\arctan2(m_{21},m_{11}) \f$
* EXT_YZX|\f$ \theta_1 = \arctan2(m_{13},m_{11})\\\theta_2 = -\arcsin(m_{12}) \\\theta_3= \arctan2(m_{32},m_{22})\f$|\f$ \theta_1= 0\\ \theta_2=\pi/2\\ \theta_3=\arctan2(m_{31},m_{33}) \f$|\f$ \theta_1=0\\ \theta_2=-\pi/2\\ \theta_3=\arctan2(-m_{23},m_{33}) \f$
* EXT_ZXY|\f$ \theta_1 = \arctan2(m_{21},m_{22})\\\theta_2 = -\arcsin(m_{23}) \\\theta_3= \arctan2(m_{13},m_{33})\f$|\f$ \theta_1= 0\\ \theta_2=\pi/2\\ \theta_3=\arctan2(m_{12},m_{11}) \f$|\f$ \theta_1= 0\\ \theta_2=-\pi/2\\ \theta_3=\arctan2(-m_{12},m_{11}) \f$
* EXT_ZYX|\f$ \theta_1 = \arctan2(-m_{12},m_{11})\\\theta_2 = \arcsin(m_{13}) \\\theta_3= \arctan2(-m_{23},m_{33})\f$|\f$ \theta_1=0\\ \theta_2=\pi/2\\ \theta_3=\arctan2(m_{21},m_{22}) \f$|\f$ \theta_1=0\\ \theta_2=-\pi/2\\ \theta_3=\arctan2(m_{32},m_{22}) \f$
*
* EulerAnglesType | Ordinary | \f$\theta_2 = 0\f$ | \f$\theta_2 = π\f$
* ------------- | -------------| -------------| -------------
* INT_XYX| \f$ \theta_1 = \arctan2(m_{21},-m_{31})\\\theta_2 =\arccos(m_{11}) \\\theta_3 = \arctan2(m_{12},m_{13}) \f$| \f$ \theta_1=\arctan2(m_{32},m_{33})\\ \theta_2=0\\ \theta_3=0 \f$| \f$ \theta_1=\arctan2(m_{23},m_{22})\\ \theta_2=\pi\\ \theta_3=0 \f$
* INT_XZX| \f$ \theta_1 = \arctan2(m_{31},m_{21})\\\theta_2 = \arccos(m_{11}) \\\theta_3 = \arctan2(m_{13},-m_{12}) \f$| \f$ \theta_1=\arctan2(m_{32},m_{33})\\ \theta_2=0\\ \theta_3=0 \f$| \f$ \theta_1=\arctan2(-m_{32},m_{33})\\ \theta_2=\pi\\ \theta_3=0 \f$
* INT_YXY| \f$ \theta_1 = \arctan2(m_{12},m_{32})\\\theta_2 = \arccos(m_{22}) \\\theta_3 = \arctan2(m_{21},-m_{23}) \f$| \f$ \theta_1=\arctan2(m_{13},m_{11})\\ \theta_2=0\\ \theta_3=0 \f$| \f$ \theta_1=\arctan2(-m_{31},m_{11})\\ \theta_2=\pi\\ \theta_3=0 \f$
* INT_YZY| \f$ \theta_1 = \arctan2(m_{32},-m_{12})\\\theta_2 = \arccos(m_{22}) \\\theta_3 =\arctan2(m_{23},m_{21}) \f$| \f$ \theta_1=\arctan2(m_{13},m_{11})\\ \theta_2=0\\ \theta_3=0 \f$| \f$ \theta_1=\arctan2(m_{13},-m_{11})\\ \theta_2=\pi\\ \theta_3=0 \f$
* INT_ZXZ| \f$ \theta_1 = \arctan2(-m_{13},m_{23})\\\theta_2 = \arccos(m_{33}) \\\theta_3 =\arctan2(m_{31},m_{32}) \f$| \f$ \theta_1=\arctan2(m_{21},m_{22})\\ \theta_2=0\\ \theta_3=0 \f$| \f$ \theta_1=\arctan2(m_{21},m_{11})\\ \theta_2=\pi\\ \theta_3=0 \f$
* INT_ZYZ| \f$ \theta_1 = \arctan2(m_{23},m_{13})\\\theta_2 = \arccos(m_{33}) \\\theta_3 = \arctan2(m_{32},-m_{31}) \f$| \f$ \theta_1=\arctan2(m_{21},m_{11})\\ \theta_2=0\\ \theta_3=0 \f$| \f$ \theta_1=\arctan2(m_{21},m_{11})\\ \theta_2=\pi\\ \theta_3=0 \f$
* EXT_XYX| \f$ \theta_1 = \arctan2(m_{12},m_{13}) \\\theta_2 = \arccos(m_{11}) \\\theta_3 = \arctan2(m_{21},-m_{31})\f$| \f$ \theta_1=0\\ \theta_2=0\\ \theta_3=\arctan2(m_{32},m_{33}) \f$| \f$ \theta_1= 0\\ \theta_2=\pi\\ \theta_3= \arctan2(m_{23},m_{22}) \f$
* EXT_XZX| \f$ \theta_1 = \arctan2(m_{13},-m_{12})\\\theta_2 = \arccos(m_{11}) \\\theta_3 = \arctan2(m_{31},m_{21})\f$| \f$ \theta_1= 0\\ \theta_2=0\\ \theta_3=\arctan2(m_{32},m_{33}) \f$| \f$ \theta_1= 0\\ \theta_2=\pi\\ \theta_3=\arctan2(-m_{32},m_{33}) \f$
* EXT_YXY| \f$ \theta_1 = \arctan2(m_{21},-m_{23})\\\theta_2 = \arccos(m_{22}) \\\theta_3 = \arctan2(m_{12},m_{32}) \f$| \f$ \theta_1= 0\\ \theta_2=0\\ \theta_3=\arctan2(m_{13},m_{11}) \f$| \f$ \theta_1= 0\\ \theta_2=\pi\\ \theta_3=\arctan2(-m_{31},m_{11}) \f$
* EXT_YZY| \f$ \theta_1 = \arctan2(m_{23},m_{21}) \\\theta_2 = \arccos(m_{22}) \\\theta_3 = \arctan2(m_{32},-m_{12}) \f$| \f$ \theta_1= 0\\ \theta_2=0\\ \theta_3=\arctan2(m_{13},m_{11}) \f$| \f$ \theta_1=0\\ \theta_2=\pi\\ \theta_3=\arctan2(m_{13},-m_{11}) \f$
* EXT_ZXZ| \f$ \theta_1 = \arctan2(m_{31},m_{32}) \\\theta_2 = \arccos(m_{33}) \\\theta_3 = \arctan2(-m_{13},m_{23})\f$| \f$ \theta_1=0\\ \theta_2=0\\ \theta_3=\arctan2(m_{21},m_{22}) \f$| \f$ \theta_1= 0\\ \theta_2=\pi\\ \theta_3=\arctan2(m_{21},m_{11}) \f$
* EXT_ZYZ| \f$ \theta_1 = \arctan2(m_{32},-m_{31})\\\theta_2 = \arccos(m_{33}) \\\theta_3 = \arctan2(m_{23},m_{13}) \f$| \f$ \theta_1=0\\ \theta_2=0\\ \theta_3=\arctan2(m_{21},m_{11}) \f$| \f$ \theta_1= 0\\ \theta_2=\pi\\ \theta_3=\arctan2(m_{21},m_{11}) \f$
*
* @param eulerAnglesType the convertion Euler angles type
*/
Vec<_Tp, 3> toEulerAngles(QuatEnum::EulerAnglesType eulerAnglesType);
_Tp w, x, y, z; _Tp w, x, y, z;
}; };

191
modules/core/include/opencv2/core/quaternion.inl.hpp
Unescape View File

@ -866,6 +866,197 @@ Quat<T> Quat<T>::spline(const Quat<T> &q0, const Quat<T> &q1, const Quat<T> &q2,
return squad(vec[1], s1, s2, vec[2], t, assumeUnit, QUAT_ASSUME_NOT_UNIT); return squad(vec[1], s1, s2, vec[2], t, assumeUnit, QUAT_ASSUME_NOT_UNIT);
} }
namespace detail {
template <typename T> static
Quat<T> createFromAxisRot(int axis, const T theta)
{
if (axis == 0)
return Quat<T>::createFromXRot(theta);
if (axis == 1)
return Quat<T>::createFromYRot(theta);
if (axis == 2)
return Quat<T>::createFromZRot(theta);
CV_Assert(0);
}
static bool isIntAngleType(QuatEnum::EulerAnglesType eulerAnglesType)
{
return eulerAnglesType < QuatEnum::EXT_XYZ;
}
inline bool isTaitBryan(QuatEnum::EulerAnglesType eulerAnglesType)
{
return eulerAnglesType/6 == 1 || eulerAnglesType/6 == 3;
}
} // namespace detail
template <typename T>
Quat<T> Quat<T>::createFromYRot(const T theta)
{
return Quat<T>{std::cos(theta * 0.5f), 0, std::sin(theta * 0.5f), 0};
}
template <typename T>
Quat<T> Quat<T>::createFromXRot(const T theta){
return Quat<T>{std::cos(theta * 0.5f), std::sin(theta * 0.5f), 0, 0};
}
template <typename T>
Quat<T> Quat<T>::createFromZRot(const T theta){
return Quat<T>{std::cos(theta * 0.5f), 0, 0, std::sin(theta * 0.5f)};
}
template <typename T>
Quat<T> Quat<T>::createFromEulerAngles(const Vec<T, 3> &angles, QuatEnum::EulerAnglesType eulerAnglesType) {
CV_Assert(eulerAnglesType < QuatEnum::EulerAnglesType::EULER_ANGLES_MAX_VALUE);
static const int rotationAxis[24][3] = {
{0, 1, 2}, ///< Intrinsic rotations with the Euler angles type X-Y-Z
{0, 2, 1}, ///< Intrinsic rotations with the Euler angles type X-Z-Y
{1, 0, 2}, ///< Intrinsic rotations with the Euler angles type Y-X-Z
{1, 2, 0}, ///< Intrinsic rotations with the Euler angles type Y-Z-X
{2, 0, 1}, ///< Intrinsic rotations with the Euler angles type Z-X-Y
{2, 1, 0}, ///< Intrinsic rotations with the Euler angles type Z-Y-X
{0, 1, 0}, ///< Intrinsic rotations with the Euler angles type X-Y-X
{0, 2, 0}, ///< Intrinsic rotations with the Euler angles type X-Z-X
{1, 0, 1}, ///< Intrinsic rotations with the Euler angles type Y-X-Y
{1, 2, 1}, ///< Intrinsic rotations with the Euler angles type Y-Z-Y
{2, 0, 2}, ///< Intrinsic rotations with the Euler angles type Z-X-Z
{2, 1, 2}, ///< Intrinsic rotations with the Euler angles type Z-Y-Z
{0, 1, 2}, ///< Extrinsic rotations with the Euler angles type X-Y-Z
{0, 2, 1}, ///< Extrinsic rotations with the Euler angles type X-Z-Y
{1, 0, 2}, ///< Extrinsic rotations with the Euler angles type Y-X-Z
{1, 2, 0}, ///< Extrinsic rotations with the Euler angles type Y-Z-X
{2, 0, 1}, ///< Extrinsic rotations with the Euler angles type Z-X-Y
{2, 1, 0}, ///< Extrinsic rotations with the Euler angles type Z-Y-X
{0, 1, 0}, ///< Extrinsic rotations with the Euler angles type X-Y-X
{0, 2, 0}, ///< Extrinsic rotations with the Euler angles type X-Z-X
{1, 0, 1}, ///< Extrinsic rotations with the Euler angles type Y-X-Y
{1, 2, 1}, ///< Extrinsic rotations with the Euler angles type Y-Z-Y
{2, 0, 2}, ///< Extrinsic rotations with the Euler angles type Z-X-Z
{2, 1, 2} ///< Extrinsic rotations with the Euler angles type Z-Y-Z
};
Quat<T> q1 = detail::createFromAxisRot(rotationAxis[eulerAnglesType][0], angles(0));
Quat<T> q2 = detail::createFromAxisRot(rotationAxis[eulerAnglesType][1], angles(1));
Quat<T> q3 = detail::createFromAxisRot(rotationAxis[eulerAnglesType][2], angles(2));
if (detail::isIntAngleType(eulerAnglesType))
{
return q1 * q2 * q3;
}
else // (!detail::isIntAngleType<T>(eulerAnglesType))
{
return q3 * q2 * q1;
}
}
template <typename T>
Vec<T, 3> Quat<T>::toEulerAngles(QuatEnum::EulerAnglesType eulerAnglesType){
CV_Assert(eulerAnglesType < QuatEnum::EulerAnglesType::EULER_ANGLES_MAX_VALUE);
Matx33d R = toRotMat3x3();
enum {
C_ZERO,
C_PI,
C_PI_2,
N_CONSTANTS,
R_0_0 = N_CONSTANTS, R_0_1, R_0_2,
R_1_0, R_1_1, R_1_2,
R_2_0, R_2_1, R_2_2
};
static const T constants_[N_CONSTANTS] = {
0, // C_ZERO
(T)CV_PI, // C_PI
(T)(CV_PI * 0.5) // C_PI_2, -C_PI_2
};
static const int rotationR_[24][12] = {
{+R_0_2, +R_1_0, +R_1_1, C_PI_2, +R_2_1, +R_1_1, -C_PI_2, -R_1_2, +R_2_2, +R_0_2, -R_0_1, +R_0_0}, // INT_XYZ
{+R_0_1, -R_1_2, +R_2_2, -C_PI_2, +R_2_0, +R_2_2, C_PI_2, +R_2_1, +R_1_1, -R_0_1, +R_0_2, +R_0_0}, // INT_XZY
{+R_1_2, -R_0_1, +R_0_0, -C_PI_2, +R_0_1, +R_0_0, C_PI_2, +R_0_2, +R_2_2, -R_1_2, +R_1_0, +R_1_1}, // INT_YXZ
{+R_1_0, +R_0_2, +R_2_2, C_PI_2, +R_0_2, +R_0_1, -C_PI_2, -R_2_0, +R_0_0, +R_1_0, -R_1_2, +R_1_1}, // INT_YZX
{+R_2_1, +R_1_0, +R_0_0, C_PI_2, +R_1_0, +R_0_0, -C_PI_2, -R_0_1, +R_1_1, +R_2_1, -R_2_0, +R_2_2}, // INT_ZXY
{+R_2_0, -R_0_1, +R_1_1, -C_PI_2, +R_1_2, +R_1_1, C_PI_2, +R_1_0, +R_0_0, -R_2_0, +R_2_1, +R_2_2}, // INT_ZYX
{+R_0_0, +R_2_1, +R_2_2, C_ZERO, +R_1_2, +R_1_1, C_PI, +R_1_0, -R_2_0, +R_0_0, +R_0_1, +R_0_2}, // INT_XYX
{+R_0_0, +R_2_1, +R_2_2, C_ZERO, -R_2_1, +R_2_2, C_PI, +R_2_0, +R_1_0, +R_0_0, +R_0_2, -R_0_1}, // INT_XZX
{+R_1_1, +R_0_2, +R_0_0, C_ZERO, -R_2_0, +R_0_0, C_PI, +R_0_1, +R_2_1, +R_1_1, +R_1_0, -R_1_2}, // INT_YXY
{+R_1_1, +R_0_2, +R_0_0, C_ZERO, +R_0_2, -R_0_0, C_PI, +R_2_1, -R_0_1, +R_1_1, +R_1_2, +R_1_0}, // INT_YZY
{+R_2_2, +R_1_0, +R_1_1, C_ZERO, +R_1_0, +R_0_0, C_PI, +R_0_2, -R_1_2, +R_2_2, +R_2_0, +R_2_1}, // INT_ZXZ
{+R_2_2, +R_1_0, +R_0_0, C_ZERO, +R_1_0, +R_0_0, C_PI, +R_1_2, +R_0_2, +R_2_2, +R_2_1, -R_2_0}, // INT_ZYZ
{+R_2_0, -C_PI_2, -R_0_1, +R_1_1, C_PI_2, +R_1_2, +R_1_1, +R_2_1, +R_2_2, -R_2_0, +R_1_0, +R_0_0}, // EXT_XYZ
{+R_1_0, C_PI_2, +R_0_2, +R_2_2, -C_PI_2, +R_0_2, +R_0_1, -R_1_2, +R_1_1, +R_1_0, -R_2_0, +R_0_0}, // EXT_XZY
{+R_2_1, C_PI_2, +R_1_0, +R_0_0, -C_PI_2, +R_1_0, +R_0_0, -R_2_0, +R_2_2, +R_2_1, -R_0_1, +R_1_1}, // EXT_YXZ
{+R_0_2, -C_PI_2, -R_1_2, +R_2_2, C_PI_2, +R_2_0, +R_2_2, +R_0_2, +R_0_0, -R_0_1, +R_2_1, +R_1_1}, // EXT_YZX
{+R_1_2, -C_PI_2, -R_0_1, +R_0_0, C_PI_2, +R_0_1, +R_0_0, +R_1_0, +R_1_1, -R_1_2, +R_0_2, +R_2_2}, // EXT_ZXY
{+R_0_2, C_PI_2, +R_1_0, +R_1_1, -C_PI_2, +R_2_1, +R_1_1, -R_0_1, +R_0_0, +R_0_2, -R_1_2, +R_2_2}, // EXT_ZYX
{+R_0_0, C_ZERO, +R_2_1, +R_2_2, C_PI, +R_1_2, +R_1_1, +R_0_1, +R_0_2, +R_0_0, +R_1_0, -R_2_0}, // EXT_XYX
{+R_0_0, C_ZERO, +R_2_1, +R_2_2, C_PI, +R_2_1, +R_2_2, +R_0_2, -R_0_1, +R_0_0, +R_2_0, +R_1_0}, // EXT_XZX
{+R_1_1, C_ZERO, +R_0_2, +R_0_0, C_PI, -R_2_0, +R_0_0, +R_1_0, -R_1_2, +R_1_1, +R_0_1, +R_2_1}, // EXT_YXY
{+R_1_1, C_ZERO, +R_0_2, +R_0_0, C_PI, +R_0_2, -R_0_0, +R_1_2, +R_1_0, +R_1_1, +R_2_1, -R_0_1}, // EXT_YZY
{+R_2_2, C_ZERO, +R_1_0, +R_1_1, C_PI, +R_1_0, +R_0_0, +R_2_0, +R_2_1, +R_2_2, +R_0_2, -R_1_2}, // EXT_ZXZ
{+R_2_2, C_ZERO, +R_1_0, +R_0_0, C_PI, +R_1_0, +R_0_0, +R_2_1, -R_2_0, +R_2_2, +R_1_2, +R_0_2}, // EXT_ZYZ
};
T rotationR[12];
for (int i = 0; i < 12; i++)
{
int id = rotationR_[eulerAnglesType][i];
unsigned idx = std::abs(id);
T value = 0.0f;
if (idx < N_CONSTANTS)
{
value = constants_[idx];
}
else
{
unsigned r_idx = idx - N_CONSTANTS;
CV_DbgAssert(r_idx < 9);
value = R.val[r_idx];
}
bool isNegative = id < 0;
if (isNegative)
value = -value;
rotationR[i] = value;
}
Vec<T, 3> angles;
if (detail::isIntAngleType(eulerAnglesType))
{
if (abs(rotationR[0] - 1) < CV_QUAT_CONVERT_THRESHOLD)
{
CV_LOG_WARNING(NULL,"Gimbal Lock occurs. Euler angles are non-unique, we set the third angle to 0");
angles = {std::atan2(rotationR[1], rotationR[2]), rotationR[3], 0};
return angles;
}
else if(abs(rotationR[0] + 1) < CV_QUAT_CONVERT_THRESHOLD)
{
CV_LOG_WARNING(NULL,"Gimbal Lock occurs. Euler angles are non-unique, we set the third angle to 0");
angles = {std::atan2(rotationR[4], rotationR[5]), rotationR[6], 0};
return angles;
}
}
else // (!detail::isIntAngleType<T>(eulerAnglesType))
{
if (abs(rotationR[0] - 1) < CV_QUAT_CONVERT_THRESHOLD)
{
CV_LOG_WARNING(NULL,"Gimbal Lock occurs. Euler angles are non-unique, we set the first angle to 0");
angles = {0, rotationR[1], std::atan2(rotationR[2], rotationR[3])};
return angles;
}
else if (abs(rotationR[0] + 1) < CV_QUAT_CONVERT_THRESHOLD)
{
CV_LOG_WARNING(NULL,"Gimbal Lock occurs. Euler angles are non-unique, we set the first angle to 0");
angles = {0, rotationR[4], std::atan2(rotationR[5], rotationR[6])};
return angles;
}
}
angles(0) = std::atan2(rotationR[7], rotationR[8]);
if (detail::isTaitBryan(eulerAnglesType))
angles(1) = std::acos(rotationR[9]);
else
angles(1) = std::asin(rotationR[9]);
angles(2) = std::atan2(rotationR[10], rotationR[11]);
return angles;
}
} // namepsace } // namepsace
//! @endcond //! @endcond

62
modules/core/test/test_quaternion.cpp
Unescape View File

@ -258,6 +258,68 @@ TEST_F(QuatTest, interpolation){
EXPECT_EQ(Quatd::spline(tr1, tr2, tr3, tr3, 0.5), Quatd(0.336889853392, 0.543600719487, 0.543600719487, 0.543600719487)); EXPECT_EQ(Quatd::spline(tr1, tr2, tr3, tr3, 0.5), Quatd(0.336889853392, 0.543600719487, 0.543600719487, 0.543600719487));
} }
static const Quatd qEuler[24] = {
Quatd(0.7233214, 0.3919013, 0.2005605, 0.5319728), //INT_XYZ
Quatd(0.8223654, 0.0222635, 0.3604221, 0.4396766), //INT_XZY
Quatd(0.822365, 0.439677, 0.0222635, 0.360422), //INT_YXZ
Quatd(0.723321, 0.531973, 0.391901, 0.20056), //INT_YZX
Quatd(0.723321, 0.20056, 0.531973, 0.391901), //INT_ZXY
Quatd(0.822365, 0.360422, 0.439677, 0.0222635), //INT_ZYX
Quatd(0.653285, 0.65328, 0.369641, -0.0990435), //INT_XYX
Quatd(0.653285, 0.65328, 0.0990435, 0.369641), //INT_XZX
Quatd(0.653285, 0.369641, 0.65328, 0.0990435), //INT_YXY
Quatd(0.653285, -0.0990435, 0.65328, 0.369641), //INT_YZY
Quatd(0.653285, 0.369641, -0.0990435, 0.65328), //INT_ZXZ
Quatd(0.653285, 0.0990435, 0.369641, 0.65328), //INT_ZYZ
Quatd(0.822365, 0.0222635, 0.439677, 0.360422), //EXT_XYZ
Quatd(0.723321, 0.391901, 0.531973, 0.20056), //EXT_XZY
Quatd(0.723321, 0.20056, 0.391901, 0.531973), //EXT_YXZ
Quatd(0.822365, 0.360422, 0.0222635, 0.439677), //EXT_YZX
Quatd(0.822365, 0.439677, 0.360422, 0.0222635), //EXT_ZXY
Quatd(0.723321, 0.531973, 0.20056, 0.391901), //EXT_ZYX
Quatd(0.653285, 0.65328, 0.369641, 0.0990435), //EXT_XYX
Quatd(0.653285, 0.65328, -0.0990435, 0.369641), //EXT_XZX
Quatd(0.653285, 0.369641, 0.65328, -0.0990435), //EXT_YXY
Quatd(0.653285, 0.0990435, 0.65328, 0.369641), //EXT_YZY
Quatd(0.653285, 0.369641, 0.0990435, 0.65328), //EXT_ZXZ
Quatd(0.653285, -0.0990435, 0.369641, 0.65328) //EXT_ZYZ
};
TEST_F(QuatTest, EulerAngles){
Vec3d test_angle = {0.523598, 0.78539, 1.04719};
for (QuatEnum::EulerAnglesType i = QuatEnum::EulerAnglesType::INT_XYZ; i <= QuatEnum::EulerAnglesType::EXT_ZYZ; i = (QuatEnum::EulerAnglesType)(i + 1))
{
SCOPED_TRACE(cv::format("EulerAnglesType=%d", i));
Quatd q = Quatd::createFromEulerAngles(test_angle, i);
EXPECT_EQ(q, qEuler[i]);
Vec3d Euler_Angles = q.toEulerAngles(i);
EXPECT_NEAR(Euler_Angles[0], test_angle[0], 1e-6);
EXPECT_NEAR(Euler_Angles[1], test_angle[1], 1e-6);
EXPECT_NEAR(Euler_Angles[2], test_angle[2], 1e-6);
}
Quatd qEuler0 = {0, 0, 0, 0};
EXPECT_ANY_THROW(qEuler0.toEulerAngles(QuatEnum::INT_XYZ));
Quatd qEulerLock1 = {0.5612665, 0.43042, 0.5607083, 0.4304935};
Vec3d test_angle_lock1 = {1.3089878, CV_PI * 0.5, 0};
Vec3d Euler_Angles_solute_1 = qEulerLock1.toEulerAngles(QuatEnum::INT_XYZ);
EXPECT_NEAR(Euler_Angles_solute_1[0], test_angle_lock1[0], 1e-6);
EXPECT_NEAR(Euler_Angles_solute_1[1], test_angle_lock1[1], 1e-6);
EXPECT_NEAR(Euler_Angles_solute_1[2], test_angle_lock1[2], 1e-6);
Quatd qEulerLock2 = {0.7010574, 0.0922963, 0.7010573, -0.0922961};
Vec3d test_angle_lock2 = {-0.2618, CV_PI * 0.5, 0};
Vec3d Euler_Angles_solute_2 = qEulerLock2.toEulerAngles(QuatEnum::INT_ZYX);
EXPECT_NEAR(Euler_Angles_solute_2[0], test_angle_lock2[0], 1e-6);
EXPECT_NEAR(Euler_Angles_solute_2[1], test_angle_lock2[1], 1e-6);
EXPECT_NEAR(Euler_Angles_solute_2[2], test_angle_lock2[2], 1e-6);
Vec3d test_angle6 = {CV_PI * 0.25, CV_PI * 0.5, CV_PI * 0.25};
Vec3d test_angle7 = {CV_PI * 0.5, CV_PI * 0.5, 0};
EXPECT_EQ(Quatd::createFromEulerAngles(test_angle6, QuatEnum::INT_ZXY), Quatd::createFromEulerAngles(test_angle7, QuatEnum::INT_ZXY));
}
} // namespace } // namespace
}// opencv_test }// opencv_test

Write Preview
Loading…
Cancel
Save