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]])