python - 如何使用 Python 在不使用 OpenCV 的情况下将罗德里格斯向量转换为旋转矩阵

Question

我正在寻找相当于

的代码片段

import cv2

def rodrigues_vec_to_rotation_mat(rodrigues_vec): 
     return cv2.Rodrigues(rodrigues_vec)[0] 

无需导入/使用 OpenCV 库。

Answer 1

import math
import sys
import numpy as np

def rodrigues_vec_to_rotation_mat(rodrigues_vec):
    theta = np.linalg.norm(rodrigues_vec)
    if theta < sys.float_info.epsilon:              
        rotation_mat = np.eye(3, dtype=float)
    else:
        r = rodrigues_vec / theta
        I = np.eye(3, dtype=float)
        r_rT = np.array([
            [r[0]*r[0], r[0]*r[1], r[0]*r[2]],
            [r[1]*r[0], r[1]*r[1], r[1]*r[2]],
            [r[2]*r[0], r[2]*r[1], r[2]*r[2]]
        ])
        r_cross = np.array([
            [0, -r[2], r[1]],
            [r[2], 0, -r[0]],
            [-r[1], r[0], 0]
        ])
        rotation_mat = math.cos(theta) * I + (1 - math.cos(theta)) * r_rT + math.sin(theta) * r_cross
    return rotation_mat 

来源:
documentation Rodrigues() 函数的。
source code cvRodrigues2() 函数。