python实现四元数和旋转矩阵之间的相互转换

四元数（quaternion）和旋转矩阵（rotation matrix）之间的相互转换在计算机图形学和三维计算中非常常见。让我们来探讨一下这两者之间的关系。

1. 四元数到旋转矩阵的转换：

   • 从四元数到旋转矩阵的转换可以通过以下步骤完成：
     1. 假设我们有一个四元数 ，其中 是实部， 是虚部。
     2. 构建旋转矩阵 ，其元素为：
        
     3. 这个旋转矩阵描述了四元数所表示的旋转。

2. 旋转矩阵到四元数的转换：

   • 从旋转矩阵到四元数的转换需要解决一个方程组。假设我们有一个旋转矩阵 。
   • 首先，计算旋转矩阵的迹（trace）：。
   • 然后，计算四元数的各个分量：
     • 
     • 
     • 
     • 

这些转换方法可以帮助你在四元数和旋转矩阵之间进行无缝切换。请注意，实际应用中，你可能需要考虑数值稳定性和精度问题。

代码实现：

from scipy.spatial.transform import Rotation as R 

# 旋转矩阵转换为四元数 
def rot2quaternion(rotation_matrix):
    r3 = R.from_matrix(rotation_matrix)
    qua = r3.as_quat()
    return qua 


# 四元数转旋转矩阵
def quaternion2rot(quaternion):
    r = R.from_quat(quaternion)
    rot = r.as_matrix()
    return rot