3 files changed, 82 insertions(+), 84 deletions(-)

M include/rotation.hpp
M src/rotation.cpp
M src/rotation_detector.cpp

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

@@ 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));
+    double bank, attitude, heading;
 
-    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;
+        heading = atan2(m02, m22);
+    } else if (m10 < -0.998) { // singularity at south pole
+        bank = 0;
+        attitude = -CV_PI / 2;
+        heading = atan2(m02, m22);
     } 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);
+        heading = atan2(-m20, m00);
     }
-    return Vec3f(x, y, z);
+
+    euler_angles.at<double>(0) = bank;
+    euler_angles.at<double>(1) = attitude;
+    euler_angles.at<double>(2) = heading;
+
+    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);
+    double heading = euler_angles.at<double>(2);
 
-    // 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 ch = cos(heading);
+    double sh = sin(heading);
+    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;
+}

@@ 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) << " ("