Rodrigues：旋转矩阵的向量表达

目的：最近看论文，遇到Rodrigues公式，一直没有怎么推导。因此自己推导一遍。

在理解Rodrigues公式，之前需要理解旋转向量的表达方式。
旋转矩阵的向量表达是基于欧拉定理推导的，它有三个参数。而它可以转化为旋转矩阵。这种转化为旋转矩阵的方式被称为Rodrigues公式。

Rotation Vectors
普通认知中，旋转矩阵是$3\times 3$的矩阵。这类矩阵有众多约束，具体哪些约束可以查阅资料（后续自己补上）。它只有三个自由度。只要知道那三个参数，剩下的6个参数能够计算，它的表达容易。在一般优化中，使用 Rotation matrix不方便，矩阵的计算复杂且耗时。

定义的来源：
在几何旋转中，任意一个旋转都可以表达为绕着一根轴旋转一定的角度。因此表达方式可以表示为:一根旋转的向量$\overrightarrow{u}$,和其旋转的角度$\theta$。因此它可以用4个参数来决定。
假设向量$\overrightarrow{r}$为主轴向量，它的旋转的向量从向量$\overrightarrow{p}$,旋转角度为$\theta$。如下图：

因为$\overrightarrow{o}$为原点。简化表达$\overrightarrow{op}=\overrightarrow{p}$,其它也使用类似的表达。

从图上可以看到，向量$\overrightarrow{p}$绕着轴$\overrightarrow{u}$旋转的时候，只和垂直于向量$\overrightarrow{u}$有关。因此按照向量$\overrightarrow{u}$把它拆分为$\overrightarrow{a}$,$\overrightarrow{b}$,它们分别平面于向量$\overrightarrow{u}$和垂直于向量$\overrightarrow{u}$。拆分了两个向量，需要计算两个向量。假设$\overrightarrow{u}$和$\overrightarrow{p}$的夹角为$\sigma$,因此可以得到
$$cos\sigma =\frac{\overrightarrow{u}\cdot \overrightarrow{p}}{|\overrightarrow{u}||\overrightarrow{p}|}(1)$$
因为$\overrightarrow{u}$为单位向量。因此可以得到
$$\overrightarrow{a}=\overrightarrow{u}|\overrightarrow{p}|cos\sigma =\frac{\overrightarrow{u}\cdot \overrightarrow{p}}{|\overrightarrow{u}||\overrightarrow{p}|}|\overrightarrow{p}|\overrightarrow{u}=(\overrightarrow{u}\cdot \overrightarrow{p})\overrightarrow{u}=\overrightarrow{u}(\overrightarrow{u}\cdot \overrightarrow{p})(2)$$
通过拆开和合并$(2)$写成向量的形式可以得到：
$$\overrightarrow{a}=\overrightarrow{u}(\overrightarrow{u}\cdot \overrightarrow{p})=\overrightarrow{u}{\overrightarrow{u}}^T\overrightarrow{p}(3)$$

对于垂直于向量$\overrightarrow{u}$的分量$\overrightarrow{b}$
$$\overrightarrow{b}=\overrightarrow{p}-\overrightarrow{a}=(1-\overrightarrow{u}{\overrightarrow{u}}^T)\overrightarrow{p}(4)$$

因为从向量$\overrightarrow{b}$旋转到向量$\overrightarrow{b^{\prime }}$,需要另一个向量$\overrightarrow{c}$，它是垂直于向量$\overrightarrow{u}$和向量$\overrightarrow{p}$所在的平面，且它的长度和向量$\overrightarrow{b}$一样。
满足：$|\overrightarrow{b}|=|\overrightarrow{c}|$
通过三角行垂直公式得到：
$$|\overrightarrow{b}|=|\overrightarrow{u}||\overrightarrow{p}|sin\sigma (5)$$
通过叉乘，可以设计向量$\overrightarrow{c}$:
$$\overrightarrow{c}=\overrightarrow{u}\times \overrightarrow{p}=(|\overrightarrow{u}||\overrightarrow{p}|sin\sigma )\frac{\overrightarrow{c}}{|\overrightarrow{c}|}=(|\overrightarrow{u}||\overrightarrow{p}|sin\sigma )\overrightarrow{n}(6)$$
其中$\overrightarrow{n}=\frac{\overrightarrow{c}}{|\overrightarrow{c}|}$单位向量。因此$|\overrightarrow{c}|=|\overrightarrow{u}||\overrightarrow{p}|sin\sigma$
通过计算它们的模，发现它们是相等的。
上面的公式$(6)$，将在下一篇blog介绍。后面更新它们的链接

因此可以得到$\overrightarrow{p}$旋转后得到的向量$\overrightarrow{p^{\prime }}$为：
$$\overrightarrow{b^{\prime }}=\overrightarrow{b}cos\theta +\overrightarrow{c}sin\theta (7)$$
然后通过向量加法
$$\begin{gathered} \overrightarrow{p^{\prime }}=\overrightarrow{a}+\overrightarrow{b^{\prime }}=\overrightarrow{a}+\overrightarrow{b}cos\theta +\overrightarrow{c}sin\theta \\ =\overrightarrow{u}{\overrightarrow{u}}^T\overrightarrow{p}+(1-\overrightarrow{u}{\overrightarrow{u}}^T)\overrightarrow{p}cos\theta +(\overrightarrow{u}\times \overrightarrow{p})sin\theta \\ =[Icos\theta +(1-cos\theta )\overrightarrow{u}{\overrightarrow{u}}^T+\overrightarrow{u}\times sin\theta ]\overrightarrow{p}(8) \end{gathered}$$

通过公式$(8)$可以得到一个向量$\overrightarrow{p}$围绕向量$\overrightarrow{u}$得到旋转$\theta$新的向量$\overrightarrow{p^{\prime }}$的公式,其中$R=Icos\theta +(1-cos\theta )\overrightarrow{u}{\overrightarrow{u}}^T+\overrightarrow{u}\times sin\theta$。它也是Rodrigues公式。
还有一种特殊情况，如果$\overrightarrow{u}=[0,0,0{]}^T$,得到的旋转矩阵为$I$

上面的公式也可以反推（因为它不太常用），后面再补上。

参考资料为：
Vector Representation of rotations的论文。