c++ - Eigen eulerAngles() 返回不正确的值

Question

我尝试从 Eigen 3x3 旋转矩阵中提取欧拉角。但是，我从 eulerAngles() 方法获得的值似乎不正确。我写了一个小测试代码，但得到了奇怪的结果。代码如下。

#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;
}

这是我从代码中得到的结果:

我做错了什么？如何使用 eulerAngles() 方法获得正确的欧拉角并从上次结果中获得单位矩阵？

Answer 1

您按 y、p、r 的顺序进行计算，因此您需要像这样调用 eulerAngles，因为您正在旋转第一个轴 2

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

你可以像这样编辑代码，结果是正确的

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;

现在样本结果是这样的

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

希望这有助于度过美好的一天