罗德里格斯（Rodrigues）旋转向量与矩阵的变换

1.旋转的表示

处理三维旋转问题时，通常采用旋转矩阵的方式来描述旋转变换。旋转矩阵有以下两种方式得到。
物体在三维空间中的旋转，可以被分为解为在直接坐标系下，分别先后围绕x,y,z坐标轴旋转得到。旋转的角度也就是我们常听到的角度roll，pitch，yew。如果已知这几个角度，就可以直接通过每一步的矩阵相乘得到整个旋转矩阵。

                          R=R(yaw)R(pitch)R(roll)

除了旋转矩阵的方式，还有欧拉角、四元数、轴角等方式，下面详细介绍轴角方式。
旋转矩阵还可以理解为围绕空间中某一个向量，直接一次旋转某一个角度得到。在openCV相机标定时得到的旋转向量就是用这种方式。即由旋转变量来描述。
$$(\mathrm{a}\mathrm{x}\mathrm{i}\mathrm{s},\mathrm{a}\mathrm{n}\mathrm{g}\mathrm{l}\mathrm{e})=\left(\left[\begin{array}{c}{e}_x\\ e_y\\ e_z\end{array}\right],\theta \right)=\left(\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right],\frac{\pi }{2}\right)=\left[\begin{array}{c}0\\ 0\\ \frac{\pi }{2}\end{array}\right].$$

注意，axis矩阵必须是单位矩阵（若不是，通过除以模长单位化）

2.旋转向量得到旋转矩阵

旋转向量的长度（模）表示绕轴逆时针旋转的角度（弧度）。旋转向量与旋转矩阵可以通过罗德里格斯（Rodrigues）变换进行转换。

3.根据两个旋转向量求旋转矩阵

（1）旋转角度
已知旋转前向量为P, 旋转后变为Q。由点积定义可知：

可推出P，Q之间的夹角为：

（2）旋转轴
旋转角所在的平面为有P和Q所构成的平面，那么旋转轴必垂直该平面。

   假定旋转前向量为a(a1, a2, a3)， 旋转后向量为b（b1, b2, b3)。由叉乘定义得：

所以旋转轴c(c1, c2, c3)为：

4.OpenCV实现Rodrigues变换的函数为

int cvRodrigues2( const CvMat* src, CvMat* dst, CvMat* jacobian=0 );
src为输入的旋转向量（3x1或者1x3）或者旋转矩阵（3x3）。
dst为输出的旋转矩阵（3x3）或者旋转向量（3x1或者1x3）。
jacobian为可选的输出雅可比矩阵（3x9或者9x3），是输入与输出数组的偏导数。

https://blog.csdn.net/wangyang20170901/article/details/78800540