python 四元數q轉旋轉矩陣R 已驗證

注意:符號相反的四元數,代表同乙個旋轉 (q = -q )

使用公式: 

使用庫scipy中的方法 rm = r.from_quat(rq)

from scipy.spatial.transform import rotation as r

rq=[-0.35, 1.23e-06, 4.18e-08, 0.39]
rm = r.from_quat(rq)
rotation_matrix = rm.as_matrix()
print('rotation:\n',rotation_matrix)
#結果:
rotation:
[[ 1.00000000e+00 -3.25420248e-06  3.38725419e-06]
[-3.01673707e-06  1.07793154e-01  9.94173343e-01]
[-3.60036417e-06 -9.94173343e-01  1.07793154e-01]]

下面兩個轉換待驗證

參考:

# calculates rotation matrix given euler angles.

def euleranglestorotationmatrix(theta) :
r_x = np.array([[1,         0,                  0                   ],
[0,         math.cos(theta[0]), -math.sin(theta[0]) ],
[0,         math.sin(theta[0]), math.cos(theta[0])  ]
])r_y = np.array([[math.cos(theta[1]),    0,      math.sin(theta[1])  ],
[0,                     1,      0                   ],
[-math.sin(theta[1]),   0,      math.cos(theta[1])  ]
])r_z = np.array([[math.cos(theta[2]),    -math.sin(theta[2]),    0],
[math.sin(theta[2]),    math.cos(theta[2]),     0],
[0,                     0,                      1]
])               
r = np.dot(r_z, np.dot( r_y, r_x ))
return r

# checks if a matrix is a valid rotation matrix.

def isrotationmatrix(r) :
rt = np.transpose(r)
shouldbeidentity = np.dot(rt, r)
i = np.identity(3, dtype = r.dtype)
n = np.linalg.norm(i - shouldbeidentity)
return n < 1e-6
# calculates rotation matrix to euler angles
# the result is the same as matlab except the order
def rotationmatrixtoeulerangles(r) :
assert(isrotationmatrix(r)) 
sy = math.sqrt(r[0,0] * r[0,0] +  r[1,0] * r[1,0]) 
singular = sy < 1e-6
if  not singular :
x = math.atan2(r[2,1] , r[2,2])
y = math.atan2(-r[2,0], sy)
z = math.atan2(r[1,0], r[0,0])
else :
x = math.atan2(-r[1,2], r[1,1])
y = math.atan2(-r[2,0], sy)
z = 0
return np.array([x, y, z])

四元數和旋轉矩陣

quaternion 四元數 quaternion 的定義 四元數一般定義如下 q w xi yj zk 其中 w,x,y,z是實數。同時，有 i i 1 j j 1 k k 1 四元數也可以表示為 q w,v 其中v x,y,z 是向量，w是標量，雖然v是向量，但不能簡單的理解為3d空間的向量，它...

方向向量轉尤拉角 旋轉矩陣 四元數

基於eigen庫實現演算法中的矩陣運算。輸入 方向向量vector3d tmpvec，參考單位向量vector3d zaxis 0，0，1 輸出 旋轉矩陣m eigen matrix3d m eigen vector3d zaxis 0,0,1 eigen vector3d tmpvec vx,vy...

旋轉矩陣 尤拉角 四元數比較

旋轉矩陣 尤拉角 四元數主要用於 向量的旋轉 座標系之間的轉換 角位移計算 方位的平滑插值計算。旋轉矩陣 尤拉角 四元數比較 不同的方位表示方法適用於不同的情況。下面是我們對合理選擇格式的一些建議 1.尤拉角最容易使用。當需要為世界中的物體指定方位時,尤拉角能大大的簡化人機互動,包括直接的鍵盤輸入方...