eigen eulerAngles() returns incorrect values

Gil-Dong Hong

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?

barkın evgin

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

Collected from the Internet

Please contact [email protected] to delete if infringement.

edited at
0

Comments

0 comments
Login to comment

Related

Dictionary of Objects returns incorrect values

OpenGL - glReadPixels returns incorrect values

checkPrime function returns incorrect values

Quaternion to Rotation Matrix, incorrect values using Eigen Library

numpy.vectorize returns incorrect values

Android MediaPlayer.getCurrentPosition() returns incorrect values

EWS FindItem call returns incorrect EffectiveRights values

Javascript date calculation returns incorrect values

Function returns incorrect False (only at higher values)

Execution of Stored Procedure in .NET application using SQLQuery returns incorrect values

Adding numbers within a range of root values returns incorrect value

Reading an array from a binary file in c returns incorrect values

Newtonsoft.Json returns incorrect values from file (with DeserializeObject)

CMake find eigen incorrect results

Redshift returns a []uint8 instead of an integer, converting between them returns incorrect values

Why does Scipy sparse implementation returns incorrect number of singular values although dense one returns correct?

Sorting eigen values and corresponding eigen vectors

Eigen LLT Module Giving incorrect result?

toLocaleString() returns incorrect time

`cublasIsamin` returns an incorrect value

getApplicationLabel returns incorrect label

Regex returns with incorrect value

SSIS returns an incorrect error

AccessibleObjectFromPoint() returns an incorrect object

getMonth returns incorrect value

Incorrect value returns

Reading columns out of order returns incorrect values (SQL Server ODBC driver)

Applying custom function to data.table by row returns incorrect amount of values

issue in getting unsorted eigen values