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

前言

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

旋转向量

旋转向量的概念

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

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

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

对于旋转矩阵 R \boldsymbol{R} R 和旋转向量 θ n \theta \boldsymbol{n} θn，他们的关系有：
R = cos ⁡ θ I + ( 1 − cos ⁡ θ ) n n T + sin ⁡ θ 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 ∧是向量的反对称的转换符号：
n = [ a b c ] , 则 n ∧ = [ 0 − c b c 0 − a − b a 0 ] \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−c0ab−a0⎦⎤

罗德里格斯公式推导

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

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

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

v ⊥ \mathbf{v}_{\perp} v⊥垂直于旋转轴 k \mathbf{k} k:
v ⊥ = v − v ∥ = v − ( k ⋅ v ) k = − k × ( k × v ) \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)
在这里我们也可以得到：
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)

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

叉乘公式和点乘公式转换：
a × ( b × c ) = ( a ⋅ c ) b − ( a ⋅ b ) c \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

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

v ⊥ \mathbf{v}_{\perp} v⊥旋转后为：
∣ v ⊥ r o t ∣ = ∣ v ⊥ ∣ v ⊥ r o t = cos ⁡ θ v ⊥ + sin ⁡ θ k × 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⊥

其中：
k × v ⊥ = k × ( v − v ∥ ) = k × v − k × v ∥ = 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

因此：
v ⊥ r o t = cos ⁡ θ v ⊥ + sin ⁡ θ 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

旋转后的向量 v r o t \mathbf{v}_{rot} vrot为：
v r o t = v ∥ r o t + v ⊥ r o t \mathbf{v}_{\mathrm{rot}}=\mathbf{v}_{\| \mathrm{rot}}+\mathbf{v}_{\perp \mathrm{rot}} vrot=v∥rot+v⊥rot
v r o t = 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{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

其中：
( k ⋅ v ) k = k ( k ⋅ v ) = k ( k T v ) = ( k k T ) 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
所以 v r o t = ( cos ⁡ θ I + ( 1 − cos ⁡ θ ) k k T + sin ⁡ θ k ∧ ) 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

注意：
如果在
v r o t = v ∥ + cos ⁡ θ v ⊥ + 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
中将 v ∥ \mathbf{v}_{\|} v∥ 替换为 v − v ⊥ \mathbf{v}- \mathbf{v}_{\perp} v−v⊥ ，再将 v ⊥ \mathbf{v}_{\perp} v⊥ 替换为 − k × ( k × v ) -\mathbf{k} \times(\mathbf{k} \times \mathbf{v}) −k×(k×v)则：
v r o t = 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 ⁡ θ ) K 2 v + sin ⁡ θ k × v = ( I + ( 1 − cos ⁡ θ ) K 2 + sin ⁡ θ 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

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

从上述两种推导，我们可以得出罗德里格斯公式
R = cos ⁡ θ I + ( 1 − cos ⁡ θ ) k k T + sin ⁡ θ k ∧ = I + ( 1 − cos ⁡ θ ) K 2 + sin ⁡ θ 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

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