以下是使用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中转换四元数、欧拉角和旋转平移矩阵之间的互换。
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’。您可以根据实际情况选择不同的顺序。
