SLAM : 旋转向量和旋转矩阵的互转换
1.c++
处理矩阵三维转换时，通常采用旋转矩阵，但是旋转变换其实只有三个自由度，用旋转向量表达时更为简洁。因此，需要实现从旋转向量和旋转矩阵之间的互转换。
旋转向量和旋转矩阵之间可以通过罗德里格斯公式进行转换：
代码实现：
可以直接采用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），输入数组对输出数组的偏导数。
2.python
# -*- coding: UTF-8 -*-
import os
import cv2
import numpy as np
T = np.zeros((1,3), np.float32)
a = (0.2,0.4,0.8)
print a
R = cv2.Rodrigues(a);
print R[0]
v3 = (R[0][2,1],R[0][0,2],R[0][1,0])
print v3
c = cv2.Rodrigues(v3)
print c[0]
b = cv2.Rodrigues(R[0])
print b[0][0]