三维坐标旋转变换——四元数与旋转矩阵的相互转换，python实现

0.公式参考

• 参考公式

1.代码

• 导包

import pandas as pd
import numpy as np

• 旋转矩阵转为四元数（输入为numpy数组）

def matrix2quaternion(m):
    #m:array
    w = ((np.trace(m) + 1) ** 0.5) / 2
    x = (m[2][1] - m[1][2]) / (4 * w)
    y = (m[0][2] - m[2][0]) / (4 * w)
    z = (m[1][0] - m[0][1]) / (4 * w)
    return w,x,y,z

• 四元数转为旋转矩阵（输入为一位列表或数组）

def quaternion2matrix(q):
    #q:list
    w,x,y,z = q
    return np.array([[1-2*y*y-2*z*z, 2*x*y-2*z*w, 2*x*z+2*y*w],
             [2*x*y+2*z*w, 1-2*x*x-2*z*z, 2*y*z-2*x*w],
             [2*x*z-2*y*w, 2*y*z+2*x*w, 1-2*x*x-2*y*y]])