[英]opencv python - applying rotation matrix from rodrigues function
[英]How to Convert a Rodrigues Vector to a Rotation Matrix without OpenCV using Python
我正在寻找相当于的代码片段
import cv2
def rodrigues_vec_to_rotation_mat(rodrigues_vec):
return cv2.Rodrigues(rodrigues_vec)[0]
无需导入/使用 OpenCV 库。
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
资料来源:
Rodrigues()
function 的文档。
cvRodrigues2()
function 的源代码。
声明:本站的技术帖子网页,遵循CC BY-SA 4.0协议,如果您需要转载,请注明本站网址或者原文地址。任何问题请咨询:yoyou2525@163.com.