罗德里格斯公式Rodrigues：
代码：
import numpy as np
def 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) * M
return R
def rodrigues_rotation_vec_to_R(v):
# r旋转向量[3x1]
theta = np.linalg.norm(v)
r = np.array(v).reshape(3, 1) / theta
return 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]])
