在 3D 空间中，旋转是 3 自由度的，刚体变换是 6 自由度的（3自由度旋转+3自由度平移）。
3x3 的旋转矩阵有 9 个量，表达了 3自由度的旋转。 旋转矩阵是有 自约束的，即一个旋转矩阵是一个 正交矩阵，而且行列式为1。
如果使用 有 4x4（或4x3）个分量 的 变换矩阵 来表示 刚体变换，那么相当于用 16 个量（12 个量）来表达 6 自由度，存在很大的信息冗余。
旋转矩阵
信息冗余，有自约束：正交约束，不适合插值
轴角/角轴/旋转向量 Axis-Angle
任意旋转都可以用一个 旋转 轴 和 一个 旋转角来刻画。
于是可以用一个向量，方向与旋转轴 一致，模长等于 旋转角，这种向量称之为旋转向量或轴角或角轴。
旋转向量跟 旋转矩阵，可以由 Rodrigues’s Formula 相互转换。
角轴缺点：旋转轴 v 和角度 theta，与旋转轴 -v，旋转角度 theta±2kpi是一样的。不连续。
欧拉角
把一个旋转分解成 3 次绕不同轴的旋转，使用 3 个分离的转角。
XYZ, ZYZ,ZYX， yaw-pitch-roll，在特定领域，会定义欧拉角的过程。
欧拉角重大缺点是 万向锁问题，在 中间旋转，比如 pitch 的旋转度数 为 90°时，会出现roll 的轴跟 yaw 的轴重叠，这样会缺少一个旋转自由度。
关于 万向锁讲解的视频：https://www.youtube.com/watch?v=vLDn-ZITDgA&t=2s
四元数
既是紧凑的，也没有奇异性。
三维旋转可以由 单位四元数描述。
空间中的一个 三维点 用虚四元数 p 描述，令一个由单位四元数指定的 旋转为 q
那么 p 经过旋转之后 得到的 p’
p’ = qpq^(-1)
q = cos( theta/2 ) +sin( theta/2 )( ui+ vj + w*k )
四元数可以和 旋转矩阵和旋转向量 相互转换。
四元数缺点： q 跟 -q 表示相同的 3D 旋转（四元数是三维旋转的 double cover”指的是：在四元数的单位球面上，每一个物理旋转对应两点 q 和 −q。这二者在几何效果上无差别，但在数值应用和插值时需要留意）
验证代码1，转化成旋转矩阵
import numpy as np
from scipy.spatial.transform import Rotation as R
def main():
# 1. 随机生成一个旋转，并获取相应的四元数
# Scipy 的 from_rotvec 可以通过轴-角表示生成一个旋转（此处随机 轴*角）
random_axis_angle = np.random.randn(3) # 随机生成一个三维向量
rot = R.from_rotvec(random_axis_angle) # 以随机轴向量的范数作为旋转角度
# 从 Rotation 对象获取四元数，scipy 默认顺序为 [x, y, z, w]
q_scipy = rot.as_quat()
print("原四元数 (x, y, z, w):", q_scipy)
# 2. 构造 -q: 只要把四元数取反即可
# 注意这里依旧要保持同样的顺序 (x, y, z, w)
neg_q_scipy = -q_scipy
print("相反四元数 (x, y, z, w):", neg_q_scipy)
# 3. 使用两个 Rotation 对象：q 和 -q
# 分别对同一个向量进行旋转，看结果是否一致
# 注意：Rotation.from_quat() 的输入必须是 [x, y, z, w]
rot_q = R.from_quat(q_scipy)
rot_neg_q = R.from_quat(neg_q_scipy)
# 4. 测试向量，这里随便选一个或多个
test_vector = np.array([-1.0, 2.5, 3.7])
rotated_by_q = rot_q.apply(test_vector)
rotated_by_neg_q = rot_neg_q.apply(test_vector)
print("向量:", test_vector)
print("使用 q 旋转后的结果:", rotated_by_q)
print("使用 -q 旋转后的结果:", rotated_by_neg_q)
# 5. 验证是否近似相等
# 若 q 和 -q 代表同一个 3D 旋转，则结果应当一致（允许一定数值误差）
if np.allclose(rotated_by_q, rotated_by_neg_q, atol=1e-7):
print("结论: q 和 -q 对该向量的旋转结果一致，代表同一个旋转。")
else:
print("结论: 结果不一致，需检查实现。")
if __name__ == "__main__":
main()
验证代码2，直接用 qpq^(-1)
#python -m pip install --upgrade --force-reinstall numpy-quaternion
import numpy as np
import quaternion # 来自 numpy-quaternion 库
def quater_rotate_vector(q, v):
"""
用单位四元数 q 旋转三维向量 v (np.array)，
计算方式：q * (0, v) * q.inverse()
返回旋转后的向量 (np.array)。
"""
# 将 v 嵌入成为 '纯四元数' (0, vx, vy, vz)
p = np.quaternion(0, v[0], v[1], v[2])
# 旋转: v' = q * p * q.inverse()
rotated = q * p * q.inverse()
# 提取旋转结果的 (x, y, z) 部分
return np.array([rotated.x, rotated.y, rotated.z])
def main():
# 1. 随机生成一个旋转轴
axis = np.random.randn(3)
axis /= np.linalg.norm(axis) # 归一化
# 随机生成一个旋转角度
angle = np.random.rand() * 2.0 * np.pi
# 2. 构造对应的单位四元数 q
half_angle = angle / 2.0
qw = np.cos(half_angle)
q_xyz = axis * np.sin(half_angle)
q = np.quaternion(qw, q_xyz[0], q_xyz[1], q_xyz[2])
# 3. 构造 -q
neg_q = -q
# 4. 随机生成一个 3D 向量 p
p = np.random.randn(3)
# 5. 分别用 q 和 -q 旋转向量 p
r1 = quater_rotate_vector(q, p)
r2 = quater_rotate_vector(neg_q, p)
print("随机单位四元数 q = ", q)
print("相反四元数 -q = ", neg_q)
print("待旋转向量 p = ", p)
print("\n用 q 旋转 p 的结果 =", r1)
print("用 -q 旋转 p 的结果 =", r2)
# 6. 验证是否近似相等
# 对单位四元数而言，q 和 -q 代表同一个旋转，结果应基本相同
if np.allclose(r1, r2, atol=1e-7):
print("\n结论: q 和 -q 对该向量的旋转结果一致，代表同一个旋转。")
else:
print("\n结论: 结果不一致，需要检查。")
if __name__ == "__main__":
main()
介绍四元数 与 3D rotation的 可交互网站： 超级推荐！
甚至可以固定 q 只变化 q^(-1)，反之亦然
https://eater.net/quaternions
https://eater.net/quaternions/video/intro
https://www.youtube.com/watch?v=d4EgbgTm0Bg
https://www.youtube.com/watch?v=zjMuIxRvygQ
