更多转换见:
基于eigen实现
基于python实现
这里就记录下顺序:
运算是左乘,下面方式记作Z-Y-X
顺序,先绕x轴roll,再绕y轴pitch,最后绕z轴yaw,即为RPY
c++版本:
// 转欧拉角
Eigen::Quaternionf q_;
q_ = Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ()) *
Eigen::AngleAxisf(pitch, Eigen::Vector3f::UnitY()) *
Eigen::AngleAxisf(roll, Eigen::Vector3f::UnitX());
// 转四元数
// euler_angles存储顺序是:yaw(z) pitch(y) roll(x)
Eigen::Matrix3d rotation_matrix
Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0);
Eigen的角度范围有点问题,
void get_euler_from_R(Vector<3> &e, const Matrix<3,3> &R) {
float phi = atan2(R(2, 1), R(2, 2));
float theta = asin(-R(2, 0));
float psi = atan2(R(1, 0), R(0, 0));
float pi = M_PI;
if (fabs(theta - pi/2.0f) < 1.0e-3) {
phi = 0.0f;
psi = atan2(R(1, 2), R(0, 2));
} else if (fabs(theta + pi/2.0f) < 1.0e-3) {
phi = 0.0f;
psi = atan2(-R(1, 2), -R(0, 2));
}
e(0) = phi;
e(1) = theta;
e(2) = psi;
}
推导:假设欧拉角yaw、pitch、roll的角度为alpha, beta, gamma,则对应先roll,后ptich,最后yaw的旋转可以计算如下:
python版本:
import numpy as np
import math
from scipy.spatial.transform import Rotation as R
Rotation = R.from_quat([x,y,z,w])
euler_angles = Rotation.as_euler('zyx', degrees=False)
Rotation = R.from_euler('zxy', [yaw, pitch, roll], degrees=False)
rotation_matrix = Rotation.as_matrix()
quaternion = Rotation.as_quat()
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)