视觉SLAM十四讲--罗德里格斯公式(Rodrigues’s Formula)推导

前言

之前看了高博的《视觉SLAM十四讲》，里面有一段关于罗德里格斯公式，但是高博没有给出具体推导。然后我查了很多博主，都没有给出怎么推到
$$\mathbf{R}=\cos \theta \mathbf{I}+(1-\cos \theta )\mathbf{n}{\mathbf{n}}^T+\sin \theta {\mathbf{n}}^{\wedge }$$
怎么也没有弄懂 $\cos \theta \mathbf{I}$ 是怎么来的。
因此我查了查维基给出的定义，然后自己再推一遍。
PS：潜水csdn多年，学到了不少东西，这篇是我的第一篇博客，分享些我自己的学习心得，如有错误的地方，请大家指正，谢谢大家。

旋转向量

旋转向量的概念

任意旋转都可以用一个旋转轴和一个旋转角来表示。

假设有一个旋转轴为$\mathbf{n}$，角度为$\theta$ 的旋转，则对应的旋转向量为：
$$\theta \mathbf{n}$$

旋转矩阵和旋转向量的关系

对于旋转矩阵$\mathbf{R}$ 和旋转向量$\theta \mathbf{n}$， 他们的关系有：
$$\mathbf{R}=\cos \theta \mathbf{I}+(1-\cos \theta )\mathbf{n}{\mathbf{n}}^T+\sin \theta {\mathbf{n}}^{\wedge }$$

符号$\wedge$是向量的反对称的转换符号：
$$\mathbf{n}=\left[\begin{array}{c}a\\ b\\ c\end{array}\right],则{\mathbf{n}}^{\wedge }=\left[\begin{array}{ccc}0& -c& b\\ c& 0& -a\\ -b& a& 0\end{array}\right]$$

罗德里格斯公式推导

假设$\mathbf{v}$在三维空间中，绕单位旋转轴 $\mathbf{k}$，旋转$\theta$。

从上图可以看出，对于向量$\mathbf{v}$ 可以分出两个分向量：
$$\mathbf{v}={\mathbf{v}}_{\Vert }+{\mathbf{v}}_{\perp }$$

其中 ${\mathbf{v}}_{\Vert }$平行于旋转轴$\mathbf{k}$:
$${\mathbf{v}}_{\Vert }=(\mathbf{v}\cdot \mathbf{k})\mathbf{k}$$

${\mathbf{v}}_{\perp }$垂直于旋转轴$\mathbf{k}$:
$${\mathbf{v}}_{\perp }=\mathbf{v}-{\mathbf{v}}_{\Vert }=\mathbf{v}-(\mathbf{k}\cdot \mathbf{v})\mathbf{k}=-\mathbf{k}\times (\mathbf{k}\times \mathbf{v})$$
在这里我们也可以得到：
$$\mathbf{v}=(\mathbf{k}\cdot \mathbf{v})\mathbf{k}-\mathbf{k}\times (\mathbf{k}\times \mathbf{v})$$

向量 $\mathbf{k}\times \mathbf{v}$可以看作 ${\mathbf{v}}_{\perp }$ 绕$\mathbf{k}$ 反向旋转 $90^{\circ }$。

叉乘公式和点乘公式转换：
$$\mathbf{a}\times (\mathbf{b}\times \mathbf{c})=(\mathbf{a}\cdot \mathbf{c})\mathbf{b}-(\mathbf{a}\cdot \mathbf{b})\mathbf{c}$$

${\mathbf{v}}_{\Vert }$旋转后的为：
$${\mathbf{v}}_{\Vert \text{rot }}={\mathbf{v}}_{\Vert }$$

${\mathbf{v}}_{\perp }$旋转后为：
$$\begin{array}{rl}\left|{\mathbf{v}}_{\perp rot}\right|& =\left|{\mathbf{v}}_{\perp }\right|\\ {\mathbf{v}}_{\perp rot}& =\cos \theta {\mathbf{v}}_{\perp }+\sin \theta \mathbf{k}\times {\mathbf{v}}_{\perp }\end{array}$$

其中：
$$\mathbf{k}\times {\mathbf{v}}_{\perp }=\mathbf{k}\times \left(\mathbf{v}-{\mathbf{v}}_{\Vert }\right)=\mathbf{k}\times \mathbf{v}-\mathbf{k}\times {\mathbf{v}}_{\Vert }=\mathbf{k}\times \mathbf{v}$$

因此：
$${\mathbf{v}}_{\perp rot}=\cos \theta {\mathbf{v}}_{\perp }+\sin \theta \mathbf{k}\times \mathbf{v}$$

旋转后的向量${\mathbf{v}}_{rot}$为：
$${\mathbf{v}}_{\mathrm{r}\mathrm{o}\mathrm{t}}={\mathbf{v}}_{\Vert \mathrm{r}\mathrm{o}\mathrm{t}}+{\mathbf{v}}_{\perp \mathrm{r}\mathrm{o}\mathrm{t}}$$
$$\begin{array}{rl}{\mathbf{v}}_{rot}& ={\mathbf{v}}_{\Vert }+\cos \theta {\mathbf{v}}_{\perp }+\sin \theta \mathbf{k}\times \mathbf{v}\\ & ={\mathbf{v}}_{\Vert }+\cos \theta \left(\mathbf{v}-{\mathbf{v}}_{\Vert }\right)+\sin \theta \mathbf{k}\times \mathbf{v}\\ & =\cos \theta \mathbf{v}+(1-\cos \theta ){\mathbf{v}}_{\Vert }+\sin \theta \mathbf{k}\times \mathbf{v}\\ & =\cos \theta \mathbf{v}+(1-\cos \theta )(\mathbf{k}\cdot \mathbf{v})\mathbf{k}+\sin \theta \mathbf{k}\times \mathbf{v}\end{array}$$

其中：
$$\begin{array}{rlrl}(\mathbf{k}\cdot \mathbf{v})\mathbf{k}& =\mathbf{k}(\mathbf{k}\cdot \mathbf{v})& =\mathbf{k}({\mathbf{k}}^{\mathbf{T}}\mathbf{v})& =(\mathbf{k}{\mathbf{k}}^{\mathbf{T}})\mathbf{v}\end{array}$$
所以 ${\mathbf{v}}_{rot}=(\cos \theta \mathbf{I}+(1-\cos \theta )\mathbf{k}{\mathbf{k}}^{\mathbf{T}}+\sin \theta {\mathbf{k}}^{\wedge })\mathbf{v}$

注意：
如果在
$${\mathbf{v}}_{rot}={\mathbf{v}}_{\Vert }+\cos \theta {\mathbf{v}}_{\perp }+\sin \theta \mathbf{k}\times \mathbf{v}$$
中将${\mathbf{v}}_{\Vert }$ 替换为 $\mathbf{v}-{\mathbf{v}}_{\perp }$ ，再将 ${\mathbf{v}}_{\perp }$ 替换为$-\mathbf{k}\times (\mathbf{k}\times \mathbf{v})$则：
$$\begin{array}{rl}{\mathbf{v}}_{rot}& =\mathbf{v}-{\mathbf{v}}_{\perp }+\cos \theta {\mathbf{v}}_{\perp }+\sin \theta \mathbf{k}\times \mathbf{v}\\ & =\mathbf{v}+\mathbf{k}\times (\mathbf{k}\times \mathbf{v})-\cos \theta \mathbf{k}\times (\mathbf{k}\times \mathbf{v})+\sin \theta \mathbf{k}\times \mathbf{v}\\ & =\mathbf{v}+(1-\cos \theta )\mathbf{k}\times (\mathbf{k}\times \mathbf{v})+\sin \theta \mathbf{k}\times \mathbf{v}\\ & =\mathbf{v}+(1-\cos \theta ){\mathbf{K}}^2\mathbf{v}+\sin \theta \mathbf{k}\times \mathbf{v}\\ & =(\mathbf{I}+(1-\cos \theta ){\mathbf{K}}^2+\sin \theta \mathbf{K})\mathbf{v}\end{array}$$

$\mathbf{K}={\mathbf{k}}^{\wedge }$ 为 $\mathbf{k}$ 的反对称矩阵。

从上述两种推导，我们可以得出罗德里格斯公式
$$\begin{array}{rl}\mathbf{R}& =\cos \theta \mathbf{I}+(1-\cos \theta )\mathbf{k}{\mathbf{k}}^{\mathbf{T}}+\sin \theta {\mathbf{k}}^{\wedge }\\ & =\mathbf{I}+(1-\cos \theta ){\mathbf{K}}^2+\sin \theta \mathbf{K}\end{array}$$

从上式子中，我们也可以得到：
$$\begin{array}{r}\mathbf{k}{\mathbf{k}}^{\mathbf{T}}={\mathbf{K}}^2+\mathbf{I}\end{array}$$
这一步的推导，可以回到上面公式
$$\mathbf{v}=(\mathbf{k}\cdot \mathbf{v})\mathbf{k}-\mathbf{k}\times (\mathbf{k}\times \mathbf{v})$$
和
$$\begin{array}{rlrl}(\mathbf{k}\cdot \mathbf{v})\mathbf{k}& =\mathbf{k}(\mathbf{k}\cdot \mathbf{v})& =\mathbf{k}({\mathbf{k}}^{\mathbf{T}}\mathbf{v})& =(\mathbf{k}{\mathbf{k}}^{\mathbf{T}})\mathbf{v}\end{array}$$
得到。