python 实现罗德里格斯公式Rodrigues 旋转向量到旋转矩阵转化

罗德里格斯公式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]])