旋轉角生成與對應旋轉矩陣的互轉函式numpy實現
阿新 • • 發佈:2021-10-20
角度->矩陣
由分別依次按照X/Y/Z三個座標軸進行旋轉角度\(\theta_x\), \(\theta_y\),\(\theta_z\),得到對應的旋轉變換矩陣
import numpy as np def GetRotationMatrix(theta_x, theta_y, theta_z): sx = np.sin(theta_x) cx = np.cos(theta_x) sy = np.sin(theta_y) cy = np.cos(theta_y) sz = np.sin(theta_z) cz = np.cos(theta_z) return np.array([ [cy*cz, cz*sx*sy-cx*sz, sx*sz+cx*cz*sy], [cy*sz, cx*cz+sx*sy*sz, cx*sy*sz-cz*sz], [-sy, cy*sx, cx*cy]])
矩陣->角度
上述過程的逆向:
def GetRotationAngles(rot_mat):
theta_x = np.arctan2(rot_mat[2,1], rot_mat[2,2])
theta_y = np.arctan2(-rot_mat[2,0], \
np.sqrt(rot_mat[2,1]*rot_mat[2,1]+rot_mat[2,2]*rot_mat[2,2]))
theta_z = np.arctan2(rot_mat[1,0], rot_mat[0,0])
return theta_x, theta_y, theta_z
測試案例
>>> theta = (0.1, 0.2, -0.3) >>> tr = GetRotationMatrix(*theta) >>> tr array([[ 0.93629336, 0.31299183, 0.15934508], [-0.28962948, 0.94470249, 0.22390374], [-0.19866933, 0.0978434 , 0.97517033]]) >>> tr array([[ 0.93629336, 0.31299183, 0.15934508], [-0.28962948, 0.94470249, 0.22390374], [-0.19866933, 0.0978434 , 0.97517033]]) >>> a,b,c = GetRotationAngles(tr) >>> a 0.09999999999999999 >>> b 0.19999999999999998 >>> c -0.3