罗德里格斯公式推导

一，基础准备
- 1. 旋转矩阵
- 2. 旋转向量
- 3. 向量叉积
二，公式推导
- 1.符号说明与图例
- 2. 公式推导
三，公式理解和深入
四，极限的方式简洁推导罗德里格斯公式

罗德里格斯公式，将3D旋转表达成了 ( n ∧ , θ ) (n^{\wedge}, \theta) (n∧,θ)的形式，一般记作 ω = θ ∗ n = ( ω x , ω y , ω z ) T \omega = \theta * n = \left(\omega_{x}, \omega_{y}, \omega_{z}\right)^{T} ω=θ∗n=(ωx,ωy,ωz)T
两种表达：
R = [ I + ( 1 − cos ⁡ ( θ ) ) ∗ N 2 + sin ⁡ ( θ ) ∗ N ] R = [I+(1-\cos (\theta)) * N^{2}+\sin (\theta) * N] R=[I+(1−cos(θ))∗N2+sin(θ)∗N]

R = cos ⁡ ( θ ) ∗ I + ( 1 − cos ⁡ ( θ ) ) ∗ n ∗ n T + sin ⁡ ( θ ) ∗ n R = \cos (\theta) * I+(1-\cos (\theta)) * n * n^{T}+\sin (\theta) * n R=cos(θ)∗I+(1−cos(θ))∗n∗nT+sin(θ)∗n

一，基础准备

1. 旋转矩阵

R = [ r x x r x y r x z r y x r y y r y z r z x r z y r z z ] R=\left[\begin{array}{lll}{r_{x x}} & {r_{x y}} & {r_{x z}} \\ {r_{y x}} & {r_{y y}} & {r_{y z}} \\ {r_{z x}} & {r_{z y}} & {r_{z z}}\end{array}\right] R=⎣⎡rxxryxrzxrxyryyrzyrxzryzrzz⎦⎤
R矩阵是标准正交矩阵，用于基向量之间的刚性变换。满足

R T R = R − 1 R = E R^{T} R=R^{-1} R=E RTR=R−1R=E
∣ R ∣ = 1 |R|=1 ∣R∣=1

2. 旋转向量

用一个单位向量n和旋转角度 θ \theta θ来表示旋转的过程。默认向量是列向量

n = [ n 1 n 2 n 3 ] n=\left[\begin{array}{l}{n_{1}} \\ {n_{2}} \\ {n_{3}}\end{array}\right] n=⎣⎡n1n2n3⎦⎤

n 1 2 + n 2 2 + n 3 2 = 1 n_{1}^{2}+n_{2}^{2}+n_{3}^{2}=1 n12+n22+n32=1

3. 向量叉积

图片来源:https://www.mathsisfun.com/algebra/vectors-cross-product.html
两个向量的叉积可以写成一个反对称矩阵与一个向量的点积
a = [ a 1 a 2 a 3 ] a=\left[\begin{array}{l}{a_{1}} \\ {a_{2}} \\ {a_{3}}\end{array}\right] a=⎣⎡a1a2a3⎦⎤

a ∧ = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] = A a^{\wedge}=\left[\begin{array}{ccc}{0} & {-a_{3}} & {a_{2}} \\ {a_{3}} & {0} & {-a_{1}} \\ {-a_{2}} & {a_{1}} & {0}\end{array}\right]=A a∧=⎣⎡0a3−a2−a30a1a2−a10⎦⎤=A

a × b = [ a 2 b 3 − a 3 b 2 − a 1 b 3 + a 3 b 1 a 1 b 2 − a 2 b 1 ] = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] ∗ [ b 1 b 2 b 3 ] = A ∗ a a \times b=\left[\begin{array}{c}{a_{2} b_{3}-a_{3} b_{2}} \\ {-a_{1} b_{3}+a_{3} b_{1}} \\ {a_{1} b_{2}-a_{2} b_{1}}\end{array}\right]=\left[\begin{array}{ccc}{0} & {-a_{3}} & {a_{2}} \\ {a_{3}} & {0} & {-a_{1}} \\ {-a_{2}} & {a_{1}} & {0}\end{array}\right] *\left[\begin{array}{l}{b_{1}} \\ {b_{2}} \\ {b_{3}}\end{array}\right]=A * a a×b=⎣⎡a2b3−a3b2−a1b3+a3b1a1b2−a2b1⎦⎤=⎣⎡0a3−a2−a30a1a2−a10⎦⎤∗⎣⎡b1b2b3⎦⎤=A∗a
所以两个向量叉积可以写成一个反对称矩阵与向量的点积乘积
a × b = a ∧ ∗ b a \times b=a^{\wedge} * b a×b=a∧∗b

二，公式推导

1.符号说明与图例

名称	符号
旋转向量	n n n
旋转向量的反对称矩阵形式	n ∧ o r N n^{\wedge} or N n∧orN
旋转角度	θ \theta θ
旋转点	P
OP向量	v v v
v旋转 θ \theta θ后的向量	v ′ v^{'} v′
v在旋转平面的投影	v ⊥ v_{\perp} v⊥
v在旋转轴的投影	v ∥ v_{\parallel} v∥
v旋转 θ \theta θ后旋转平面的分量	v ⊥ ′ v_{\perp}^{'} v⊥′
v旋转 θ \theta θ后在n的分量	v ∥ ′ v_{\parallel}^{'} v∥′
w向量，垂直与n向量与v向量的平面, w与n向量与v向量都正交
v方向的单位向量$\frac{v}{\left	v\right

2. 公式推导

空间中的一个点P，构成向量 v = O P → v=\overrightarrow{O P} v=OP ，则分解后有

v = v ⊥ + v ∥ v = v_{\perp} + v_{\parallel} v=v⊥+v∥
同理旋转后的向量 v ′ = v ⊥ ′ + v ∥ ′ v' = v_{\perp}^{'} + v_{\parallel}^{'} v′=v⊥′+v∥′
显然 v ∥ = v ∥ ′ v_{\parallel} = v_{\parallel}^{'} v∥=v∥′
所以 v ′ = v ⊥ ′ + v ∥ v^{'} = v_{\perp}^{'} + v_{\parallel} v′=v⊥′+v∥
定义一个向量w
w = n × v w=n \times v w=n×v
根据几何意义，w的模长就是黄色阴影的的面积。即
∣ w ∣ = ∣ n × v ∣ |w|=|n \times v| ∣w∣=∣n×v∣
这部分面积也可以看成平行四边形面积公式底乘以高，因为n向量的模长是1，高就是 ∣ v ⊥ ∣ |v_{\perp}| ∣v⊥∣
所以 ∣ w ∣ = ∣ n × v ∣ = ∣ v ⊥ ∣ |w|=|n \times v|=|v_{\perp}| ∣w∣=∣n×v∣=∣v⊥∣
而由于 n × w n \times w n×w与 v ⊥ v_{\perp} v⊥方向相反
至此我们可以用n与v向量表示出v的两个投影的分量，即
v ⊥ = − n × w = − n × ( n × v ) v_{\perp}=-n \times w=-n \times(n \times v) v⊥=−n×w=−n×(n×v)

v ∥ = v − v ⊥ = v + n × ( n × v ) v_{\parallel}=v-v_{\perp}=v+n \times(n \times v) v∥=v−v⊥=v+n×(n×v)
我们现在讲v’表示出来
v ⊥ ′ = ∣ v ⊥ ∣ ∗ [ cos ⁡ ( θ ) ∗ v ⊥ ∣ v ⊥ ∣ + sin ⁡ ( θ ) ∗ w ∣ w ∣ ] = cos ⁡ ( θ ) ∗ v ⊥ + sin ⁡ ( θ ) ∗ ∣ v ⊥ ∣ ∗ w ∣ w ∣ = cos ⁡ ( θ ) ∗ v ⊥ + sin ⁡ ( θ ) ∗ w \begin{aligned} v_{\perp}^{\prime} &=\left|v_{\perp}\right| *\left[\cos (\theta) * \frac{v_{\perp}}{\left|v_{\perp}\right|}+\sin (\theta) * \frac{w}{|w|}\right] \\ &=\cos (\theta) * v_{\perp}+\sin (\theta) *\left|v_{\perp}\right| * \frac{w}{|w|} \\ &=\cos (\theta) * v_{\perp}+\sin (\theta) * w \end{aligned} v⊥′=∣v⊥∣∗[cos(θ)∗∣v⊥∣v⊥+sin(θ)∗∣w∣w]=cos(θ)∗v⊥+sin(θ)∗∣v⊥∣∗∣w∣w=cos(θ)∗v⊥+sin(θ)∗w
即
v ⊥ ′ = cos ⁡ ( θ ) ∗ v ⊥ + sin ⁡ ( θ ) ∗ w \begin{aligned} v_{\perp}^{\prime} = \cos (\theta) * v_{\perp}+\sin (\theta) * w \end{aligned} v⊥′=cos(θ)∗v⊥+sin(θ)∗w
将上式子带入
v ′ = v / / + v ⊥ ′ = ( v − v ⊥ ) + cos ⁡ ( θ ) ∗ v ⊥ + sin ⁡ ( θ ) ∗ w = v + ( cos ⁡ ( θ ) − 1 ) ∗ v ⊥ + sin ⁡ ( θ ) ∗ w = v + ( 1 − cos ⁡ ( θ ) ) ∗ n × ( n × v ) + sin ⁡ ( θ ) ∗ n × v = v + ( 1 − cos ⁡ ( θ ) ) ∗ n ∧ ∗ ( n ∧ ∗ v ) + sin ⁡ ( θ ) ∗ n ∧ ∗ v = v + ( 1 − cos ⁡ ( θ ) ) ∗ N ∗ N ∗ v + sin ⁡ ( θ ) ∗ N ∗ v = v + ( 1 − cos ⁡ ( θ ) ) ∗ N 2 ∗ v + sin ⁡ ( θ ) ∗ N ∗ v = [ I + ( 1 − cos ⁡ ( θ ) ) ∗ N 2 + sin ⁡ ( θ ) ∗ N ] ∗ v \begin{aligned} v^{\prime} &=v_{/ /}+v_{\perp}^{\prime} \\ &=\left(v-v_{\perp}\right)+\cos (\theta) * v_{\perp}+\sin (\theta) * w \\ &=v+(\cos (\theta)-1) * v_{\perp}+\sin (\theta) * w \\ &=v+(1-\cos (\theta)) * n \times(n \times v)+\sin (\theta) * n \times v \\ &=v+(1-\cos (\theta)) * n^{\wedge} * (n^{\wedge} * v)+\sin (\theta) * n^{\wedge} * v \\ &=v+(1-\cos (\theta)) * N * N * v+\sin (\theta) * N * v \\ &=v+(1-\cos (\theta)) * N^{2} * v+\sin (\theta) * N * v \\ &=\left[I+(1-\cos (\theta)) * N^{2}+\sin (\theta) * N\right] * v \end{aligned} v′=v//+v⊥′=(v−v⊥)+cos(θ)∗v⊥+sin(θ)∗w=v+(cos(θ)−1)∗v⊥+sin(θ)∗w=v+(1−cos(θ))∗n×(n×v)+sin(θ)∗n×v=v+(1−cos(θ))∗n∧∗(n∧∗v)+sin(θ)∗n∧∗v=v+(1−cos(θ))∗N∗N∗v+sin(θ)∗N∗v=v+(1−cos(θ))∗N2∗v+sin(θ)∗N∗v=[I+(1−cos(θ))∗N2+sin(θ)∗N]∗v

至此我们就可以得到：
v ′ = [ I + ( 1 − cos ⁡ ( θ ) ) ∗ N 2 + sin ⁡ ( θ ) ∗ N ] ∗ v v^{'} = [I+(1-\cos (\theta)) * N^{2}+\sin (\theta) * N]* v v′=[I+(1−cos(θ))∗N2+sin(θ)∗N]∗v
所以 R = [ I + ( 1 − cos ⁡ ( θ ) ) ∗ N 2 + sin ⁡ ( θ ) ∗ N ] R = [I+(1-\cos (\theta)) * N^{2}+\sin (\theta) * N] R=[I+(1−cos(θ))∗N2+sin(θ)∗N]
有些地方习惯用n来表示，那么继续推导
n ∗ n T = [ n 1 2 n 1 n 2 n 1 n 3 n 1 n 2 n 2 2 n 2 n 3 n 1 n 3 n 2 n 3 n 3 2 ] n * n^{T}=\left[\begin{array}{ccc}{n_{1}^{2}} & {n_{1} n_{2}} & {n_{1} n_{3}} \\ {n_{1} n_{2}} & {n_{2}^{2}} & {n_{2} n_{3}} \\ {n_{1} n_{3}} & {n_{2} n_{3}} & {n_{3}^{2}}\end{array}\right] n∗nT=⎣⎡n12n1n2n1n3n1n2n22n2n3n1n3n2n3n32⎦⎤

n ∧ = N = [ 0 − n 3 n 2 n 3 0 − n 1 − n 2 n 1 0 ] n^{\wedge}=N=\left[\begin{array}{ccc}{0} & {-n_{3}} & {n_{2}} \\ {n_{3}} & {0} & {-n_{1}} \\ {-n_{2}} & {n_{1}} & {0}\end{array}\right] n∧=N=⎣⎡0n3−n2−n30n1n2−n10⎦⎤

N ∗ N = [ − n 2 2 − n 3 2 n 1 n 2 n 1 n 3 n 1 n 2 − n 1 2 − n 3 2 n 2 n 3 n 1 n 3 n 2 n 3 − n 1 2 − n 2 2 ] N * N=\left[\begin{array}{ccc}{-n_{2}^{2}-n_{3}^{2}} & {n_{1} n_{2}} & {n_{1} n_{3}} \\ {n_{1} n_{2}} & {-n_{1}^{2}-n_{3}^{2}} & {n_{2} n_{3}} \\ {n_{1} n_{3}} & {n_{2} n_{3}} & {-n_{1}^{2}-n_{2}^{2}}\end{array}\right] N∗N=⎣⎡−n22−n32n1n2n1n3n1n2−n12−n32n2n3n1n3n2n3−n12−n22⎦⎤

n ∗ n T = I + N ∗ N n * n^{T}=I+N * N n∗nT=I+N∗N

N ∗ N = n ∗ n T − I N * N=n * n^{T}-I N∗N=n∗nT−I
带入 R = [ I + ( 1 − cos ⁡ ( θ ) ) ∗ N 2 + sin ⁡ ( θ ) ∗ N ] R = [I+(1-\cos (\theta)) * N^{2}+\sin (\theta) * N] R=[I+(1−cos(θ))∗N2+sin(θ)∗N]可以得到

三，公式理解和深入

这是一种非常简洁和简单的表示形式，但是这不是一个奇异的变换。主要原因有两点

旋转 θ \theta θ和旋转 θ + 2 π \theta + 2\pi θ+2π表达了相同的旋转
( n ∧ , θ ) (n^{\wedge}, \theta) (n∧,θ)与 ( − n ∧ , − θ ) (-n^{\wedge}, -\theta) (−n∧,−θ)也表达了相同的旋转
但是这样的好处在于，对于非常小的旋转，罗德里格斯公式可以表示成
R ( ω ) = R ( n ∧ , θ ) ≈ I + s i n ( θ ) ∗ N ≈ I + θ ∗ N = [ 1 − θ ∗ n 3 θ ∗ n 2 θ ∗ n 3 1 − θ ∗ n 1 − θ ∗ n 2 θ ∗ n 1 1 ] = [ 1 − ω z ω y ω z 1 ω x − ω y ω x 1 ] R(\omega) = R(n^{\wedge}, \theta) \approx I + sin(\theta) * N \\ \approx I + \theta * N \\=\left[\begin{array}{ccc}{1} & {-\theta * n_{3}} & {\theta * n_{2}} \\ {\theta *n_{3}} & {1} & {-\theta * n_{1}} \\ {-\theta * n_{2}} & {\theta *n_{1}} & {1}\end{array}\right] \\\\ = \left[\begin{array}{ccc}{1} & {-\omega_{z} } & {\omega_{y}} \\ {\omega_{z} } & {1} & {\omega_{x} } \\ {-\omega_{y} } & {\omega_{x} } & {1}\end{array}\right] R(ω)=R(n∧,θ)≈I+sin(θ)∗N≈I+θ∗N=⎣⎡1θ∗n3−θ∗n2−θ∗n31θ∗n1θ∗n2−θ∗n11⎦⎤=⎣⎡1ωz−ωy−ωz1ωxωyωx1⎦⎤
这个式子给出了，非常漂亮的的 ω \omega ω与R之间的线性的关系。
我们也可以写成， R v ≈ v + w × v Rv \approx v + w \times v Rv≈v+w×v,这样我们只需要简单的求导，就可以得到v
∂ R v ∂ ω = − v = [ 0 z − y − z 0 x y − x 0 ] \frac{\partial \boldsymbol{R} \boldsymbol{v}}{\partial \boldsymbol{\omega}}=-{v}=\left[\begin{array}{ccc}{0} & {z} & {-y} \\ {-z} & {0} & {x} \\ {y} & {-x} & {0}\end{array}\right] ∂ω∂Rv=−v=⎣⎡0−zyz0−x−yx0⎦⎤

四，极限的方式简洁推导罗德里格斯公式

叫做exponential twist(Murray, Li, and Sastry 1994), 旋转 θ \theta θ角度，等价于旋转k次 θ / k \theta/k θ/k角度。
R ( n ∧ , θ ) = lim ⁡ k → ∞ ( I + 1 k ( θ ∗ Ｎ ) ) k = e θ ∗ N R(n^{\wedge}, \theta) = \lim _{k \rightarrow \infty}\left(\boldsymbol{I}+\frac{1}{k}(\theta *Ｎ)\right)^{k} = e^{\theta *N} R(n∧,θ)=k→∞lim(I+k1(θ∗Ｎ))k=eθ∗N

e θ ∗ N = I + ( θ ∗ N ) + ( θ ∗ N ) 2 2 + ( θ ∗ N ) 3 3 ! + . . . e^{\theta *N} = I + (\theta *N) + \frac{(\theta *N)^{2}}{2} + \frac{(\theta *N)^{3}}{3!} + ... eθ∗N=I+(θ∗N)+2(θ∗N)2+3!(θ∗N)3+...
因为 N k + 2 = − N k , k > 0 N^{k+2} = -N^{k}, k>0 Nk+2=−Nk,k>0
所以
e θ ∗ N = I + ( θ − θ 3 3 ! + . . . ) ∗ N + ( θ 2 2 − θ 3 4 ! + . . . ) ∗ N 2 = I + s i n θ ∗ N + ( 1 − c o s θ ) ∗ N 2 e^{\theta *N} = I + (\theta - \frac{\theta^{3}}{3!}+...)*N + (\frac{\theta^{2}}{2} - \frac{\theta^{3}}{4!}+...)*N^{2} \\ = I+sin{\theta}*N + (1-cos{\theta})*N^{2} eθ∗N=I+(θ−3!θ3+...)∗N+(2θ2−4!θ3+...)∗N2=I+sinθ∗N+(1−cosθ)∗N2

罗德里格斯公式附图推导，理解相关推荐

罗德里格斯公式及其推导
罗德里格斯公式及其推导文章目录罗德里格斯公式及其推导一.基础准备 1. 旋转矩阵 2. 旋转向量 3. 向量叉积及反对称矩阵二.公式推导 1. 符号说明与图例 2. 推导三.公式应用及深入理 ...
《视觉SLAM十四讲》-第三章第3节公式推导-旋转向量-欧拉角--罗德里格斯公式详细推导
旋转矩阵罗德里格斯公式（Rodriguez formula）的理解证明
一.罗德里格斯公式的表达式罗德里格斯公式(Rodriguez formula)是计算机视觉中的一大经典公式,在描述相机位姿的过程中很常用,其形式如下: 二.对罗德里格斯公式的理解我们假设v⃗\ve ...
关于罗德里格斯公式(Rodrigues‘sFormula)的详细推导过程
关于罗德里格斯公式[Rodrigues'sFormula]的详细推导过程 1 旋转向量 2 罗德里格斯公式 2.1 罗德里格斯公式定义 2.2 罗德里格斯公式推导 3 旋转矩阵到旋转向量的转换 1 旋 ...
罗德里格斯公式理解、推导
罗德里格斯公式(Rodriguez formula)是计算机视觉中的一大经典公式,在描述相机位姿的过程中很常用.公式: R = I + s i n ( θ ) K + ( 1 − c o s ( ...
三维空间刚体运动2：旋转向量与罗德里格斯公式（最详细推导）
三维空间刚体运动2:旋转向量与罗德里格斯公式(最详推导) 1.旋转向量定义 2.罗德里格斯公式-向量转换为矩阵 2.1 定义 2.2 推导 2.2.1 推导一 2.2.2 推导二 2.2.3 推导向量 ...
罗德里格斯公式(Rodrigues‘ rotation formula)推导
罗德里格斯公式(Rodrigues' rotation formula)推导向量形式如图所示,三维空间中的一个矢量 v \bold v v绕轴 k \bold k k旋转 θ \theta θ角度 ...
视觉SLAM十四讲--罗德里格斯公式(Rodrigues’s Formula)推导
前言之前看了高博的<视觉SLAM十四讲>,里面有一段关于罗德里格斯公式,但是高博没有给出具体推导.然后我查了很多博主,都没有给出怎么推到 R = cos ⁡ θ I + ( 1 − co ...
罗德里格斯公式Rodrigues‘Rotation Formula推导
原文链接 :https://zhuanlan.zhihu.com/p/113299607 罗德里格斯公式是计算三维空间中,一个向量绕旋转轴旋转一定角度后得到一个新的向量的计算公式,而且可以改写为矩阵的 ...

罗德里格斯公式附图推导，理解