mirror of
https://github.com/opencv/opencv.git
synced 2024-11-25 19:50:38 +08:00
Merge pull request #19098 from chargerKong:EulerAngle
* 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>
This commit is contained in:
Liangqian
GitHub
parent
bec7b297ed
commit
e4c7fca755
@ -27,6 +27,7 @@
|
||||
#define OPENCV_CORE_QUATERNION_HPP
|
||||
|
||||
#include <opencv2/core.hpp>
|
||||
#include <opencv2/core/utils/logger.hpp>
|
||||
#include <iostream>
|
||||
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 <typename _Tp> class Quat;
|
||||
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");
|
||||
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 <typename 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;
|
||||
|
||||
};
|
||||
|
||||
@ -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);
|
||||
}
|
||||
|
||||
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
|
||||
//! @endcond
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user