python將四元數變換為旋轉矩陣的例項
阿新 • • 發佈:2020-01-09
如下所示:
import numpy as np from autolab_core import RigidTransform # 寫上用四元數表示的orientation和xyz表示的position orientation = {'y': -0.6971278819736084,'x': -0.716556549511624,'z': -0.010016582945017661,'w': 0.02142651612120239} position = {'y': -0.26022684372145516,'x': 0.6453529828252734,'z': 1.179122068068349} rotation_quaternion = np.asarray([orientation['w'],orientation['x'],orientation['y'],orientation['z']]) translation = np.asarray([position['x'],position['y'],position['z']]) # 這裡用的是UC Berkeley的autolab_core,比較方便吧,當然可以自己寫一個fuction來計算,計算公式在https://www.cnblogs.com/flyinggod/p/8144100.html T_qua2rota = RigidTransform(rotation_quaternion,translation) print(T_qua2rota) # 以下是列印的結果 Tra: [ 0.64535298 -0.26022684 1.17912207] Rot: [[ 0.02782477 0.99949234 -0.01551915] [ 0.99863386 -0.02710724 0.0446723 ] [ 0.04422894 -0.01674094 -0.99888114]] Qtn: [-0.02142652 0.71655655 0.69712788 0.01001658] from unassigned to world
自己寫的話
def quaternion_to_rotation_matrix(quat): q = quat.copy() n = np.dot(q,q) if n < np.finfo(q.dtype).eps: return np.identity(4) q = q * np.sqrt(2.0 / n) q = np.outer(q,q) rot_matrix = np.array( [[1.0 - q[2,2] - q[3,3],q[1,2] + q[3,0],3] - q[2,0.0],[q[1,1.0 - q[1,1] - q[3,q[2,3] + q[1,3] + q[2,3] - q[1,1] - q[2,2],[0.0,0.0,1.0]],dtype=q.dtype) return rot_matrix
描述有兩種方式,即XYZABC和XYZ+quaternion:
https://doc.rc-visard.com/latest/de/pose_formats.html?highlight=format
以上這篇python將四元數變換為旋轉矩陣的例項就是小編分享給大家的全部內容了,希望能給大家一個參考,也希望大家多多支援我們。