自己的 eulerAngles() returns 不正确的值
eigen eulerAngles() returns incorrect values
我尝试从本征 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() 方法获得正确的欧拉角并从上次结果中获得单位矩阵？
您按照 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
希望这有助于度过愉快的一天
我尝试从本征 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() 方法获得正确的欧拉角并从上次结果中获得单位矩阵？
您按照 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
希望这有助于度过愉快的一天