Search code examples
c++matrixeigen

eigen eulerAngles() returns incorrect values

I try to extract euler angles from eigen 3x3 rotation matrix. However the values I got from eulerAngles() method seems not correct. I wrote a small test code and I've got the strange results. The code is belows.

#include <iostream>
#include <random>
#include <Eigen/Core>
#include <Eigen/Geometry>

int main() {
    std::random_device rd;
    std::mt19937_64 gen(rd());
    std::normal_distribution<double> dis(0, 2);

    while (true) {
        // 1. given euler angles and rotation matrix
        Eigen::Vector3d euler;
        Eigen::Matrix3d rot;
        euler << dis(gen), dis(gen), dis(gen);

        {
            Eigen::AngleAxisd Y(euler(2), Eigen::Vector3d::UnitZ());
            Eigen::AngleAxisd P(euler(1), Eigen::Vector3d::UnitY());
            Eigen::AngleAxisd R(euler(0), Eigen::Vector3d::UnitX());
            rot = Eigen::Quaterniond(Y*P*R).toRotationMatrix();
        }

        // 2. generate a new rotation matrix using eular angles calculated from given rotation matrix
        Eigen::Matrix3d rot_from_euler_mine;
        Eigen::Vector3d euler_mine;
        euler_mine(0) = std::atan2(rot(2, 1), rot(2, 2));
        euler_mine(1) = std::atan2(-rot(2, 0), std::sqrt(rot(2, 1) * rot(2, 1) + rot(2, 2) * rot(2, 2)));
        euler_mine(2) = std::atan2(rot(1, 0), rot(0, 0));

        {
            Eigen::AngleAxisd Y(euler_mine(2), Eigen::Vector3d::UnitZ());
            Eigen::AngleAxisd P(euler_mine(1), Eigen::Vector3d::UnitY());
            Eigen::AngleAxisd R(euler_mine(0), Eigen::Vector3d::UnitX());
            rot_from_euler_mine = Eigen::Quaterniond(Y*P*R).toRotationMatrix();
        }

        // 3. generate a new rotation matrices using eulerAngles() with given rotation matrix
        Eigen::Matrix3d rot_from_euler_t;
        Eigen::Vector3d euler_t = rot.eulerAngles(0, 1, 2);

        {
            Eigen::AngleAxisd Y(euler_t(2), Eigen::Vector3d::UnitZ());
            Eigen::AngleAxisd P(euler_t(1), Eigen::Vector3d::UnitY());
            Eigen::AngleAxisd R(euler_t(0), Eigen::Vector3d::UnitX());
            rot_from_euler_t = Eigen::Quaterniond(Y*P*R).toRotationMatrix();
        }

        std::cout << "=== 1. given euler angles =========================================================" << std::endl;
        std::cout << euler(0) << ", " << euler(1) << ", " << euler(2) << std::endl;
        std::cout << "=== 2. euler angles from given rotation matrix (using my calculation) =============" << std::endl;
        std::cout << euler_mine(0) << ", " << euler_mine(1) << ", " << euler_mine(2) << std::endl;
        std::cout << "=== 3. euler angles from given rotation matrix (using eulerAngles()) ==============" << std::endl;
        std::cout << euler_t(0) << ", " << euler_t(1) << ", " << euler_t(2) << std::endl;
        std::cout << "=== 1. given rotation matrix ======================================================" << std::endl;
        std::cout << rot << std::endl;
        std::cout << "=== 2. rotation matrix from my calculation ========================================" << std::endl;
        std::cout << rot_from_euler_mine << std::endl;
        std::cout << "=== 3. rotation matrix from eulerAngles() =========================================" << std::endl;
        std::cout << rot_from_euler_t << std::endl;
        std::cout << "=== !! this should be an identity matrix ==========================================" << std::endl;
        std::cout << rot.inverse() * rot_from_euler_mine << std::endl;
        std::cout << "=== !! this should be an identity matrix too ======================================" << std::endl;
        std::cout << rot.inverse() * rot_from_euler_t << std::endl;
        std::cout << std::endl;
    }
    return 0;
}

This is result I've got from the code: 1

What did I do wrong? How do I get the correct euler angles using eulerAngles() method and get the identity matrix from last result?

Solution

  • you do your calculations in order of y,p,r so you need to call eulerAngles like this since you are rotating first axis 2

    Eigen::Vector3d euler_t = rot.eulerAngles(2, 1, 0);

    you can edit the code like this and the results will be correct

    Eigen::Matrix3d rot_from_euler_t;
Eigen::Vector3d euler_t = rot.eulerAngles(2, 1, 0);

{
    Eigen::AngleAxisd Y(euler_t(0), Eigen::Vector3d::UnitZ());
    Eigen::AngleAxisd P(euler_t(1), Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd R(euler_t(2), Eigen::Vector3d::UnitX());
    rot_from_euler_t = Eigen::Quaterniond(Y*P*R).toRotationMatrix();
}

std::cout << "=== 1. given euler angles =========================================================" << std::endl;
std::cout << euler(0) << ", " << euler(1) << ", " << euler(2) << std::endl;
std::cout << "=== 2. euler angles from given rotation matrix (using eulerAngles()) ==============" << std::endl;
std::cout << euler_t(2) << ", " << euler_t(1) << ", " << euler_t(0) << std::endl;
std::cout << "=== 3. euler angles from given rotation matrix (using my calculation) =============" << std::endl;
std::cout << euler_mine(0) << ", " << euler_mine(1) << ", " << euler_mine(2) << std::endl;
std::cout << "=== 1. given rotation matrix ======================================================" << std::endl;
std::cout << rot << std::endl;
std::cout << "=== 2. rotation matrix from my calculation ========================================" << std::endl;
std::cout << rot_from_euler_mine << std::endl;
std::cout << "=== 3. rotation matrix from eulerAngles() =========================================" << std::endl;
std::cout << rot_from_euler_t << std::endl;
std::cout << "=== !! this should be an identity matrix ==========================================" << std::endl;
std::cout << rot.inverse() * rot_from_euler_mine << std::endl;
std::cout << "=== !! this should be an identity matrix too ======================================" << std::endl;
std::cout << rot.inverse() * rot_from_euler_t << std::endl;
std::cout << std::endl;

    now sample results are like that

    === 1. given euler angles =========================================================
2.30282, -1.34473, 0.988813
=== 2. euler angles from given rotation matrix (using eulerAngles()) ==============
2.30282, -1.34473, 0.988813
=== 3. euler angles from given rotation matrix (using my calculation) =============
2.30282, -1.34473, 0.988813
=== 1. given rotation matrix ======================================================
 0.123211  0.159882  0.979417
 0.187249 -0.972954  0.135271
 0.974555  0.166728 -0.149816
=== 2. rotation matrix from my calculation ========================================
 0.123211  0.159882  0.979417
 0.187249 -0.972954  0.135271
 0.974555  0.166728 -0.149816
=== 3. rotation matrix from eulerAngles() =========================================
 0.123211  0.159882  0.979417
 0.187249 -0.972954  0.135271
 0.974555  0.166728 -0.149816
=== !! this should be an identity matrix ==========================================
           1 -1.66533e-16  1.11022e-16
 1.11022e-16            1  7.45931e-16
-2.91434e-16 -7.77156e-16            1
=== !! this should be an identity matrix too ======================================
           1 -1.94289e-16 -4.16334e-16
 8.32667e-17            1   1.9082e-16
-3.05311e-16 -3.33067e-16            1

    Hope this helps have a good day