【发布时间】：2019-01-10 09:10:01
【问题描述】：
我尝试从 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() 方法获得正确的欧拉角并从最后的结果中获取单位矩阵？
【问题讨论】：