diff --git a/modules/core/include/opencv2/core/quaternion.hpp b/modules/core/include/opencv2/core/quaternion.hpp index 7bc51e6c6d..8c21501e3f 100644 --- a/modules/core/include/opencv2/core/quaternion.hpp +++ b/modules/core/include/opencv2/core/quaternion.hpp @@ -27,6 +27,7 @@ #define OPENCV_CORE_QUATERNION_HPP #include +#include #include namespace cv { @@ -51,6 +52,83 @@ enum QuatAssumeType 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 class Quat; template 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"); using value_type = _Tp; - public: static constexpr _Tp CV_QUAT_EPS = (_Tp)1.e-6; + static constexpr _Tp CV_QUAT_CONVERT_THRESHOLD = (_Tp)1.e-6; Quat(); @@ -182,6 +260,41 @@ public: */ 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. * @param index over a range [0, 3]. @@ -811,8 +924,8 @@ public: /** * @brief transform a quaternion to a 3x3 rotation matrix. * @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 - * quaternion at first then to do the transformation. + * this function will save some computations. Otherwise, this function will normalize this + * quaternion at first then do the transformation. * * @note Matrix A which is to be rotated should have the form * \f[\begin{bmatrix} @@ -845,8 +958,8 @@ public: /** * @brief transform a quaternion to a 4x4 rotation matrix. * @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 - * quaternion at first then to do the transformation. + * this function will save some computations. Otherwise, this function will normalize this + * quaternion at first then do the transformation. * * The operations is similar as toRotMat3x3 * except that the points matrix should have the form @@ -1434,6 +1547,74 @@ public: template friend std::ostream& cv::operator<<(std::ostream&, const Quat&); + /** + * @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; }; diff --git a/modules/core/include/opencv2/core/quaternion.inl.hpp b/modules/core/include/opencv2/core/quaternion.inl.hpp index f665dbe6c8..260144c841 100644 --- a/modules/core/include/opencv2/core/quaternion.inl.hpp +++ b/modules/core/include/opencv2/core/quaternion.inl.hpp @@ -866,6 +866,197 @@ Quat Quat::spline(const Quat &q0, const Quat &q1, const Quat &q2, return squad(vec[1], s1, s2, vec[2], t, assumeUnit, QUAT_ASSUME_NOT_UNIT); } +namespace detail { + +template static +Quat createFromAxisRot(int axis, const T theta) +{ + if (axis == 0) + return Quat::createFromXRot(theta); + if (axis == 1) + return Quat::createFromYRot(theta); + if (axis == 2) + return Quat::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 +Quat Quat::createFromYRot(const T theta) +{ + return Quat{std::cos(theta * 0.5f), 0, std::sin(theta * 0.5f), 0}; +} + +template +Quat Quat::createFromXRot(const T theta){ + return Quat{std::cos(theta * 0.5f), std::sin(theta * 0.5f), 0, 0}; +} + +template +Quat Quat::createFromZRot(const T theta){ + return Quat{std::cos(theta * 0.5f), 0, 0, std::sin(theta * 0.5f)}; +} + + +template +Quat Quat::createFromEulerAngles(const Vec &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 q1 = detail::createFromAxisRot(rotationAxis[eulerAnglesType][0], angles(0)); + Quat q2 = detail::createFromAxisRot(rotationAxis[eulerAnglesType][1], angles(1)); + Quat q3 = detail::createFromAxisRot(rotationAxis[eulerAnglesType][2], angles(2)); + if (detail::isIntAngleType(eulerAnglesType)) + { + return q1 * q2 * q3; + } + else // (!detail::isIntAngleType(eulerAnglesType)) + { + return q3 * q2 * q1; + } +} + +template +Vec Quat::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 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(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 //! @endcond diff --git a/modules/core/test/test_quaternion.cpp b/modules/core/test/test_quaternion.cpp index 324d535bff..9da248313e 100644 --- a/modules/core/test/test_quaternion.cpp +++ b/modules/core/test/test_quaternion.cpp @@ -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)); } +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 }// opencv_test