Table of Contents
0、简介
一、四元数的定义
二、欧拉角到四元数的转换
2.1 公式:
2.2 code:
三、四元数到欧拉角的转换
3.1 公式
3.2 code:
3.3 四元素到旋转矩阵转换
四. 奇点
五. 矢量旋转
证明:
六 . 其他参考
七 python 四元素欧拉角互相转换
8 Eigen transform
欧拉角到四元素
四元素得到yaw
四元素到旋转向量
旋转轴向量到四元素
四元数与欧拉角之间的转换
百度百科四元素
在3D图形学中,最常用的旋转表示方法便是四元数和欧拉角,比起矩阵来具有节省存储空间和方便插值的优点。
本文主要归纳了两种表达方式的转换,计算公式采用3D笛卡尔坐标系:
定义,,分别为绕Z轴、Y轴、X轴的旋转角度,如果用Tait-Bryan angle表示,分别为Yaw、Pitch、Roll。
利用欧拉角也可以实现一个物体在空间的旋转,它按照既定的顺序,如依次绕z,y,x分别旋转一个固定角度,使用roll,yaw ,pitch分别表示物体绕,x,y,z的旋转角度,记为,,,可以利用三个四元数依次表示这三次旋转,即:
struct Quaternion
{
double w, x, y, z;
};
Quaternion ToQuaternion(double yaw, double pitch, double roll) // yaw (Z), pitch (Y), roll (X)
{
// Abbreviations for the various angular functions
double cy = cos(yaw * 0.5);
double sy = sin(yaw * 0.5);
double cp = cos(pitch * 0.5);
double sp = sin(pitch * 0.5);
double cr = cos(roll * 0.5);
double sr = sin(roll * 0.5);
Quaternion q;
q.w = cy * cp * cr + sy * sp * sr;
q.x = cy * cp * sr - sy * sp * cr;
q.y = sy * cp * sr + cy * sp * cr;
q.z = sy * cp * cr - cy * sp * sr;
return q;
}
可以从四元数通过以下关系式获得欧拉角:
#define _USE_MATH_DEFINES
#include
struct Quaternion {
double w, x, y, z;
};
struct EulerAngles {
double roll, pitch, yaw;
};
EulerAngles ToEulerAngles(Quaternion q) {
EulerAngles angles;
// roll (x-axis rotation)
double 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.roll = 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.pitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range
else
angles.pitch = 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.yaw = std::atan2(siny_cosp, cosy_cosp);
return angles;
}
或等效地,通过齐次表达式:
当螺距接近±90°(南北极)时,必须意识到欧拉角参数化的奇异性。这些情况必须特别处理。这种情况的通用名称是万向节锁。
处理奇异点的代码可从以下网站获取:www.euclideanspace.com
定义四元素的尺度 和向量 ,有
请注意,通过定义欧拉旋转的四元数来旋转三维矢量的规范方法是通过公式:
这儿:是包含嵌入向量的四元数,是共轭四元数,
在计算实现中,这需要两个四元数乘法。一种替代方法是应用一对关系:
:表示三维矢量叉积。这涉及较少的乘法,因此计算速度更快。数值测试表明,对于矢量旋转,后一种方法可能比原始方法快30%[4]。
标量和矢量部分的四元数乘法的一般规则由下式给出:
利用这种关系可以找到:
并替换为三乘积:
可得到:
在定义时,可以按标量和矢量部分来表示:
def EulerAndQuaternionTransform( intput_data):
data_len = len(intput_data)
angle_is_not_rad = False
if data_len == 3:
r = 0
p = 0
y = 0
if angle_is_not_rad: # 180 ->pi
r = math.radians(intput_data[0])
p = math.radians(intput_data[1])
y = math.radians(intput_data[2])
else:
r = intput_data[0]
p = intput_data[1]
y = intput_data[2]
sinp = math.sin(p/2)
siny = math.sin(y/2)
sinr = math.sin(r/2)
cosp = math.cos(p/2)
cosy = math.cos(y/2)
cosr = math.cos(r/2)
w = cosr*cosp*cosy + sinr*sinp*siny
x = sinr*cosp*cosy - cosr*sinp*siny
y = cosr*sinp*cosy + sinr*cosp*siny
z = cosr*cosp*siny - sinr*sinp*cosy
return {w,x,y,z}
elif data_len == 4:
w = intput_data[0]
x = intput_data[1]
y = intput_data[2]
z = intput_data[2]
r = math.atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y))
p = math.asin(2 * (w * y - z * x))
y = math.atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z))
if angle_is_not_rad ==False: # 180 ->pi
r = r / math.pi * 180
p = p / math.pi * 180
y = y / math.pi * 180
return {r,p,y}
Eigen::Quaterniond RollPitchYaw(const double roll, const double pitch,
const double yaw) {
const Eigen::AngleAxisd roll_angle(roll, Eigen::Vector3d::UnitX());
const Eigen::AngleAxisd pitch_angle(pitch, Eigen::Vector3d::UnitY());
const Eigen::AngleAxisd yaw_angle(yaw, Eigen::Vector3d::UnitZ());
return yaw_angle * pitch_angle * roll_angle;
}
template
T GetYaw(const Eigen::Quaternion& rotation) {
const Eigen::Matrix direction =
rotation * Eigen::Matrix::UnitX();
return atan2(direction.y(), direction.x());
}
template
Eigen::Matrix RotationQuaternionToAngleAxisVector(
const Eigen::Quaternion& quaternion) {
Eigen::Quaternion normalized_quaternion = quaternion.normalized();
// We choose the quaternion with positive 'w', i.e., the one with a smaller
// angle that represents this orientation.
if (normalized_quaternion.w() < 0.) {
// Multiply by -1. http://eigen.tuxfamily.org/bz/show_bug.cgi?id=560
normalized_quaternion.w() = -1. * normalized_quaternion.w();
normalized_quaternion.x() = -1. * normalized_quaternion.x();
normalized_quaternion.y() = -1. * normalized_quaternion.y();
normalized_quaternion.z() = -1. * normalized_quaternion.z();
}
// We convert the normalized_quaternion into a vector along the rotation axis
// with length of the rotation angle.
const T angle =
2. * atan2(normalized_quaternion.vec().norm(), normalized_quaternion.w());
constexpr double kCutoffAngle = 1e-7; // We linearize below this angle.
const T scale = angle < kCutoffAngle ? T(2.) : angle / sin(angle / 2.);
return Eigen::Matrix(scale * normalized_quaternion.x(),
scale * normalized_quaternion.y(),
scale * normalized_quaternion.z());
}
template
Eigen::Quaternion AngleAxisVectorToRotationQuaternion(
const Eigen::Matrix& angle_axis) {
T scale = T(0.5);
T w = T(1.);
constexpr double kCutoffAngle = 1e-8; // We linearize below this angle.
if (angle_axis.squaredNorm() > kCutoffAngle) {
const T norm = angle_axis.norm();
scale = sin(norm / 2.) / norm;
w = cos(norm / 2.);
}
const Eigen::Matrix quaternion_xyz = scale * angle_axis;
return Eigen::Quaternion(w, quaternion_xyz.x(), quaternion_xyz.y(),
quaternion_xyz.z());
}