From d02ecff881d58137878b1c2b9425b25834ef2562 Mon Sep 17 00:00:00 2001
From: catree <catree.catreus@outlook.com>
Date: Sat, 12 May 2018 01:11:13 +0200
Subject: [PATCH] Clarify the Euler angles convention chosen. Replace rotation
 inverse with matrix transpose.

---
 .../real_time_pose_estimation/src/Utils.cpp   | 52 +++++++++++--------
 1 file changed, 29 insertions(+), 23 deletions(-)

diff --git a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Utils.cpp b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Utils.cpp
index b548bed463c5..1945a2eb8bb1 100644
--- a/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Utils.cpp
+++ b/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/src/Utils.cpp
@@ -179,13 +179,16 @@ double get_translation_error(const cv::Mat &t_true, const cv::Mat &t)
 double get_rotation_error(const cv::Mat &R_true, const cv::Mat &R)
 {
   cv::Mat error_vec, error_mat;
-  error_mat = R_true * cv::Mat(R.inv()).mul(-1);
+  error_mat = -R_true * R.t();
   cv::Rodrigues(error_mat, error_vec);
 
   return cv::norm(error_vec);
 }
 
 // Converts a given Rotation Matrix to Euler angles
+// Convention used is Y-Z-X Tait-Bryan angles
+// Reference code implementation:
+// https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToEuler/index.htm
 cv::Mat rot2euler(const cv::Mat & rotationMatrix)
 {
   cv::Mat euler(3,1,CV_64F);
@@ -198,49 +201,52 @@ cv::Mat rot2euler(const cv::Mat & rotationMatrix)
   double m20 = rotationMatrix.at<double>(2,0);
   double m22 = rotationMatrix.at<double>(2,2);
 
-  double x, y, z;
+  double bank, attitude, heading;
 
   // Assuming the angles are in radians.
   if (m10 > 0.998) { // singularity at north pole
-    x = 0;
-    y = CV_PI/2;
-    z = atan2(m02,m22);
+    bank = 0;
+    attitude = CV_PI/2;
+    heading = atan2(m02,m22);
   }
   else if (m10 < -0.998) { // singularity at south pole
-    x = 0;
-    y = -CV_PI/2;
-    z = atan2(m02,m22);
+    bank = 0;
+    attitude = -CV_PI/2;
+    heading = atan2(m02,m22);
   }
   else
   {
-    x = atan2(-m12,m11);
-    y = asin(m10);
-    z = atan2(-m20,m00);
+    bank = atan2(-m12,m11);
+    attitude = asin(m10);
+    heading = atan2(-m20,m00);
   }
 
-  euler.at<double>(0) = x;
-  euler.at<double>(1) = y;
-  euler.at<double>(2) = z;
+  euler.at<double>(0) = bank;
+  euler.at<double>(1) = attitude;
+  euler.at<double>(2) = heading;
 
   return euler;
 }
 
 // Converts a given Euler angles to Rotation Matrix
+// Convention used is Y-Z-X Tait-Bryan angles
+// Reference:
+// https://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToMatrix/index.htm
 cv::Mat euler2rot(const cv::Mat & euler)
 {
   cv::Mat rotationMatrix(3,3,CV_64F);
 
-  double x = euler.at<double>(0);
-  double y = euler.at<double>(1);
-  double z = euler.at<double>(2);
+  double bank = euler.at<double>(0);
+  double attitude = euler.at<double>(1);
+  double heading = euler.at<double>(2);
 
   // Assuming the angles are in radians.
-  double ch = cos(z);
-  double sh = sin(z);
-  double ca = cos(y);
-  double sa = sin(y);
-  double cb = cos(x);
-  double sb = sin(x);
+  double ch = cos(heading);
+  double sh = sin(heading);
+  double ca = cos(attitude);
+  double sa = sin(attitude);
+  double cb = cos(bank);
+  double sb = sin(bank);
 
   double m00, m01, m02, m10, m11, m12, m20, m21, m22;