I have the following code
Eigen::AngleAxisd z(0, Eigen::Vector3d::UnitZ());
Eigen::AngleAxisd pan(2 / 180. * std::acos(-1), Eigen::Vector3d::UnitY());
Eigen::AngleAxisd tilt(3 / 180. * std::acos(-1), Eigen::Vector3d::UnitX());
auto q = z * pan * tilt;
auto euler = q.toRotationMatrix().eulerAngles(2, 1, 0);
auto a = euler[1] * 180 / std::acos(-1);
auto b = euler[2] * 180 / std::acos(-1);
The output is a = 178, b = -177
. Why this happens? how I got the actual values (2,3).
BTW, if I changed the input angle to (2,5), I got the exact correct result(2, 5). It seems random for me. Any hint?
Edit
Using this code from Wikipedia giving me the results I am expecting:
std::vector<T> angles(3);
// roll (x-axis rotation)
auto sinr_cosp = 2 * (q.w() * q.x() + q.y() * q.z());
double cosr_cosp = 1 - 2 * (q.x() * q.x() + q.y() * q.y());
angles[1] = std::atan2(sinr_cosp, cosr_cosp);
// pitch (y-axis rotation)
double sinp = 2 * (q.w() * q.y() - q.z() * q.x());
if (std::abs(sinp) >= 1)
angles[0] = std::copysign(std::acos(-1) / 2, sinp); // use 90 degrees if out of range
else
angles[0] = std::asin(sinp);
// yaw (z-axis rotation)
double siny_cosp = 2 * (q.w() * q.z() + q.x() * q.y());
double cosy_cosp = 1 - 2 * (q.y() * q.y() + q.z() * q.z());
angles[2] = std::atan2(siny_cosp, cosy_cosp);
return angles;