1 罗德里格斯公式推导

• 参考：https://blog.csdn.net/qq_22235957/article/details/80461290 # 维基百科翻译
• 参考：https://blog.csdn.net/q583956932/article/details/78933245
• 参考：https://blog.csdn.net/weixin_41855010/article/details/108208331
• 参考：https://blog.csdn.net/qq_40475529/article/details/89409303

在三维空间中，旋转矩阵R 可以对坐标系（基向量组）进行刚性的旋转变换：

$$ R=\left[\begin{array}{lll} r_{x x} & r_{x y} & r_{x z} \\ r_{y x} & r_{y y} & r_{y z} \\ r_{z x} & r_{z y} & r_{z z} \end{array}\right] $$

2 cv2.Rodrigues进行旋转矩阵和旋转向量之间的相互转化

我们在使用retval, rvec, tvec = cv2.solvePnP(objp, imgp, K, D_0, flags=cv2.SOLVEPNP_ITERATIVE)会计算得到一个旋转向量rvec，但是我们通常需要把这个旋转向量转换为旋转矩阵。

而opencv内置的cv.Rodrigues()函数可以进行将：

• 旋转向量转换为旋转矩阵
• 或把旋转矩阵转换为旋转向量

1、Rodrigues(src[, dst[, jacobian]]) -> dst, jacobian

• 输入src：旋转向量（(3,1)或者(1,3)）或者旋转矩阵（3,3）；
• 输出dst：旋转矩阵（3,3）或者旋转向量（(3,1)或者(1,3)）；
• 输出jacobin：可选项，输出雅克比矩阵（3x9或者9x3），输入数组对输出数组的偏导数。

2、代码

import cv2
import numpy as np

extrinsic = np.array([[0.05812254, 0.9969995, 0.05112498, 0.043909],
                    [-0.02821786, -0.04955038, 0.99837293, -0.026862],
                    [0.99791058, -0.05947061, 0.02525319, -0.006717],
                    [0., 0., 0., 1.]])
rot_mat = extrinsic[:3, :3]
print(f"rot_mat:\n {rot_mat}")

# 把旋转矩阵转化为旋转向量
rvec, _ = cv2.Rodrigues(rot_mat)
print(f"rvec:\n {rvec}")

# 把旋转向量转换为旋转矩阵
rot_mat, _ = cv2.Rodrigues(rvec)
print(f"rot_mat:\n {rot_mat}")

输出结果如下：

rot_mat:
 [[ 0.05812254  0.9969995   0.05112498]
 [-0.02821786 -0.04955038  0.99837293]
 [ 0.99791058 -0.05947061  0.02525319]]
rvec:
 [[-1.25346463]
 [-1.12186936]
 [-1.21480507]]
rot_mat:
 [[ 0.05812254  0.9969995   0.05112498]
 [-0.02821786 -0.04955038  0.99837293]
 [ 0.99791058 -0.05947061  0.02525319]]

注意：

使用cv.Rodrigues把旋转矩阵转换为旋转向量，这个旋转向量并不是欧拉角，因此通过这个旋转矩阵和我们用欧拉角转换得到的旋转矩阵也是有区别的！！！！