C++ Euler to Quaternion / Quaternion to Euler using Eigen
声明：本页面是StackOverFlow热门问题的中英对照翻译，遵循CC BY-SA 4.0协议，如果您需要使用它，必须同样遵循CC BY-SA许可，注明原文地址和作者信息，同时你必须将它归于原作者（不是我）：StackOverFlow
原文地址: http://stackoverflow.com/questions/31589901/
Warning: these are provided under cc-by-sa 4.0 license. You are free to use/share it, But you must attribute it to the original authors (not me):
StackOverFlow
Euler to Quaternion / Quaternion to Euler using Eigen
提问by Little-God
I'm trying to implement a functionality that can convert an Euler angle into an Quaternion and back "YXZ"-convention using Eigen. Later this should be used to let the user give you Euler angles and rotate around as Quaternion and convert Back for the user. In fact i am realy bad at math but tried my best. I have no Idea if this matrices are correct or anything. The code Works, but my results are way to off, i suppose. Any idea where i take the wrong turn? This is what my Quat.cpp looks like:
我正在尝试实现一种功能，该功能可以将欧拉角转换为四元数并使用 Eigen 支持“YXZ”约定。稍后这应该用于让用户为您提供欧拉角并作为四元数旋转并为用户转换返回。事实上，我的数学真的很差，但我已经尽力了。我不知道这个矩阵是否正确或任何东西。代码有效，但我想我的结果很糟糕。知道我在哪里走错了吗？这是我的 Quat.cpp 的样子：
#include "Quat.h"
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <cmath>
#include <iostream>
using namespace Eigen;
Vector3f Quat::MyRotation(const Vector3f YPR)
{
Matrix3f matYaw(3, 3), matRoll(3, 3), matPitch(3, 3), matRotation(3, 3);
const auto yaw = YPR[2]*M_PI / 180;
const auto pitch = YPR[0]*M_PI / 180;
const auto roll = YPR[1]*M_PI / 180;
matYaw << cos(yaw), sin(yaw), 0.0f,
-sin(yaw), cos(yaw), 0.0f, //z
0.0f, 0.0f, 1.0f;
matPitch << cos(pitch), 0.0f, -sin(pitch),
0.0f, 1.0f, 0.0f, // X
sin(pitch), 0.0f, cos(pitch);
matRoll << 1.0f, 0.0f, 0.0f,
0.0f, cos(roll), sin(roll), // Y
0.0f, -sin(roll), cos(roll);
matRotation = matYaw*matPitch*matRoll;
Quaternionf quatFromRot(matRotation);
quatFromRot.normalize(); //Do i need to do this?
return Quat::toYawPitchRoll(quatFromRot);
}
Vector3f Quat::toYawPitchRoll(const Eigen::Quaternionf& q)
{
Vector3f retVector;
const auto x = q.y();
const auto y = q.z();
const auto z = q.x();
const auto w = q.w();
retVector[2] = atan2(2.0 * (y * z + w * x), w * w - x * x - y * y + z * z);
retVector[1] = asin(-2.0 * (x * z - w * y));
retVector[0] = atan2(2.0 * (x * y + w * z), w * w + x * x - y * y - z * z);
#if 1
retVector[0] = (retVector[0] * (180 / M_PI));
retVector[1] = (retVector[1] * (180 / M_PI))*-1;
retVector[2] = retVector[2] * (180 / M_PI);
#endif
return retVector;
}
Input: x = 55.0, y = 80.0, z = 12.0 Quaternion: w:0.872274, x: -0.140211, y:0.447012, z:-0.140211 Return Value: x:-55.5925, y: -6.84901, z:-21.8771 The X-Value seems about right disregarding the prefix, but Y and z are off.
输入：x = 55.0, y = 80.0, z = 12.0 四元数：w:0.872274, x: -0.140211, y:0.447012, z:-0.140211 返回值：x:-55.5925, y: -12.8.7忽略前缀，X 值似乎是正确的，但 Y 和 z 关闭。
回答by 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
摘自https://eigen.tuxfamily.org/dox/classEigen_1_1AngleAxis.html
回答by Shital Shah
Here's one approach (not tested):
这是一种方法（未测试）：
Vector3d euler = quaternion.toRotationMatrix().eulerAngles(2, 1, 0);
yaw = euler[0]; pitch = euler[1]; roll = euler[2];
回答by JKTesla
When I use
当我使用
auto euler = q.toRotationMatrix().eulerAngles(0, 1, 2)
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.
不可能一直完美运行，欧拉角总是有规律的节拍（实际值和计算值有±π的偏差）。例如，通过 rqt图片读取和显示偏航角 。
I have no idea about this, but I find ros tf::getYaw()also can achieve "Quaternion to Euler" (because I just need yaw angle).
我对此一无所知，但我发现 ros tf::getYaw()也可以实现“四元数到欧拉”（因为我只需要偏航角）。
