TLDR:
How to get Euler angles in RPY form from a rotation matrix using the Eigen library. I tried ChatGPT, Google search and the documentation. The following is supposed to be the way: mat.eulerAngles(0, 1, 2). But, this does not seem to work.
Given a rotation matrix I want to get the Euler angles in the "sxyz" form. In other words I am trying to get ax, ay, az for a given rotation matrix such the the rotation matrix can be obtained as Rx * Ry * Rz, where each Ri is rotation about the axis i by the angle ai.
I am using the C++ Eigen library version 3.7 for this.
I tried the following (please see code below) where I generate the matrix mat from Euler angles ax, ay, az by roll-pitch-yaw (RPY) rotation about static frame. I have verified that this matrix is correct. Following this I obtained the Euler angles (presumable in RPY format) from mat and used those euler angles to get mat2 by
RPY rotation about static frame. I expect these two matrices to be same. But they are very different. What is wrong here?
#include <iostream>
#include <eigen3/Eigen/Dense>
int main() {
// Initialize euler angles.
double ax, ay, az;
ax = M_PI / 2;
ay = M_PI / 2;
az = 0;
// Quaternion from Euler angles.
Eigen::Quaterniond q = Eigen::AngleAxisd(az, Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(ay, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(ax, Eigen::Vector3d::UnitX());
Eigen::Matrix3d mat = q.toRotationMatrix();
// Print rotation matrix to verify.
std::cout << mat << std::endl;
std::cout << "----" << std::endl;
// Get Eigen angles from rotation matrix.
Eigen::Vector3d ea = mat.eulerAngles(0, 1, 2);
// Print euler angles.
std::cout << ea << std::endl;
std::cout << "----" << std::endl;
Eigen::Quaterniond q2 = Eigen::AngleAxisd(ea(2), Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(ea(1), Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(ea(0), Eigen::Vector3d::UnitX());
Eigen::Matrix3d mat2 = q2.toRotationMatrix();
//
std::cout << mat2 << std::endl;
std::cout << "----" << std::endl;
}
The output is as follows:
2.22045e-16 1 2.22045e-16
0 2.22045e-16 -1
-1 2.22045e-16 0
----
1.5708
2.22045e-16
-1.5708
----
3.33067e-16 2.77556e-16 -1
-1 0 -2.77556e-16
-5.55112e-17 1 3.33067e-16
I think the Euler angles are obviously wrong. Also they do not generate the original rotation matrix. I have also tried alternatives like mat.eulerAngles(2, 1, 0). But none of them work.
Eigen::Vector3d ea = mat.eulerAngles(2, 1, 0);Eigen::Quaterniond q2 = Eigen::AngleAxisd(ea(0), Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(ea(1), Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(ea(2), Eigen::Vector3d::UnitX());#include <bits/stdc++.h> using namespace std;please remove this abomination from your code. stackoverflow.com/q/31816095/4165552 , stackoverflow.com/q/1452721/4165552