旋转矩阵、四元数和欧拉角之间互相转换代码
python3代码：
import numpy as np import math import os def rotation2quaternion(M): tr = np.trace(M) m = M.reshape(-1) if tr > 0: s = np.sqrt(tr + 1.0) * 2 w = 0.25 * s x = (m[7] - m[5]) / s y = (m[2] - m[6]) / s z = (m[3] - m[1]) / s elif m[0] > m[4] and m[0] > m[8]: s = np.sqrt(1.0 + m[0] - m[4] - m[8]) * 2 w = (m[7] - m[5]) / s x = 0.25 * s y = (m[1] + m[3]) / s z = (m[2] + m[6]) / s elif m[4] > m[8]: s = np.sqrt(1.0 + m[4] - m[0] - m[8]) * 2 w = (m[2] - m[6]) / s x = (m[1] + m[3]) / s y = 0.25 * s z = (m[5] + m[7]) / s else: s = np.sqrt(1.0 + m[8] - m[0] - m[4]) * 2 w = (m[3] - m[1]) / s x = (m[2] + m[6]) / s y = (m[5] + m[7]) / s z = 0.25 * s Q = np.array([w, x, y, z]).reshape(-1) return Q def quaternion2rotation(quat): # quat:(w,x,y,z) assert (len(quat) == 4) # normalize first quat = quat / np.linalg.norm(quat) a, b, c, d = quat a2 = a * a b2 = b * b c2 = c * c d2 = d * d ab = a * b ac = a * c ad = a * d bc = b * c bd = b * d cd = c * d # s = a2 + b2 + c2 + d2 m0 = a2 + b2 - c2 - d2 m1 = 2 * (bc - ad) m2 = 2 * (bd + ac) m3 = 2 * (bc + ad) m4 = a2 - b2 + c2 - d2 m5 = 2 * (cd - ab) m6 = 2 * (bd - ac) m7 = 2 * (cd + ab) m8 = a2 - b2 - c2 + d2 return np.array([m0, m1, m2, m3, m4, m5, m6, m7, m8]).reshape(3, 3) def rotationMatrixToEulerAngles(R) : # 旋转矩阵到欧拉角(x,y,z) # 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]) def eulerAnglesToRotationMatrix(angles1) : theta = np.zeros((3, 1), dtype=np.float64) theta[0] = angles1[0]*3.141592653589793/180.0 theta[1] = angles1[1]*3.141592653589793/180.0 theta[2] = angles1[2]*3.141592653589793/180.0 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
