视觉slam十四讲 ppt,视觉slam优缺点
终极管理员 知识笔记 80阅读
文章目录CH3-8证明旋转四元数的虚部是零实部和指数映射CH4-2 SE(3)在CH4上李群和CH4-1so (3)在李代数李代数导出指数映射CH4-3在对极几何上本质矩阵奇异值分解矩阵内积和迹CH3-8证明旋转四元数的虚部是零实部和罗德里格斯公式结果之前已经导出：v' p，p，p，p，p，p，p， p * PVP {-1} v' PVP，其中v [0，v] v [0，v}] p [cos 2，sin 2 u] p [\ cos \ frac {\ Theta} {2}，\ sin \ frac { \ Theta } { 2 } \ vec { u }]p[cos 2，sin2 u]代入上式v' PVP。
∗ [ cos θ 2 , sin θ 2 u ⃗ ] [ 0 , v ⃗ ] [ cos θ 2 , − sin θ 2 u ⃗ ] [ 0 − sin θ 2 u ⃗ ⋅ v ⃗ , cos θ 2 v ⃗ 0 sin θ 2 u ⃗ × v ⃗ ] [ cos θ 2 , − sin θ 2 u ⃗ ] [ − sin θ 2 u ⃗ ⋅ v ⃗ , cos θ 2 v ⃗ sin θ 2 u ⃗ × v ⃗ ] [ cos θ 2 , − sin θ 2 u ⃗ ] (3-8-1) \begin{aligned} v&pvp^* \\ &[\cos\frac{\theta}{2},\sin\frac{\theta}{2}\vec{u}][0,\vec{v}][\cos\frac{\theta}{2},-\sin\frac{\theta}{2}\vec{u}] \\ &[0-\sin\frac{\theta}{2}\vec{u}\cdot\vec{v},\cos\frac{\theta}{2}\vec{v}0\sin\frac{\theta}{2}\vec{u}\times\vec{v}][\cos\frac{\theta}{2},-\sin\frac{\theta}{2}\vec{u}] \\ &[-\sin\frac{\theta}{2}\vec{u}\cdot\vec{v},\cos\frac{\theta}{2}\vec{v}\sin\frac{\theta}{2}\vec{u}\times\vec{v}][\cos\frac{\theta}{2},-\sin\frac{\theta}{2}\vec{u}] \end{aligned} \tag{3-8-1} v′pvp∗[cos2θ,sin2θu ][0,v ][cos2θ,−sin2θu ][0−sin2θu ⋅v ,cos2θv 0sin2θu ×v ][cos2θ,−sin2θu ][−sin2θu ⋅v ,cos2θv sin2θu ×v ][cos2θ,−sin2θu ](3-8-1)
分别计算实部和虚部
R e − sin θ 2 cos θ 2 u ⃗ ⋅ v ⃗ ( cos θ 2 v ⃗ sin θ 2 u ⃗ × v ⃗ ) ⋅ sin θ 2 u ⃗ − sin θ 2 cos θ 2 u ⃗ ⋅ v ⃗ cos θ 2 sin θ 2 u ⃗ ⋅ v ⃗ sin θ 2 ( u ⃗ × v ⃗ ) ⋅ u ⃗ 0 0 0 (3-8-2) \begin{aligned} \mathrm{Re}&-\sin\frac{\theta}{2}\cos\frac{\theta}{2}\vec{u}\cdot\vec{v}(\cos\frac{\theta}{2}\vec{v}\sin\frac{\theta}{2}\vec{u}\times\vec{v})\cdot\sin\frac{\theta}{2}\vec{u}\\ &-\sin\frac{\theta}{2}\cos\frac{\theta}{2}\vec{u}\cdot\vec{v}\cos\frac{\theta}{2}\sin\frac{\theta}{2}\vec{u}\cdot\vec{v}\sin\frac{\theta}{2}(\vec{u}\times\vec{v})\cdot\vec{u}\\ &00 \\ &0 \end{aligned} \tag{3-8-2} Re−sin2θcos2θu ⋅v (cos2θv sin2θu ×v )⋅sin2θu −sin2θcos2θu ⋅v cos2θsin2θu ⋅v sin2θ(u ×v )⋅u 000(3-8-2)
I m ( − sin θ 2 u ⃗ ⋅ v ⃗ ) ⋅ ( − sin θ 2 u ⃗ ) ( cos θ 2 v ⃗ sin θ 2 u ⃗ × v ⃗ ) cos θ 2 ( cos θ 2 v ⃗ sin θ 2 u ⃗ × v ⃗ ) × ( − sin θ 2 u ⃗ ) (3-8-3) \begin{aligned} \mathrm{Im}&(-\sin\frac{\theta}{2}\vec{u}\cdot\vec{v})\cdot(-\sin\frac{\theta}{2}\vec{u})(\cos\frac{\theta}{2}\vec{v}\sin\frac{\theta}{2}\vec{u}\times\vec{v})\cos\frac{\theta}{2} \\ &(\cos\frac{\theta}{2}\vec{v}\sin\frac{\theta}{2}\vec{u}\times\vec{v})\times(-\sin\frac{\theta}{2}\vec{u}) \end{aligned} \tag{3-8-3} Im(−sin2θu ⋅v )⋅(−sin2θu )(cos2θv sin2θu ×v )cos2θ(cos2θv sin2θu ×v )×(−sin2θu )(3-8-3)
我们希望将其写成矩阵乘法形式。
先证明公式 a ⃗ × ( b ⃗ × c ⃗ ) ( a ⃗ ⋅ c ⃗ ) ⋅ b ⃗ − ( a ⃗ ⋅ b ⃗ ) ⋅ c ⃗ \vec{a}\times(\vec{b}\times\vec{c})(\vec{a}\cdot\vec{c})\cdot\vec{b}-(\vec{a}\cdot\vec{b})\cdot\vec{c} a ×(b ×c )(a ⋅c )⋅b −(a ⋅b )⋅c 。
证明
a ⃗ × b ⃗ a ∧ b [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] [ b 1 b 2 b 3 ] △ T a b (3-8-4) \begin{aligned} \vec{a}\times\vec{b}&\boldsymbol{a}^{\wedge}\boldsymbol{b} \\ &\left[\begin{array}{c} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \end{array}\right]\left[\begin{array}{c} b_1 \\ b_2 \\ b_3 \end{array}\right]\stackrel{\bigtriangleup}\boldsymbol{T}_a\boldsymbol{b} \end{aligned} \tag{3-8-4} a ×b a∧b 0a3−a2−a30a1a2−a10 b1b2b3 △Tab(3-8-4)
那么矩阵乘法满足结合律
a ⃗ × ( b ⃗ × c ⃗ ) ( T a T b ) c T a T b c \vec{a}\times(\vec{b}\times\vec{c})(\boldsymbol{T}_a\boldsymbol{T}_b)\boldsymbol{c}\boldsymbol{T}_a\boldsymbol{T}_b\boldsymbol{c} a ×(b ×c )(TaTb)cTaTbc
而
T a T b [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] [ 0 − b 3 b 2 b 3 0 − b 1 − b 2 b 1 0 ] [ − a 3 b 3 − a 2 b 2 a 2 b 1 a 3 b 1 a 1 b 2 − a 3 b 3 − a 1 b 1 a 3 b 2 a 1 b 3 a 2 b 3 − a 2 b 2 − a 1 b 1 ] − ( a ⃗ ⋅ b ⃗ ) I b a T \begin{aligned} \boldsymbol{T}_a\boldsymbol{T}_b&\left[\begin{array}{c} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \end{array}\right]\left[\begin{array}{c} 0 & -b_3 & b_2 \\ b_3 & 0 & -b_1 \\ -b_2 & b_1 & 0 \end{array}\right] \\ &\left[\begin{array}{c} -a_3b_3-a_2b_2 & a_2b_1 & a_3b_1 \\ a_1b_2 & -a_3b_3-a_1b_1 & a_3b_2 \\ a_1b_3 & a_2b_3 & -a_2b_2 -a_1b_1 \end{array}\right]\\ &-(\vec{a}\cdot\vec{b})\boldsymbol{I}\boldsymbol{b}\boldsymbol{a}^{\mathrm{T}} \end{aligned} TaTb 0a3−a2−a30a1a2−a10 0b3−b2−b30b1b2−b10 −a3b3−a2b2a1b2a1b3a2b1−a3b3−a1b1a2b3a3b1a3b2−a2b2−a1b1 −(a ⋅b )IbaT
则用到了矩阵结合律
a ⃗ × ( b ⃗ × c ⃗ ) T a T b c ( − ( a ⃗ ⋅ b ⃗ ) I b a T ) c − ( a ⃗ ⋅ b ⃗ ) c b ( a T c ) − ( a ⃗ ⋅ b ⃗ ) c ( a ⃗ ⋅ c ⃗ ) b (3-8-5) \begin{aligned} \vec{a}\times(\vec{b}\times\vec{c})\boldsymbol{T}_a\boldsymbol{T}_b\boldsymbol{c}&(-(\vec{a}\cdot\vec{b})\boldsymbol{I}\boldsymbol{b}\boldsymbol{a}^{\mathrm{T}})\boldsymbol{c} \\ &-(\vec{a}\cdot\vec{b})\boldsymbol{c}\boldsymbol{b}(\boldsymbol{a}^{\mathrm{T}}\boldsymbol{c}) \\ &-(\vec{a}\cdot\vec{b})\boldsymbol{c}(\vec{a}\cdot\vec{c})\boldsymbol{b} \end{aligned} \tag{3-8-5} a ×(b ×c )TaTbc(−(a ⋅b )IbaT)c−(a ⋅b )cb(aTc)−(a ⋅b )c(a ⋅c )b(3-8-5)
同理可证
( a ⃗ × b ⃗ ) × c ⃗ ( a ⃗ ⋅ c ⃗ ) b − ( b ⃗ ⋅ c ⃗ ) a (3-8-6) (\vec{a}\times\vec{b})\times\vec{c}(\vec{a}\cdot\vec{c})\boldsymbol{b}-(\vec{b}\cdot\vec{c})\boldsymbol{a} \tag{3-8-6} (a ×b )×c (a ⋅c )b−(b ⋅c )a(3-8-6)
证毕。
下面继续推导式3-8-3
I m sin 2 θ 2 u ⃗ ⋅ v ⃗ ⋅ u ⃗ cos 2 θ 2 v ⃗ sin θ 2 cos θ 2 u ⃗ × v ⃗ − sin θ 2 cos θ 2 v ⃗ × u ⃗ − sin 2 θ 2 u ⃗ × v ⃗ × u ⃗ sin 2 θ 2 ( u ⃗ ⋅ v ⃗ ) ⋅ u ⃗ cos 2 θ 2 v ⃗ sin θ u ⃗ × v ⃗ − sin 2 θ 2 [ ( u ⃗ ⋅ u ⃗ ) v − ( v ⃗ ⋅ u ⃗ ) u ] sin 2 θ 2 ( u ⃗ ⋅ v ⃗ ) ⋅ u ‾ cos 2 θ 2 v sin θ u ⃗ × v ⃗ − sin 2 θ 2 v sin 2 θ 2 ( v ⃗ ⋅ u ⃗ ) u ‾ cos θ v 2 sin 2 θ 2 ( v ⃗ ⋅ u ⃗ ) u sin θ u ⃗ × v ⃗ cos θ v ( 1 − cos θ ) ( v ⃗ ⋅ u ⃗ ) u sin θ u ⃗ × v ⃗ (3-8-7) \begin{aligned} \mathrm{Im}&\sin^2\frac{\theta}{2}\vec{u}\cdot\vec{v}\cdot\vec{u}\cos^2\frac{\theta}{2}\vec{v}\sin\frac{\theta}{2}\cos\frac{\theta}{2}\vec{u}\times\vec{v}-\sin\frac{\theta}{2}\cos\frac{\theta}{2}\vec{v}\times\vec{u}-\sin^2\frac{\theta}{2}\vec{u}\times\vec{v}\times\vec{u} \\ &\sin^2\frac{\theta}{2}(\vec{u}\cdot\vec{v})\cdot\vec{u}\cos^2\frac{\theta}{2}\vec{v}\sin\theta\vec{u}\times\vec{v}-\sin^2\frac{\theta}{2}[(\vec{u}\cdot\vec{u})\boldsymbol{v}-(\vec{v}\cdot\vec{u})\boldsymbol{u}] \\ &\underline{\sin^2\frac{\theta}{2}(\vec{u}\cdot\vec{v})\cdot\boldsymbol{u}}\cos^2\frac{\theta}{2}\boldsymbol{v}\sin\theta\vec{u}\times\vec{v}-\sin^2\frac{\theta}{2}\boldsymbol{v}\underline{\sin^2\frac{\theta}{2}(\vec{v}\cdot\vec{u})\boldsymbol{u}} \\ &\cos\theta\boldsymbol{v}2\sin^2\frac{\theta}{2}(\vec{v}\cdot\vec{u})\boldsymbol{u}\sin\theta\vec{u}\times\vec{v} \\ &\cos\theta\boldsymbol{v}(1-\cos\theta)(\vec{v}\cdot\vec{u})\boldsymbol{u}\sin\theta\vec{u}\times\vec{v} \end{aligned} \tag{3-8-7} Imsin22θu ⋅v ⋅u cos22θv sin2θcos2θu ×v −sin2θcos2θv ×u −sin22θu ×v ×u sin22θ(u ⋅v )⋅u cos22θv sinθu ×v −sin22θ[(u ⋅u )v−(v ⋅u )u]sin22θ(u ⋅v )⋅ucos22θvsinθu ×v −sin22θvsin22θ(v ⋅u )ucosθv2sin22θ(v ⋅u )usinθu ×v cosθv(1−cosθ)(v ⋅u )usinθu ×v (3-8-7)
注意 u ⃗ \vec{u} u 是单位向量故 u ⃗ ⋅ u ⃗ 1 \vec{u}\cdot\vec{u}1 u ⋅u 1。
也就是拉格朗日公式结果。证毕。
CH4 李群与李代数 CH4-1 SO(3) 上的指数映射将指数函数 e x e^x ex 在 x 0 x0 x0 处泰勒展开即
e x 1 x 1 2 ! x 2 1 3 ! x 3 . . . 1 n ! x n ∑ n 0 ∞ x n n ! (4-1-1) \begin{aligned} e^x & 1x\frac{1}{2!}x^2\frac{1}{3!}x^3...\frac{1}{n!}x^n \\ &\sum_{n0}^{\infty}\frac{x^n}{n!} \end{aligned} \tag{4-1-1} ex1x2!1x23!1x3...n!1xnn0∑∞n!xn(4-1-1)
将矩阵 A \boldsymbol{A} A 代入上式 则
e A ∑ n 0 ∞ A n n ! e^{\boldsymbol{A}}\sum_{n0}^{\infty}\frac{\boldsymbol{A}^n}{n!} eAn0∑∞n!An
同样的也有
e ϕ ∧ ∑ n 0 ∞ ( ϕ ∧ ) n n ! (4-1-2) e^{\boldsymbol{\phi}^{\wedge}}\sum_{n0}^{\infty}\frac{(\boldsymbol{\phi}^{\wedge})^n}{n!} \tag{4-1-2} eϕ∧n0∑∞n!(ϕ∧)n(4-1-2)
令 ϕ θ a \boldsymbol{\phi}\theta\boldsymbol{a} ϕθa θ \theta θ为模长 a \boldsymbol{a} a 为单位方向向量。则上式可写为
e ( θ a ) ∧ ∑ n 0 ∞ ( θ a ∧ ) n n ! e^{\boldsymbol({\theta\boldsymbol{a}})^{\wedge}}\sum_{n0}^{\infty}\frac{(\theta\boldsymbol{a}^{\wedge})^n}{n!} e(θa)∧n0∑∞n!(θa∧)n
我们知道
a ∧ [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] \boldsymbol{a}^{\wedge}\left[\begin{array}{c} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \end{array}\right] a∧ 0a3−a2−a30a1a2−a10
则
a ∧ a ∧ [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] [ − a 3 2 − a 2 2 a 1 a 2 a 1 a 3 a 1 a 2 − a 3 2 − a 1 2 a 2 a 3 a 1 a 3 a 2 a 3 − a 2 2 − a 1 2 ] (4-1-3) \begin{aligned} \boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}&\left[\begin{array}{c} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \end{array}\right]\left[\begin{array}{c} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \end{array}\right] \\ &\left[\begin{array}{c} -a_3^2-a_2^2 & a_1a_2 & a_1a_3 \\ a_1a_2 & -a_3^2-a_1^2 & a_2a_3 \\ a_1a_3 & a_2a_3 & -a_2^2-a_1^2 \end{array}\right] \end{aligned} \tag{4-1-3} a∧a∧ 0a3−a2−a30a1a2−a10 0a3−a2−a30a1a2−a10 −a32−a22a1a2a1a3a1a2−a32−a12a2a3a1a3a2a3−a22−a12 (4-1-3)
因为 a \boldsymbol{a} a 是单位向量则有 a 1 2 a 2 2 a 3 2 1 a_1^2a_2^2a_3^21 a12a22a321可得
a a T − I [ a 1 a 2 a 3 ] [ a 1 a 2 a 3 ] [ a 1 2 a 1 a 2 a 1 a 3 a 2 a 1 a 2 2 a 2 a 3 a 1 a 3 a 2 a 3 a 3 2 ] − [ 1 0 0 0 1 0 0 0 1 ] [ − a 3 2 − a 2 2 a 1 a 2 a 1 a 3 a 1 a 2 − a 3 2 − a 1 2 a 2 a 3 a 1 a 3 a 2 a 3 − a 2 2 − a 1 2 ] a ∧ a ∧ (4-1-4) \begin{aligned} \boldsymbol{a}\boldsymbol{a}^{\mathrm{T}}-\boldsymbol{I}\left[\begin{array}{c} a_1 \\ a_2 \\ a_3 \end{array}\right]\left[\begin{array}{ccc} a_1 & a_2 & a_3 \end{array}\right] &\left[\begin{array}{c} a_1^2 & a_1a_2 & a_1a_3 \\ a_2a_1 & a_2^2 & a_2a_3 \\ a_1a_3 & a_2a_3 & a_3^2 \end{array}\right]- \left[\begin{array}{c} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{array}\right] \\ &\left[\begin{array}{c} -a_3^2-a_2^2 & a_1a_2 & a_1a_3 \\ a_1a_2 & -a_3^2-a_1^2 & a_2a_3 \\ a_1a_3 & a_2a_3 & -a_2^2-a_1^2 \end{array}\right] \\ &\boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge} \end{aligned} \tag{4-1-4} aaT−I a1a2a3 [a1a2a3] a12a2a1a1a3a1a2a22a2a3a1a3a2a3a32 − 100010001 −a32−a22a1a2a1a3a1a2−a32−a12a2a3a1a3a2a3−a22−a12 a∧a∧(4-1-4)
a ∧ a ∧ a ∧ [ − a 3 2 − a 2 2 a 1 a 2 a 1 a 3 a 1 a 2 − a 3 2 − a 1 2 a 2 a 3 a 1 a 3 a 2 a 3 − a 2 2 − a 1 2 ] [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] [ 0 a 2 2 a 3 a 3 3 a 1 2 a 3 − a 2 3 − a 2 a 3 2 − a 1 a 2 2 − a 1 2 a 3 − a 3 3 − a 1 2 a 3 0 a 1 a 2 2 a 1 3 a 1 a 3 2 a 2 a 3 2 a 1 2 a 2 a 2 3 − a 1 a 3 2 − a 1 3 − a 1 a 2 2 0 ] \begin{aligned} \boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}&\left[\begin{array}{c} -a_3^2-a_2^2 & a_1a_2 & a_1a_3 \\ a_1a_2 & -a_3^2-a_1^2 & a_2a_3 \\ a_1a_3 & a_2a_3 & -a_2^2-a_1^2 \end{array}\right]\left[\begin{array}{c} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \end{array}\right] \\ &\left[\begin{array}{c} 0 & a_2^2a_3a_3^3a_1^2a_3 & -a_2^3-a_2a_3^2-a_1a_2^2 \\ -a_1^2a_3-a_3^3-a_1^2a_3 & 0 & a_1a_2^2a_1^3a_1a_3^2 \\ a_2a_3^2a_1^2a_2a_2^3 & -a_1a_3^2-a_1^3-a_1a_2^2 & 0 \end{array}\right] \\ \end{aligned} a∧a∧a∧ −a32−a22a1a2a1a3a1a2−a32−a12a2a3a1a3a2a3−a22−a12 0a3−a2−a30a1a2−a10 0−a12a3−a33−a12a3a2a32a12a2a23a22a3a33a12a30−a1a32−a13−a1a22−a23−a2a32−a1a22a1a22a13a1a320
又 a 1 2 a 2 2 a 3 2 1 a_1^2a_2^2a_3^21 a12a22a321上式写为
a ∧ a ∧ a ∧ [ 0 a 3 − a 2 − a 3 0 a 1 a 2 − a 1 0 ] − a ∧ (4-1-5) \boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}\left[\begin{array}{c} 0 & a_3 & -a_2 \\ -a_3 & 0 & a_1 \\ a_2 & -a_1 & 0 \end{array}\right]-\boldsymbol{a}^{\wedge} \tag{4-1-5} a∧a∧a∧ 0−a3a2a30−a1−a2a10 −a∧(4-1-5)
对式4-1-2
e ϕ ∧ e ( θ a ) ∧ ∑ n 0 ∞ ( θ a ∧ ) n n ! I θ a ∧ 1 2 ! θ 2 a ∧ a ∧ 1 3 ! θ 3 a ∧ a ∧ a ∧ 1 4 ! θ 4 a ∧ a ∧ a ∧ a ∧ . . . ( a a T − a ∧ a ∧ ) θ a ∧ 1 2 ! θ 2 a ∧ a ∧ − 1 3 ! θ 3 a ∧ − 1 4 ! θ 4 a ∧ a ∧ . . . a a T ( θ − 1 3 ! θ 3 1 5 ! θ 5 . . . ) a ∧ ( − 1 1 2 ! θ 2 − 1 4 ! θ 4 . . . ) a ∧ a ∧ ( a ∧ a ∧ I ) sin θ a ∧ − cos θ ( a ∧ a ∧ ) ( 1 − cos θ ) a ∧ a ∧ I sin θ a ∧ ( 1 − cos θ ) ( a a T − I ) I sin θ a ∧ a a T − I − cos θ a a T cos θ I I sin θ a ∧ cos θ I ( 1 − cos θ ) a a T sin θ a ∧ \begin{aligned} e^{\boldsymbol{\phi}^{\wedge}}e^{\boldsymbol({\theta\boldsymbol{a}})^{\wedge}}&\sum_{n0}^{\infty}\frac{(\theta\boldsymbol{a}^{\wedge})^n}{n!} \\ &\boldsymbol{I}\theta\boldsymbol{a}^{\wedge}\frac{1}{2!}\theta^2 \boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}\frac{1}{3!}\theta^3 \boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}\frac{1}{4!}\theta^4 \boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}...\\ &(\boldsymbol{a}\boldsymbol{a}^{\mathrm{T}}-\boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge})\theta\boldsymbol{a}^{\wedge}\frac{1}{2!}\theta^2 \boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}-\frac{1}{3!}\theta^3 \boldsymbol{a}^{\wedge}-\frac{1}{4!}\theta^4 \boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}...\\ &\boldsymbol{a}\boldsymbol{a}^{\mathrm{T}}(\theta-\frac{1}{3!}\theta^3\frac{1}{5!}\theta^5...)\boldsymbol{a}^{\wedge}(-1\frac{1}{2!}\theta^2-\frac{1}{4!}\theta^4...)\boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}\\ &(\boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}\boldsymbol{I})\sin\theta \boldsymbol{a}^{\wedge}-\cos\theta(\boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge})\\ &(1-\cos\theta)\boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}\boldsymbol{I}\sin\theta \boldsymbol{a}^{\wedge} \\ &(1-\cos\theta)(\boldsymbol{a}\boldsymbol{a}^{\mathrm{T}}-\boldsymbol{I})\boldsymbol{I}\sin\theta \boldsymbol{a}^{\wedge}\\ &\boldsymbol{a}\boldsymbol{a}^{\mathrm{T}}-\boldsymbol{I}-\cos\theta\boldsymbol{a}\boldsymbol{a}^{\mathrm{T}}\cos\theta\boldsymbol{I}\boldsymbol{I}\sin\theta \boldsymbol{a}^{\wedge}\\ &\cos\theta\boldsymbol{I}(1-\cos\theta)\boldsymbol{a}\boldsymbol{a}^{\mathrm{T}}\sin\theta \boldsymbol{a}^{\wedge} \end{aligned} eϕ∧e(θa)∧n0∑∞n!(θa∧)nIθa∧2!1θ2a∧a∧3!1θ3a∧a∧a∧4!1θ4a∧a∧a∧a∧...(aaT−a∧a∧)θa∧2!1θ2a∧a∧−3!1θ3a∧−4!1θ4a∧a∧...aaT(θ−3!1θ35!1θ5...)a∧(−12!1θ2−4!1θ4...)a∧a∧(a∧a∧I)sinθa∧−cosθ(a∧a∧)(1−cosθ)a∧a∧Isinθa∧(1−cosθ)(aaT−I)Isinθa∧aaT−I−cosθaaTcosθIIsinθa∧cosθI(1−cosθ)aaTsinθa∧
于是得到李代数 ϕ \boldsymbol{\phi} ϕ 和旋转矩阵 R \boldsymbol{R} R 之间的映射关系即
R e ϕ ∧ cos θ I ( 1 − cos θ ) a a T sin θ a ∧ (4-1-6) \boldsymbol{R}e^{\boldsymbol{\phi}^{\wedge}}\cos\theta\boldsymbol{I}(1-\cos\theta)\boldsymbol{a}\boldsymbol{a}^{\mathrm{T}}\sin\theta \boldsymbol{a}^{\wedge} \tag{4-1-6} Reϕ∧cosθI(1−cosθ)aaTsinθa∧(4-1-6)
也就是 罗德里格斯公式。
CH4-2 SE(3) 上的指数映射已知李代数 ξ [ ρ ϕ ] T ∈ R 6 \boldsymbol{\xi}[\rho \quad \phi]^{\mathrm{T}}\in \boldsymbol{\mathbb{R}}^6 ξ[ρϕ]T∈R6它的反对称矩阵为
ξ ∧ [ ϕ ∧ ρ 0 T 0 ] \boldsymbol{\xi}^{\wedge}\left[\begin{array}{c} \phi^{\wedge} & \rho \\ \boldsymbol{0}^{\mathrm{T}} & 0 \end{array}\right] ξ∧[ϕ∧0Tρ0]
则李群为
T exp ( ξ ∧ ) [ ∑ n 0 ∞ ( ϕ ∧ ) n n ! ∑ n 0 ∞ ( ϕ ∧ ) n ( n 1 ) ! ρ 0 T 0 ] ≜ [ R J ρ 0 T 1 ] (4-2-1) \begin{aligned} \boldsymbol{T}\exp(\boldsymbol{\xi}^{\wedge})&\left[\begin{array}{c} \sum_{n0}^{\infty}\frac{(\boldsymbol{\phi}^{\wedge})^n}{n!} & \sum_{n0}^{\infty}\frac{(\boldsymbol{\phi}^{\wedge})^n}{(n1)!}\rho \\ \boldsymbol{0}^{\mathrm{T}} & 0 \end{array}\right] \\ &\triangleq \left[\begin{array}{c} \boldsymbol{R} & \boldsymbol{J}\rho \\ \boldsymbol{0}^{\mathrm{T}} & 1 \end{array}\right] \end{aligned} \tag{4-2-1} Texp(ξ∧)[∑n0∞n!(ϕ∧)n0T∑n0∞(n1)!(ϕ∧)nρ0]≜[R0TJρ1](4-2-1)
下面开始证明
同样假设 ϕ θ a \boldsymbol{\phi}\theta\boldsymbol{a} ϕθa θ \theta θ为模长 a \boldsymbol{a} a为单位方向向量。将 exp ( ξ ∧ ) \exp(\boldsymbol{\xi}^{\wedge}) exp(ξ∧) 泰勒展开
exp ( ξ ∧ ) 1 n ! ∑ n 0 ∞ [ ϕ ∧ ρ 0 T 0 ] n 1 n ! ∑ n 0 ∞ [ θ a ∧ ρ 0 T 0 ] n (4-2-2) \exp(\boldsymbol{\xi}^{\wedge})\frac{1}{n!}\sum_{n0}^{\infty}\left[\begin{array}{c} \phi^{\wedge} & \rho \\ \boldsymbol{0}^{\mathrm{T}} & 0 \end{array}\right]^n\frac{1}{n!}\sum_{n0}^{\infty}\left[\begin{array}{c} \theta\boldsymbol{a}^{\wedge} & \rho \\ \boldsymbol{0}^{\mathrm{T}} & 0 \end{array}\right]^n \tag{4-2-2} exp(ξ∧)n!1n0∑∞[ϕ∧0Tρ0]nn!1n0∑∞[θa∧0Tρ0]n(4-2-2)
当 n 0 n0 n0 时
1 0 ! [ ϕ ∧ ρ 0 T 0 ] 0 I \frac{1}{0!}\left[\begin{array}{c} \phi^{\wedge} & \rho \\ \boldsymbol{0}^{\mathrm{T}} & 0 \end{array}\right]^0\boldsymbol{I} 0!1[ϕ∧0Tρ0]0I
当 n 1 n1 n1 时
1 1 ! [ θ a ∧ ρ 0 T 0 ] 1 [ θ a ∧ ρ 0 T 0 ] \frac{1}{1!}\left[\begin{array}{c} \theta\boldsymbol{a}^{\wedge} & \rho \\ \boldsymbol{0}^{\mathrm{T}} & 0 \end{array}\right]^1\left[\begin{array}{c} \theta\boldsymbol{a}^{\wedge} & \rho \\ \boldsymbol{0}^{\mathrm{T}} & 0 \end{array}\right] 1!1[θa∧0Tρ0]1[θa∧0Tρ0]
当 n 2 n2 n2 时
1 2 ! [ θ a ∧ ρ 0 T 0 ] [ θ a ∧ ρ 0 T 0 ] [ ( θ a ∧ ) 2 θ a ∧ ρ 0 T 0 ] \frac{1}{2!}\left[\begin{array}{c} \theta\boldsymbol{a}^{\wedge} & \rho \\ \boldsymbol{0}^{\mathrm{T}} & 0 \end{array}\right]\left[\begin{array}{c} \theta\boldsymbol{a}^{\wedge} & \rho \\ \boldsymbol{0}^{\mathrm{T}} & 0 \end{array}\right]\left[\begin{array}{c} (\theta\boldsymbol{a}^{\wedge})^2 & \theta\boldsymbol{a}^{\wedge}\rho \\ \boldsymbol{0}^{\mathrm{T}} & 0 \end{array}\right] 2!1[θa∧0Tρ0][θa∧0Tρ0][(θa∧)20Tθa∧ρ0]
当 n 3 n3 n3 时
1 3 ! [ θ a ∧ ρ 0 T 0 ] [ θ a ∧ ρ 0 T 0 ] [ θ a ∧ ρ 0 T 0 ] 1 3 ! [ ( θ a ∧ ) 3 ( θ a ∧ ) 2 ρ 0 T 0 ] \frac{1}{3!}\left[\begin{array}{c} \theta\boldsymbol{a}^{\wedge} & \rho \\ \boldsymbol{0}^{\mathrm{T}} & 0 \end{array}\right]\left[\begin{array}{c} \theta\boldsymbol{a}^{\wedge} & \rho \\ \boldsymbol{0}^{\mathrm{T}} & 0 \end{array}\right]\left[\begin{array}{c} \theta\boldsymbol{a}^{\wedge} & \rho \\ \boldsymbol{0}^{\mathrm{T}} & 0 \end{array}\right]\frac{1}{3!}\left[\begin{array}{c} (\theta\boldsymbol{a}^{\wedge})^3 & (\theta\boldsymbol{a}^{\wedge})^2\rho \\ \boldsymbol{0}^{\mathrm{T}} & 0 \end{array}\right] 3!1[θa∧0Tρ0][θa∧0Tρ0][θa∧0Tρ0]3!1[(θa∧)30T(θa∧)2ρ0]
以此类推
1 n ! [ θ a ∧ ρ 0 T 0 ] n 1 n ! [ ( θ a ∧ ) n ( θ a ∧ ) n − 1 ρ 0 T 0 ] (4-2-3) \frac{1}{n!}\left[\begin{array}{c} \theta\boldsymbol{a}^{\wedge} & \rho \\ \boldsymbol{0}^{\mathrm{T}} & 0 \end{array}\right]^n\frac{1}{n!}\left[\begin{array}{c} (\theta\boldsymbol{a}^{\wedge})^n & (\theta\boldsymbol{a}^{\wedge})^{n-1}\rho \\ \boldsymbol{0}^{\mathrm{T}} & 0 \end{array}\right] \tag{4-2-3} n!1[θa∧0Tρ0]nn!1[(θa∧)n0T(θa∧)n−1ρ0](4-2-3)
那么式4-1-8可化为
exp ( ξ ∧ ) 1 n ! ∑ n 0 ∞ [ ϕ ∧ ρ 0 T 0 ] n I [ θ a ∧ ρ 0 T 0 ] [ ( θ a ∧ ) 2 θ a ∧ ρ 0 T 0 ] . . . 1 n ! [ ( θ a ∧ ) n ( θ a ∧ ) n − 1 ρ 0 T 0 ] [ ∑ n 0 ∞ 1 n ! ( θ a ∧ ) n ∑ n 0 ∞ 1 ( n 1 ) ! ( θ a ∧ ) n ρ 0 T 1 ] (4-2-4) \begin{aligned} \exp(\boldsymbol{\xi}^{\wedge})&\frac{1}{n!}\sum_{n0}^{\infty}\left[\begin{array}{c} \phi^{\wedge} & \rho \\ \boldsymbol{0}^{\mathrm{T}} & 0 \end{array}\right]^n \\ &\boldsymbol{I}\left[\begin{array}{c} \theta\boldsymbol{a}^{\wedge} & \rho \\ \boldsymbol{0}^{\mathrm{T}} & 0 \end{array}\right]\left[\begin{array}{c} (\theta\boldsymbol{a}^{\wedge})^2 & \theta\boldsymbol{a}^{\wedge}\rho \\ \boldsymbol{0}^{\mathrm{T}} & 0 \end{array}\right]...\frac{1}{n!}\left[\begin{array}{c} (\theta\boldsymbol{a}^{\wedge})^n & (\theta\boldsymbol{a}^{\wedge})^{n-1}\rho \\ \boldsymbol{0}^{\mathrm{T}} & 0 \end{array}\right]\\ &\left[\begin{array}{c} \sum_{n0}^{\infty}\frac{1}{n!}(\theta\boldsymbol{a}^{\wedge})^n & \sum_{n0}^{\infty}\frac{1}{(n1)!}(\theta\boldsymbol{a}^{\wedge})^n\rho \\ \boldsymbol{0}^{\mathrm{T}} & 1 \end{array}\right] \tag{4-2-4} \end{aligned} exp(ξ∧)n!1n0∑∞[ϕ∧0Tρ0]nI[θa∧0Tρ0][(θa∧)20Tθa∧ρ0]...n!1[(θa∧)n0T(θa∧)n−1ρ0][∑n0∞n!1(θa∧)n0T∑n0∞(n1)!1(θa∧)nρ1](4-2-4)
其中左上角为 S O ( 3 ) SO(3) SO(3) 指数映射前面已经证明。令
J ∑ n 0 ∞ 1 ( n 1 ) ! ( θ a ∧ ) n I 1 2 ! θ a ∧ 1 3 ! ( θ a ∧ ) 2 1 4 ! ( θ a ∧ ) 3 1 5 ! ( θ a ∧ ) 4 1 θ ( 1 2 ! θ 2 − 1 4 ! θ 4 . . . ) a ∧ 1 θ ( 1 3 ! θ 3 − 1 5 ! θ 5 . . . ) ( a ∧ ) 2 I 1 − cos θ θ a ∧ θ − sin θ θ ( a ∧ ) 2 I 1 − cos θ θ a ∧ ( 1 − sin θ θ ) ( a a T − I ) I 1 − cos θ θ a ∧ ( 1 − sin θ θ ) a a T − I sin θ θ I I sin θ θ I ( 1 − sin θ θ ) a a T 1 − cos θ θ a ∧ (4-2-5) \begin{aligned} \boldsymbol{J}&\sum_{n0}^{\infty}\frac{1}{(n1)!}(\theta\boldsymbol{a}^{\wedge})^n \\ &\boldsymbol{I}\frac{1}{2!}\theta\boldsymbol{a}^{\wedge}\frac{1}{3!}(\theta\boldsymbol{a}^{\wedge})^2\frac{1}{4!}(\theta\boldsymbol{a}^{\wedge})^3\frac{1}{5!}(\theta\boldsymbol{a}^{\wedge})^4 \\ &\frac{1}{\theta}(\frac{1}{2!}\theta^2-\frac{1}{4!}\theta^4...)\boldsymbol{a}^{\wedge}\frac{1}{\theta}(\frac{1}{3!}\theta^3-\frac{1}{5!}\theta^5...)(\boldsymbol{a}^{\wedge})^2\boldsymbol{I} \\ &\frac{1-\cos\theta}{\theta}\boldsymbol{a}^{\wedge}\frac{\theta-\sin\theta}{\theta}(\boldsymbol{a}^{\wedge})^2\boldsymbol{I}\\ &\frac{1-\cos\theta}{\theta}\boldsymbol{a}^{\wedge}(1-\frac{\sin\theta}{\theta})(\boldsymbol{a}\boldsymbol{a}^{\mathrm{T}}-\boldsymbol{I})\boldsymbol{I}\\ &\frac{1-\cos\theta}{\theta}\boldsymbol{a}^{\wedge}(1-\frac{\sin\theta}{\theta})\boldsymbol{a}\boldsymbol{a}^{\mathrm{T}}-\boldsymbol{I}\frac{\sin\theta}{\theta}\boldsymbol{I}\boldsymbol{I}\\ &\frac{\sin\theta}{\theta}\boldsymbol{I}(1-\frac{\sin\theta}{\theta})\boldsymbol{a}\boldsymbol{a}^{\mathrm{T}}\frac{1-\cos\theta}{\theta}\boldsymbol{a}^{\wedge} \tag{4-2-5} \end{aligned} Jn0∑∞(n1)!1(θa∧)nI2!1θa∧3!1(θa∧)24!1(θa∧)35!1(θa∧)4θ1(2!1θ2−4!1θ4...)a∧θ1(3!1θ3−5!1θ5...)(a∧)2Iθ1−cosθa∧θθ−sinθ(a∧)2Iθ1−cosθa∧(1−θsinθ)(aaT−I)Iθ1−cosθa∧(1−θsinθ)aaT−IθsinθIIθsinθI(1−θsinθ)aaTθ1−cosθa∧(4-2-5)
注意这里用到式4-1-5 ( a ∧ ) 3 − a ∧ (\boldsymbol{a}^{\wedge})^3-\boldsymbol{a}^{\wedge} (a∧)3−a∧ 和 a a T − I a ∧ a ∧ \boldsymbol{a}\boldsymbol{a}^{\mathrm{T}}-\boldsymbol{I}\boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge} aaT−Ia∧a∧ 以及泰勒展开
cos θ 1 − 1 2 ! θ 2 1 4 ! θ 4 . . . \cos\theta1-\frac{1}{2!}\theta^2\frac{1}{4!}\theta^4... cosθ1−2!1θ24!1θ4...
sin θ θ − 1 3 ! θ 3 1 5 ! θ 5 . . . \sin\theta\theta-\frac{1}{3!}\theta^3\frac{1}{5!}\theta^5... sinθθ−3!1θ35!1θ5...
综上证毕。
CH4-3 李代数求导一、1 S O ( 3 ) \mathrm{SO(3)} SO(3) 直接求导
对极几何本质矩阵奇异值分解 矩阵内积和迹矩阵具有 弗罗比尼乌斯内积类似向量的内积。它被定义为两个相同大小的矩阵 A \boldsymbol{A} A 和 B \boldsymbol{B} B 的对应元素的积的和 即
< A , B > ∑ i 1 n ∑ j 1 n a i j b i j <\boldsymbol{A},\boldsymbol{B}>\sum_{i1}^{n}\sum_{j1}^na_{ij}b_{ij} <A,B>i1∑nj1∑naijbij
以 3 × 3 3\times 3 3×3 矩阵为例设
A [ a 11 a 12 a 13 a 21 a 22 a 23 a 31 a 32 a 33 ] B [ b 11 b 12 b 13 b 21 b 22 b 23 b 31 b 32 b 33 ] \boldsymbol{A}\left[\begin{array}{c} a_{11} & a_{12} & a_{13} \\ a_{21} & a_{22} & a_{23} \\ a_{31} & a_{32} & a_{33} \end{array}\right] \boldsymbol{B}\left[\begin{array}{c} b_{11} & b_{12} & b_{13} \\ b_{21} & b_{22} & b_{23} \\ b_{31} & b_{32} & b_{33} \end{array}\right] A a11a21a31a12a22a32a13a23a33 B b11b21b31b12b22b32b13b23b33
则有
< A , B > ∑ i 1 n ∑ j 1 n a i j b i j <\boldsymbol{A},\boldsymbol{B}>\sum_{i1}^{n}\sum_{j1}^na_{ij}b_{ij} <A,B>i1∑nj1∑naijbij
对于 A T B \boldsymbol{A}^{\mathrm{T}}\boldsymbol{B} ATB
A T B [ a 11 a 21 a 31 a 12 a 22 a 32 a 13 a 23 a 33 ] [ b 11 b 12 b 13 b 21 b 22 b 23 b 31 b 32 b 33 ] [ a 11 b 11 a 21 b 21 a 31 b 31 ? ? ? a 12 b 12 a 22 b 22 a 32 b 32 ? ? ? a 13 b 13 a 23 b 23 a 33 b 33 ] \boldsymbol{A}^{\mathrm{T}}\boldsymbol{B}\left[\begin{array}{c} a_{11} & a_{21} & a_{31} \\ a_{12} & a_{22} & a_{32} \\ a_{13} & a_{23} & a_{33} \end{array}\right] \left[\begin{array}{c} b_{11} & b_{12} & b_{13} \\ b_{21} & b_{22} & b_{23} \\ b_{31} & b_{32} & b_{33} \end{array}\right] \left[\begin{array}{c} a_{11}b_{11}a_{21}b_{21}a_{31}b_{31} & ? & ? \\ ? & a_{12}b_{12}a_{22}b_{22}a_{32}b_{32} & ? \\ ? & ? & a_{13}b_{13}a_{23}b_{23}a_{33}b_{33} \end{array}\right] ATB a11a12a13a21a22a23a31a32a33 b11b21b31b12b22b32b13b23b33 a11b11a21b21a31b31???a12b12a22b22a32b32???a13b13a23b23a33b33
则 T r ( A T B ) \mathrm{Tr}(\boldsymbol{A}^{\mathrm{T}}\boldsymbol{B}) Tr(ATB)等于
T r ( A T B ) a 11 b 11 a 21 b 21 a 31 b 31 a 12 b 12 a 22 b 22 a 32 b 32 a 13 b 13 a 23 b 23 a 33 b 33 ∑ i 1 n ∑ j 1 n a i j b i j \mathrm{Tr}(\boldsymbol{A}^{\mathrm{T}}\boldsymbol{B})a_{11}b_{11}a_{21}b_{21}a_{31}b_{31}a_{12}b_{12}a_{22}b_{22}a_{32}b_{32}a_{13}b_{13}a_{23}b_{23}a_{33}b_{33}\sum_{i1}^{n}\sum_{j1}^na_{ij}b_{ij} Tr(ATB)a11b11a21b21a31b31a12b12a22b22a32b32a13b13a23b23a33b33i1∑nj1∑naijbij
也就是说 A T B \boldsymbol{A}^{\mathrm{T}}\boldsymbol{B} ATB 的迹等于两矩阵对应元素相乘的积的和。