Euler to Quaternion / Quaternion to Euler using Eigen

六眼飞鱼酱① 提交于 2019-12-03 03:39:35
Ross

From Euler to Quaternion:

using namespace Eigen;
//Roll pitch and yaw in Radians
float roll = 1.5707, pitch = 0, yaw = 0.707;    
Quaternionf q;
q = AngleAxisf(roll, Vector3f::UnitX())
    * AngleAxisf(pitch, Vector3f::UnitY())
    * AngleAxisf(yaw, Vector3f::UnitZ());
std::cout << "Quaternion" << std::endl << q.coeffs() << std::endl;

From Quaternion to Euler:

auto euler = q.toRotationMatrix().eulerAngles(0, 1, 2);
std::cout << "Euler from quaternion in roll, pitch, yaw"<< std::endl << euler << std::endl;

Taken from https://eigen.tuxfamily.org/dox/classEigen_1_1AngleAxis.html

Here's one approach (not tested):

  Vector3d euler = quaternion.toRotationMatrix().eulerAngles(2, 1, 0);
  yaw = euler[0]; pitch = euler[1]; roll = euler[2];

When I use

auto euler = q.toRotationMatrix().eulerAngles(0, 1, 2)

It can not work perfectly all the time, the euler angle always has a regular beat (the actual value and the calculated value have a deviation of ±π). For example, read and show yaw angle by rqt picture.

I have no idea about this, but I find ros tf::getYaw() also can achieve "Quaternion to Euler" (because I just need yaw angle).

易学教程内所有资源均来自网络或用户发布的内容,如有违反法律规定的内容欢迎反馈
该文章没有解决你所遇到的问题?点击提问,说说你的问题,让更多的人一起探讨吧!