Rodrigues：旋转矩阵的向量表达

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

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

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

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

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

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

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

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

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

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

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

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