~hedgepigdaniel/dewobble

ab2b417fa7c28a71b27ecb17d5cce7a1f4667ccb — Daniel Playfair Cal 2 years ago d89f3de
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));
    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;
}

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