欧拉到四元数/四元数到欧拉使用特征
Euler to Quaternion / Quaternion to Euler using Eigen
我正在尝试实现一种功能,可以将欧拉角转换为四元数,并使用Eigen返回"YXZ"惯例。稍后,这应该用于让用户为您提供欧拉角,并以四元数形式旋转并为用户转换回来。事实上,我的数学很差,但我已经尽力了。我不知道这个矩阵是否正确。代码工作,但我的结果是方式off,我想。你知道我哪里拐错弯了吗?这是我的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;
}
输入: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: -6.84901, z:-21.8771忽略前缀,x值似乎是正确的,但Y和z是错误的。
从欧拉到四元数:
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;
从四元数到欧拉:
auto euler = q.toRotationMatrix().eulerAngles(0, 1, 2);
std::cout << "Euler from quaternion in roll, pitch, yaw"<< std::endl << euler << std::endl;
摘自https://eigen.tuxfamily.org/dox/classEigen_1_1AngleAxis.html
这里有一个方法(未测试):
Vector3d euler = quaternion.toRotationMatrix().eulerAngles(2, 1, 0);
yaw = euler[0]; pitch = euler[1]; roll = euler[2];
四元数到欧拉的解决方案不适合我,所以我研究和修改了代码,现在它为我的目的工作:
Vector3f ToEulerAngles(const Eigen::Quaternionf& q) {
Vector3f angles; //yaw pitch roll
const auto x = q.x();
const auto y = q.y();
const auto z = q.z();
const auto w = q.w();
// roll (x-axis rotation)
double sinr_cosp = 2 * (w * x + y * z);
double cosr_cosp = 1 - 2 * (x * x + y * y);
angles[2] = std::atan2(sinr_cosp, cosr_cosp);
// pitch (y-axis rotation)
double sinp = 2 * (w * y - z * x);
if (std::abs(sinp) >= 1)
angles[1] = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range
else
angles[1] = std::asin(sinp);
// yaw (z-axis rotation)
double siny_cosp = 2 * (w * z + x * y);
double cosy_cosp = 1 - 2 * (y * y + z * z);
angles[0] = std::atan2(siny_cosp, cosy_cosp);
return angles;
}
我受到这个wiki条目的启发,并对这里提出的解决方案进行了一些基准测试。查看wiki:https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
当我使用
auto euler = q.toRotationMatrix()。eulerAngles(0, 1, 2)
它不可能一直完美工作,欧拉角总是有规律的跳动(实际值与计算值有±π的偏差)。例如,通过rqt读取和显示偏航角。
我对此一无所知,但我发现ros tf::getYaw()也可以实现"四元数到欧拉"(因为我只需要偏航角)。
没有Eigen(以防万一),我这样做了:
tf2::Matrix3x3 ( quat ) . getEulerYPR( &roll, &pitch, &yaw );
// and
tf2::Matrix3x3 ( quat ) . getRPY( &roll, &pitch, &yaw );
但是,这些只能给出24种可能配置中的两种。
- 单元测试欧拉到四元数实现失败
- 零四元数和任何向量都不为零的特征积,这是一个错误吗?
- 将四元数旋转的游戏对象旋转另一个四元数时出现问题
- 在OpenGL(GLM/C++)中使用四元数旋转时出现问题
- 想要将 CGAL 与四元数相结合是否合乎逻辑
- Eigen::Quaternion FromTwoVectors() 不返回有效的四元数
- C++四元数方程程序
- 四元数旋转不起作用
- 四元数和翻译
- 使用四元数防止绕某个轴旋转
- 如何使用四元数来描述超过360度的旋转角度?
- 四元数旋转错误
- 为什么我简单的四元数乘法比SSE快
- 特征:将矩阵3d旋转转换为四元数
- 从四元数查看矩阵
- 用四元数绕轴旋转矢量
- 基于四元数的相机不需要的卷轴
- 特征c++ / Matlab四元数与旋转矩阵不匹配
- 如何将特征::四元数转换为<float>矩阵4f?
- 欧拉到四元数/四元数到欧拉使用特征