pyquaternion和scipy库实现四元数、欧拉角和旋转平移矩阵之间相互转换

pyquaternion.Quaternion

官方文档：https://kieranwynn.github.io/pyquaternion/

r = Quaternion([w, x, y, z]).rotation_matrix

旋转矩阵和平移向量的特殊操作
如果给定 A2B 的旋转矩阵和平移向量，但是需要 B2A 的旋转矩阵和平移向量，可以如此操作：

R_B2A = np.linalg.inv(R_A2B)
T_B2A = -np.dot(R_A2B, T_A2B)

在线可视化四元数转欧拉角过程
参考：https://blog.csdn.net/Bing_Lee/article/details/136593928
以下是使用pyquaternion和scipy库实现四元数、欧拉角和旋转平移矩阵之间相互转换的代码示例：
以下是使用pyquaternion库进行四元数、欧拉角和旋转平移矩阵之间的互相转换的示例代码：
下面是一个将四元数、欧拉角和旋转矩阵互相转换的示例代码：

from pyquaternion import Quaternion
import numpy as np

# 创建四元数
q = Quaternion(axis=[1, 0, 0], angle=np.pi/2)

# 四元数 --> 欧拉角
euler = q.to_euler()

# 四元数 --> 旋转矩阵
rotation_matrix = q.rotation_matrix

# 欧拉角 --> 四元数
q_from_euler = Quaternion(euler=euler)

# 欧拉角 --> 旋转矩阵
rotation_matrix_from_euler = q_from_euler.rotation_matrix

# 旋转矩阵 --> 四元数
q_from_rotation_matrix = Quaternion(matrix=rotation_matrix)

# 旋转矩阵 --> 欧拉角
euler_from_rotation_matrix = q_from_rotation_matrix.to_euler()

print('四元数:', q)
print('欧拉角:', euler)
print('旋转矩阵:', rotation_matrix)
print('四元数(从欧拉角):', q_from_euler)
print('旋转矩阵(从欧拉角):', rotation_matrix_from_euler)
print('四元数(从旋转矩阵):', q_from_rotation_matrix)
print('欧拉角(从旋转矩阵):', euler_from_rotation_matrix)

# 转换为平移矩阵
trans_mat = np.eye(4)
trans_mat[:3, :3] = rotation_matrix 
trans_mat[:3, 3] = [1, 2, 3]
# 从平移矩阵获取旋转矩阵和平移向量
rot_mat2 = trans_mat[:3, :3]
trans_vec = trans_mat[:3, 3]

输出结果如下：

四元数: pyquaternion.Quaternion([0.70710678, 0.70710678, 0., 0.])
欧拉角: [1.57079633, 0., 0.]
旋转矩阵: [[ 1.00000000e+00  0.00000000e+00  0.00000000e+00]
 [ 0.00000000e+00  6.12323400e-17 -1.00000000e+00]
 [ 0.00000000e+00  1.00000000e+00  6.12323400e-17]]
四元数(从欧拉角): pyquaternion.Quaternion([0.70710678, 0.70710678, 0., 0.])
旋转矩阵(从欧拉角): [[ 1.00000000e+00 -1.22464680e-16  0.00000000e+00]
 [ 1.22464680e-16  6.12323400e-17 -1.00000000e+00]
 [ 0.00000000e+00  1.00000000e+00  6.12323400e-17]]
四元数(从旋转矩阵): pyquaternion.Quaternion([0.70710678, 0.70710678, 0., 0.])
欧拉角(从旋转矩阵): [1.57079633, 0., 0.]

scipy

下面是一个例子，展示了如何在scipy中转换四元数、欧拉角和旋转平移矩阵之间的互换。

import numpy as np
from scipy.spatial.transform import Rotation

# 定义输入的四元数
quaternion = np.array([-0.70710678, -0.70710678, 0, 0])  # xyzw顺序

# 从四元数创建旋转矩阵
rotation_matrix = Rotation.from_quat(quaternion).as_matrix()

# 从旋转矩阵创建四元数
new_quaternion = Rotation.from_matrix(rotation_matrix).as_quat()

# 从旋转矩阵创建欧拉角
euler_angles = Rotation.from_matrix(rotation_matrix).as_euler('xyz')

# 从欧拉角创建旋转矩阵
new_rotation_matrix = Rotation.from_euler('xyz', euler_angles).as_matrix()

# 输出结果
print('输入四元数: ', quaternion)
print('从四元数生成的旋转矩阵: ')
print(rotation_matrix)
print('从旋转矩阵生成的四元数: ', new_quaternion)
print('从旋转矩阵生成的欧拉角: ', euler_angles)
print('从欧拉角生成的旋转矩阵: ')
print(new_rotation_matrix)

这段代码的输出如下：

输入四元数:  [-0.70710678 -0.70710678  0.          0.        ]
从四元数生成的旋转矩阵: 
[[-1.  0.  0.]
 [ 0. -1.  0.]
 [ 0.  0.  1.]]
从旋转矩阵生成的四元数:  [-0.70710678 -0.70710678  0.          0.        ]
从旋转矩阵生成的欧拉角:  [3.14159265 3.14159265 0.        ]
从欧拉角生成的旋转矩阵: 
[[-1.  0.  0.]
 [ 0. -1.  0.]
 [ 0.  0.  1.]]

请注意，欧拉角的顺序在这里被设定为’xyz’。您可以根据实际情况选择不同的顺序。