## ~hedgepigdaniel/dewobble

ab2b417fa7c28a71b27ecb17d5cce7a1f4667ccb — Daniel Playfair Cal 2 years ago
```refactor: replace euler angles helper with licensed code
```
```3 files changed, 82 insertions(+), 84 deletions(-)

M include/rotation.hpp
M src/rotation.cpp
M src/rotation_detector.cpp
```
`M include/rotation.hpp => include/rotation.hpp +6 -11`
```@@ 1,19 1,14 @@
#ifndef DEWOBBLE_ROTATION_HPP
#define DEWOBBLE_ROTATION_HPP

-// Copied from https://learnopencv.com/rotation-matrix-to-euler-angles/
+// Copied from OpenCV real time pose estimation sample

#include <opencv2/core.hpp>

-// Checks if a matrix is a valid rotation matrix.
-bool isRotationMatrix(cv::Mat R);
+// Converts a given Rotation Matrix to Euler angles
+cv::Mat rotation_matrix_to_euler_angles(const cv::Mat &rotation_matrix);

-// Calculates rotation matrix to euler angles
-// The result is the same as MATLAB except the order
-// of the euler angles ( x and z are swapped ).
-cv::Vec3f rotationMatrixToEulerAngles(cv::Mat R);
+// Converts a given Euler angles to Rotation Matrix
+cv::Mat euler_angles_to_rotation_matrix(const cv::Mat &euler_angles);

-// Calculates rotation matrix given euler angles.
-cv::Mat eulerAnglesToRotationMatrix(cv::Vec3f theta);
-
-#endif // DEWOBBLE_ROTATION_HPP>
\ No newline at end of file
+#endif // DEWOBBLE_ROTATION_HPP

```
`M src/rotation.cpp => src/rotation.cpp +71 -68`
```@@ 4,84 4,87 @@

using namespace cv;

-// Checks if a matrix is a valid rotation matrix.
-bool isRotationMatrix(Mat R)
+// 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 rotation_matrix_to_euler_angles(const cv::Mat &rotation_matrix)
{
-    Mat Rt;
-    transpose(R, Rt);
-    Mat shouldBeIdentity = Rt * R;
-    Mat I = Mat::eye(3, 3, shouldBeIdentity.type());
+    cv::Mat euler_angles(3, 1, CV_64F);

-    return norm(I, shouldBeIdentity) < 1e-6;
-}
-
-// Calculates rotation matrix to euler angles
-// The result is the same as MATLAB except the order
-// of the euler angles ( x and z are swapped ).
-Vec3f rotationMatrixToEulerAngles(Mat R)
-{
-    assert(isRotationMatrix(R));
+    double m00 = rotation_matrix.at<double>(0, 0);
+    double m02 = rotation_matrix.at<double>(0, 2);
+    double m10 = rotation_matrix.at<double>(1, 0);
+    double m11 = rotation_matrix.at<double>(1, 1);
+    double m12 = rotation_matrix.at<double>(1, 2);
+    double m20 = rotation_matrix.at<double>(2, 0);
+    double m22 = rotation_matrix.at<double>(2, 2);

-    float sy = sqrt(
-        R.at<double>(0, 0) * R.at<double>(0, 0) +
-        R.at<double>(1, 0) * R.at<double>(1, 0));

-    bool singular = sy < 1e-6; // If
-
-    float x, y, z;
-    if (!singular) {
-        x = atan2(R.at<double>(2, 1), R.at<double>(2, 2));
-        y = atan2(-R.at<double>(2, 0), sy);
-        z = atan2(R.at<double>(1, 0), R.at<double>(0, 0));
+    // Assuming the angles are in radians.
+    if (m10 > 0.998) { // singularity at north pole
+        bank = 0;
+        attitude = CV_PI / 2;
+    } else if (m10 < -0.998) { // singularity at south pole
+        bank = 0;
+        attitude = -CV_PI / 2;
} else {
-        x = atan2(-R.at<double>(1, 2), R.at<double>(1, 1));
-        y = atan2(-R.at<double>(2, 0), sy);
-        z = 0;
+        bank = atan2(-m12, m11);
+        attitude = asin(m10);
}
-    return Vec3f(x, y, z);
+
+    euler_angles.at<double>(0) = bank;
+    euler_angles.at<double>(1) = attitude;
+
+    return euler_angles;
}

-// Calculates rotation matrix given euler angles.
-Mat eulerAnglesToRotationMatrix(Vec3f theta)
+// 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 euler_angles_to_rotation_matrix(const cv::Mat &euler_angles)
{
-    // Calculate rotation about x axis
-    Mat R_x =
-        (Mat_<double>(3, 3) << 1,
-         0,
-         0,
-         0,
-         cos(theta[0]),
-         -sin(theta[0]),
-         0,
-         sin(theta[0]),
-         cos(theta[0]));
+    cv::Mat rotation_matrix(3, 3, CV_64F);
+
+    double bank = euler_angles.at<double>(0);
+    double attitude = euler_angles.at<double>(1);

-    // Calculate rotation about y axis
-    Mat R_y =
-        (Mat_<double>(3, 3) << cos(theta[1]),
-         0,
-         sin(theta[1]),
-         0,
-         1,
-         0,
-         -sin(theta[1]),
-         0,
-         cos(theta[1]));
+    // Assuming the angles are in radians.
+    double ca = cos(attitude);
+    double sa = sin(attitude);
+    double cb = cos(bank);
+    double sb = sin(bank);

-    // Calculate rotation about z axis
-    Mat R_z =
-        (Mat_<double>(3, 3) << cos(theta[2]),
-         -sin(theta[2]),
-         0,
-         sin(theta[2]),
-         cos(theta[2]),
-         0,
-         0,
-         0,
-         1);
+    double m00, m01, m02, m10, m11, m12, m20, m21, m22;

-    // Combined rotation matrix
-    Mat R = R_z * R_y * R_x;
+    m00 = ch * ca;
+    m01 = sh * sb - ch * sa * cb;
+    m02 = ch * sa * sb + sh * cb;
+    m10 = sa;
+    m11 = ca * cb;
+    m12 = -ca * sb;
+    m20 = -sh * ca;
+    m21 = sh * sa * cb + ch * sb;
+    m22 = -sh * sa * sb + ch * cb;

-    return R;
-}>
\ No newline at end of file
+    rotation_matrix.at<double>(0, 0) = m00;
+    rotation_matrix.at<double>(0, 1) = m01;
+    rotation_matrix.at<double>(0, 2) = m02;
+    rotation_matrix.at<double>(1, 0) = m10;
+    rotation_matrix.at<double>(1, 1) = m11;
+    rotation_matrix.at<double>(1, 2) = m12;
+    rotation_matrix.at<double>(2, 0) = m20;
+    rotation_matrix.at<double>(2, 1) = m21;
+    rotation_matrix.at<double>(2, 2) = m22;
+
+    return rotation_matrix;
+}

```
`M src/rotation_detector.cpp => src/rotation_detector.cpp +5 -5`
```@@ 179,9 179,9 @@ void RotationDetector::push_frame(Frame frame)
continue;
}

-            Vec3f angles = rotationMatrixToEulerAngles(measured_rotation);
+            Vec3f angles = rotation_matrix_to_euler_angles(measured_rotation);
Vec3f last_frame_angles =
-                rotationMatrixToEulerAngles(m_last_frame_rotation);
+                rotation_matrix_to_euler_angles(m_last_frame_rotation);
double angle_change = norm(angles - last_frame_angles);
if (angle_change > MAX_ANGLE_CHANGE_PER_FRAME) {
cerr << "Rejecting detection of excessive angle in a single "

@@ 260,10 260,10 @@ FrameWithOrientation RotationDetector::pull_frame()
}
float source_weight = (target_distance - 1.f) / target_distance;
float target_weight = 1.f / target_distance;
-        frame.orientation = eulerAnglesToRotationMatrix(
+        frame.orientation = euler_angles_to_rotation_matrix(
source_weight *
-                rotationMatrixToEulerAngles(m_last_output_frame_rotation) +
-            target_weight * rotationMatrixToEulerAngles(target_rotation));
+                rotation_matrix_to_euler_angles(m_last_output_frame_rotation) +
+            target_weight * rotation_matrix_to_euler_angles(target_rotation));
// cerr << "interpolating between " <<
// rotationMatrixToEulerAngles(m_last_output_frame_rotation) <<
//     " and " << rotationMatrixToEulerAngles(target_rotation) << " ("

```