python 实现四元数转旋转矩阵 源代码

以下是利用Python实现四元数转旋转矩阵的源代码：

import math
import numpy as np

def quaternion_to_rotation_matrix(q):
    """
    Convert quaternion to rotation matrix
    :param q: Quaternion in form of list or numpy array [q0, q1, q2, q3]
    :return: 3x3 Rotation matrix
    """
    q0, q1, q2, q3 = q
    R = np.array([[1-2*q2**2-2*q3**2, 2*q1*q2-2*q0*q3, 2*q1*q3+2*q0*q2],
                  [2*q1*q2+2*q0*q3, 1-2*q1**2-2*q3**2, 2*q2*q3-2*q0*q1],
                  [2*q1*q3-2*q0*q2, 2*q2*q3+2*q0*q1, 1-2*q1**2-2*q2**2]])
    return R

希望能够帮助到你！