rodrigues Numpy 实现

import numpy as npr = np.array([[1.6674245], [0.50620349], [-0.4608852]])
# r = np.array([-2.100418, -2.167796, 0.273330]) # 不能是这种形式
sita = np.sqrt(np.square(r).sum())
r = r / sita
rx = r[0][0]
ry = r[1][0]
rz = r[2][0]
K = np.array([[0, -rz, ry],[rz, 0, -rx],[-ry, rx, 0]])
I = np.identity(3)
R = np.cos(sita) * I + (1 - np.cos(sita)) * r * r.T + np.sin(sita) * K
print(R)# 这里sita只是为了验证， 实际要自己通过R求解才能得到k = (R-R.T)/2/np.sin(sita)*sita
print(k)

结果和cv.rodrigues()是一样的