罗德里格斯公式Rodrigues：

代码：

import numpy as npdef rodrigues_rotation(r, theta):# n旋转轴[3x1]# theta为旋转角度# 旋转是过原点的，n是旋转轴r = np.array(r).reshape(3, 1)rx, ry, rz = r[:, 0]M = np.array([[0, -rz, ry],[rz, 0, -rx],[-ry, rx, 0]])R = np.eye(3)R[:3, :3] = np.cos(theta) * np.eye(3) + \(1 - np.cos(theta)) * r @ r.T + \np.sin(theta) * Mreturn Rdef rodrigues_rotation_vec_to_R(v):# r旋转向量[3x1]theta = np.linalg.norm(v)r = np.array(v).reshape(3, 1) / thetareturn rodrigues_rotation(r, theta)

案例：绕x轴旋转90度：

rodrigues_rotation_vec_to_R(np.asarray([1,0,0])*np.pi/2)

结果：

array([[ 1.000000e+00, 0.000000e+00, 0.000000e+00],[ 0.000000e+00, 6.123234e-17, -1.000000e+00],[ 0.000000e+00, 1.000000e+00, 6.123234e-17]])