旋转向量和旋转矩阵的互转换

处理矩阵三维转换时，通常采用旋转矩阵，但是旋转变换其实只有三个自由度，用旋转向量表达时更为简洁。因此，需要实现从旋转向量和旋转矩阵之间的互转换。
旋转向量和旋转矩阵之间可以通过罗德里格斯公式进行转换：

将旋转向量转换为旋转矩阵：

$${R\bf}=\cos\theta{I\bf}+(1-\cos\theta){n\bf}{n\bf^T}+\sin\theta{n\bf}^\wedge$$
将旋转矩阵转换为旋转向量：
$$tr({R\bf})=\cos{\theta}tr({I\bf})+(1-\cos\theta)tr({n\bf}{n\bf}^T)+\sin{\theta}tr({n\bf}^\wedge)$$
因此： $$\theta=arccos(\frac{tr({R\bf}-1)}{2})$$

代码实现：
可以直接采用opencv中的Rodrigues函数实现，函数原型：

void Rodrigues( InputArray src, OutputArray dst, OutputArray jacobian = noArray() );

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