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

前言

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

旋转向量

旋转向量的概念

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

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

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

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

符号$\wedge$∧是向量的反对称的转换符号：
$\boldsymbol{n} = \left[\begin{matrix} a\\ b\\ c\\ \end{matrix}\right], 则\boldsymbol{n}^{\wedge} = \begin{bmatrix} 0 & -c & b\\ c & 0 & -a\\ -b& a & 0 \end{bmatrix}$n=⎣⎡​abc​⎦⎤​,则n∧=⎣⎡​0c−b​−c0a​b−a0​⎦⎤​

罗德里格斯公式推导

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

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

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

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

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

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

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

$\mathbf{v}_{\perp}$v⊥​旋转后为：
$\begin{aligned} \left|\mathbf{v}_{\perp r o t}\right| &=\left|\mathbf{v}_{\perp}\right| \\ \mathbf{v}_{\perp r o t} &=\cos \theta \mathbf{v}_{\perp}+\sin \theta \mathbf{k} \times \mathbf{v}_{\perp} \end{aligned}$∣v⊥rot​∣v⊥rot​​=∣v⊥​∣=cosθv⊥​+sinθk×v⊥​​

其中：
$\mathbf{k} \times \mathbf{v}_{\perp}=\mathbf{k} \times\left(\mathbf{v}-\mathbf{v}_{\|}\right)=\mathbf{k} \times \mathbf{v}-\mathbf{k} \times \mathbf{v}_{\|}=\mathbf{k} \times \mathbf{v}$k×v⊥​=k×(v−v∥​)=k×v−k×v∥​=k×v

因此：
$\mathbf{v}_{\perp r o t}=\cos \theta \mathbf{v}_{\perp}+\sin \theta \mathbf{k} \times \mathbf{v}$v⊥rot​=cosθv⊥​+sinθk×v

旋转后的向量$\mathbf{v}_{rot}$vrot​为：
$\mathbf{v}_{\mathrm{rot}}=\mathbf{v}_{\| \mathrm{rot}}+\mathbf{v}_{\perp \mathrm{rot}}$vrot​=v∥rot​+v⊥rot​
$\begin{aligned} \mathbf{v}_{rot} &=\mathbf{v}_{\|}+\cos \theta \mathbf{v}_{\perp}+\sin \theta \mathbf{k} \times \mathbf{v} \\ &=\mathbf{v}_{\|}+\cos \theta\left(\mathbf{v}-\mathbf{v}_{\mathbf{\|}}\right)+\sin \theta \mathbf{k} \times \mathbf{v} \\ &=\cos \theta \mathbf{v}+(1-\cos \theta) \mathbf{v}_{\|}+\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{aligned}$vrot​​=v∥​+cosθv⊥​+sinθk×v=v∥​+cosθ(v−v∥​)+sinθk×v=cosθv+(1−cosθ)v∥​+sinθk×v=cosθv+(1−cosθ)(k⋅v)k+sinθk×v​

其中：
$\begin{aligned} (\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{aligned}$(k⋅v)k​=k(k⋅v)​=k(kTv)​=(kkT)v​
所以 $\mathbf{v}_{rot} = (\cos \theta \mathbf{I} +(1 - \cos \theta) \mathbf{k} \mathbf{k}^\mathbf{T} + \sin \theta \mathbf{k}^{\wedge}) \mathbf{v}$vrot​=(cosθI+(1−cosθ)kkT+sinθk∧)v

注意：
如果在
$\mathbf{v}_{rot}=\mathbf{v}_{\|}+\cos \theta \mathbf{v}_{\perp}+\sin \theta \mathbf{k} \times \mathbf{v}$vrot​=v∥​+cosθv⊥​+sinθk×v
中将$\mathbf{v}_{\|}$v∥​ 替换为 $\mathbf{v}- \mathbf{v}_{\perp}$v−v⊥​ ，再将 $\mathbf{v}_{\perp}$v⊥​ 替换为$-\mathbf{k} \times(\mathbf{k} \times \mathbf{v})$−k×(k×v)则：
$\begin{aligned} \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{aligned}$vrot​​=v−v⊥​+cosθv⊥​+sinθk×v=v+k×(k×v)−cosθk×(k×v)+sinθk×v=v+(1−cosθ)k×(k×v)+sinθk×v=v+(1−cosθ)K2v+sinθk×v=(I+(1−cosθ)K2+sinθK)v​

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

从上述两种推导，我们可以得出罗德里格斯公式
$\begin{aligned} \boldsymbol{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{aligned}$R​=cosθI+(1−cosθ)kkT+sinθk∧=I+(1−cosθ)K2+sinθK​

从上式子中，我们也可以得到：
$\begin{aligned} \mathbf{k} \mathbf{k}^\mathbf{T} = \mathbf{K}^2 + \mathbf{I} \end{aligned}$kkT=K2+I​
这一步的推导，可以回到上面公式
$\mathbf{v} = (\mathbf{k} \cdot \mathbf{v}) \mathbf{k} - \mathbf{k} \times(\mathbf{k} \times \mathbf{v})$v=(k⋅v)k−k×(k×v)
和
$\begin{aligned} (\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{aligned}$(k⋅v)k​=k(k⋅v)​=k(kTv)​=(kkT)v​
得到。