Add to/from Euler Angles in quaternion #19098

chargerKong · 2020-12-13T06:51:52Z

We finally support all types of Euler angles to/from the quaternion.

Pull Request Readiness Checklist

See details at https://github.com/opencv/opencv/wiki/How_to_contribute#making-a-good-pull-request

I agree to contribute to the project under Apache 2 License.
To the best of my knowledge, the proposed patch is not based on a code under GPL or other license that is incompatible with OpenCV
The PR is proposed to proper branch
There is reference to original bug report and related work
There is accuracy test, performance test and test data in opencv_extra repository, if applicable
Patch to opencv_extra has the same branch name.
The feature is well documented and sample code can be built with the project CMake

alalek · 2020-12-13T18:20:38Z

modules/core/include/opencv2/core/quaternion.inl.hpp

+    {
+        Quat<T> q1 =  Quat<T>::createFromXRot(angles(0) / 2.);
+        Quat<T> q2 =  Quat<T>::createFromYRot(angles(1) / 2.);
+        Quat<T> q3 =  Quat<T>::createFromZRot(angles(2) / 2.);


/ 2.

* 0.5f

OK! Thanks for your reviewing code for us.

modules/core/include/opencv2/core/quaternion.hpp

alalek · 2020-12-13T18:33:34Z

modules/core/include/opencv2/core/quaternion.inl.hpp

+        Quat<T> q1 =  Quat<T>::createFromXRot(angles(0) / 2.);
+        Quat<T> q2 =  Quat<T>::createFromYRot(angles(1) / 2.);
+        Quat<T> q3 =  Quat<T>::createFromZRot(angles(2) / 2.);
+        q = q1 * q2 * q3;


Lets reduce amount of generated binary code.

Make private function (detail namespace should be fine)

template <typename T> Quat<T> createQuatFromRot(int axis, T angle) { if (i == 0) return Quat<T>::createFromXRot(angle * 0.5f); if (i == 1) return Quat<T>::createFromXRot(angle * 0.5f); if (i == 2) return Quat<T>::createFromXRot(angle * 0.5f); CV_Assert(0); }

Fill axis1,axis2,axis3 (preferable from static const int rotationAxis[][3] = { { i1, i2, i3 }, { i1, i2, i3}, ... }; table):

int axis1, axis2, axis3; // values are 0/1/2 for X/Y/Z

enum EulerAnglesType { ... #ifndef CV_DOXYGEN EULER_ANGLES_MAX_VALUE #endif }; + static assert for table size using EULER_ANGLES_MAX_VALUE + runtime assert: eulerAnglesType < EULER_ANGLES_MAX_VALUE

Finally, use this common code in epilog:

Quat<T> q1 = Quat<T>::createQuatFromRot(axis1, angles(0)); Quat<T> q2 = Quat<T>::createQuatFromRot(axis2, angles(1)); Quat<T> q3 = Quat<T>::createQuatFromRot(axis3, angles(2)); q = q1 * q2 * q3;

Thanks, I got it and I will update the implementation of toEulerAngle() in the same way.

@alalek The table size of rotationAxis is fixed (24,4). Is it necessary to add static_assert?

Static assert is a compilation guard if someone would change/update corresponding enum values.

alalek · 2020-12-13T18:36:58Z

modules/core/include/opencv2/core/quaternion.inl.hpp

+        if (abs(R(0, 2) - 1) < CV_QUAT_CONVERT_THRESHOLD)
+        {
+            std::cout << "WARNING: Gimbal Lock will occur. Euler angles are non-unique." << std::endl;
+            std::cout << "For intrinsic rotations, we set the third angle to zero, and for external rotation, we set the first angle to zero." << std::endl;


std::cout

Avoid that. There is CV_LOG_WARNING(). However messages probably should be reduced to INFO level.

restruct codes

savuor · 2020-12-15T19:59:13Z

modules/core/include/opencv2/core/quaternion.hpp

+     *
+     * 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).


Please fix the links to paragraphs in that wiki article

OK, we will update doc.

savuor · 2020-12-15T22:50:29Z

modules/core/include/opencv2/core/quaternion.hpp

+     * 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$.


Please clarify what is s

OK, thanks! s_2 mean sin\theta_2, I will fix the issue.

modules/core/include/opencv2/core/quaternion.inl.hpp

alalek

Please add commits with code optimizations from here: https://github.com/alalek/opencv/commits/pr19098_r

alalek · 2020-12-17T03:42:16Z

modules/core/include/opencv2/core/quaternion.hpp

@@ -845,8 +953,8 @@ class Quat
    /**
     * @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 normaliz this


OK, thanks for your commits! We have added these codes and removed the typo.

alalek · 2020-12-17T03:44:29Z

modules/core/include/opencv2/core/quaternion.inl.hpp

+namespace
+{


namespace detail { should be better

alalek · 2020-12-17T03:44:39Z

modules/core/include/opencv2/core/quaternion.inl.hpp

+namespace
+{
+    template <typename T>
+    Quat<T> createFromAxisRot(int axis, const T theta)


It make sense to add static to keep this local:

template <typename T> static Quat<T> createFromAxisRot(int axis, const T theta)

Thank you! I got it！

alalek · 2020-12-17T03:46:53Z

modules/core/include/opencv2/core/quaternion.inl.hpp

+        Quat<T> q3 = createFromAxisRot(rotationAxis[eulerAnglesType][2], angles(2));
+        return q1 * q2 * q3;
+    }
+    else if (rotationAxis[eulerAnglesType][3]){


} else // (rotationAxis[eulerAnglesType][3]) {

and remove CV_Assert(0).

modules/core/include/opencv2/core/quaternion.inl.hpp

alalek · 2020-12-17T04:07:22Z

modules/core/include/opencv2/core/quaternion.inl.hpp

+        {R(2, 2), 0, R(1, 0), R(1, 1), CV_PI, R(1, 0), R(0, 0),R(2, 0), R(2, 1), R(2, 2),R(0, 2), -R(1, 2), 1}, //EXT_ZXZ
+        {R(2, 2), 0, R(1, 0), R(0, 0), CV_PI, R(1, 0), R(0, 0),R(2, 1), -R(2, 0), R(2, 2),R(1, 2), R(0, 2), 1} //EXT_ZYZ
+    };
+    if(!rotationR[eulerAnglesType][12]){


Before this if build dynamic table:

T rotationR[12] through decoding in a loop using the indexes above.

And apply replacement rotationR[eulerAnglesType] => rotationR for code below (it should not have large changes).

OK! We really appreciate your help.

alalek · 2020-12-17T07:01:04Z

modules/core/include/opencv2/core/quaternion.hpp

+     *   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


It is better to avoid placing enums in template classes.

I see these options:

base non-template class with such constants (QuatEnum?)

external enum with proper prefixes (like QUAT_ASSUME_NOT_UNIT)

OK, we will create class QuatEnum to place enum EulerAnglesType.

alalek · 2020-12-17T07:04:47Z

modules/core/test/test_quaternion.cpp

+    Quatd qEuler1 = Quatd::createFromEulerAngles(test_angle, Quatd::INT_XYZ);
+    Quatd qEuler2 = Quatd::createFromEulerAngles(test_angle, Quatd::INT_XZY);
+    Quatd qEuler3 = Quatd::createFromEulerAngles(test_angle, Quatd::INT_XZX);


Consider using arrays and for loops to process/check values. Like qEuler[24]
This should reduce amount of code.

Use error messages to help with problem investigations. See proposed commits.

OK, we use for loops and array qEuler[24] in test codes to reduce duplicate code.

savuor · 2020-12-17T14:23:17Z

modules/core/test/test_quaternion.cpp

+
+TEST_F(QuatTest, EulerAngles){
+    Vec3d test_angle={0.523598,0.78539,1.04719};
+    Quatd qEuler0 = Quatd(0, 0, 0, 0);


Why not transform these huge lists (here and below) to loops?

OK, thanks, I solved this problem by using the for loop method and array qEuler.

alalek

Good progress!

modules/core/include/opencv2/core/quaternion.inl.hpp

modules/core/test/test_quaternion.cpp

…test_quaternion.cpp

alalek

Looks good to me 👍

* 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>

chargerKong force-pushed the EulerAngle branch from 6e9de09 to 4785bea Compare December 13, 2020 06:59

chargerKong changed the title ~~add to/from Euler Angles in quaternion~~ Add to/from Euler Angles in quaternion Dec 13, 2020

add to/from Euler Angles

77d94e1

chargerKong force-pushed the EulerAngle branch from 4785bea to 77d94e1 Compare December 13, 2020 07:13

alalek reviewed Dec 13, 2020

View reviewed changes

ShanChenqi and others added 2 commits December 15, 2020 23:39

restruct codes

75bac33

Merge pull request #2 from shanchenqi/rotation_converter-2

43e470d

restruct codes

savuor reviewed Dec 15, 2020

View reviewed changes

chargerKong requested review from savuor and alalek December 17, 2020 03:12

alalek added 2 commits December 17, 2020 06:55

quat: optimize implementation

bdb64b3

cleanup debug code

64e7477

alalek reviewed Dec 17, 2020

View reviewed changes

correct spelling errors

3699fa2

chargerKong force-pushed the EulerAngle branch from d0bbc0a to 3699fa2 Compare December 17, 2020 10:33

create QuatEnum for enum EulerAnglesType

cfdff44

savuor reviewed Dec 17, 2020

View reviewed changes

use for loop for test_quaternion

22a0dd4

chargerKong force-pushed the EulerAngle branch from 0851141 to 22a0dd4 Compare December 18, 2020 07:00

alalek reviewed Dec 23, 2020

View reviewed changes

drop template from isIntAngleType & add minimal error information in …

2fe2b19

…test_quaternion.cpp

alalek approved these changes Dec 27, 2020

View reviewed changes

alalek merged commit e4c7fca into opencv:master Dec 31, 2020

chargerKong deleted the EulerAngle branch January 28, 2021 06:21

alalek mentioned this pull request Feb 2, 2021

Transformation from Euler angle to quaternion. #18908

Closed

alalek mentioned this pull request Apr 9, 2021

(5.x) Merge 4.x #19885

Merged

Uh oh!

Add to/from Euler Angles in quaternion #19098

Add to/from Euler Angles in quaternion #19098

Uh oh!

Conversation

chargerKong commented Dec 13, 2020

Pull Request Readiness Checklist

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

alalek left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

alalek left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

alalek left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!