旋转角生成与对应旋转矩阵的互转函数numpy实现

角度->矩阵

由分别依次按照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