三维坐标旋转变换——四元数与旋转矩阵的相互转换,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]])

你可能感兴趣的:(python,矩阵,numpy,四元数,旋转矩阵)