[视觉SLAM十四讲]学习笔记5-李群与李代数基础及指数/对数映射
在之前的学习中,我们知道了三维世界刚体运动的一些描述方式,例如:旋转矩阵、欧拉角、四元数等表示方式,但是SLAM中除了表示这些运动,还要进行估计和优化。因为在SLAM中位姿是未知的,我们需要解决例如 “相机处于一个什么样的位姿才能观测出最真实的数据” 这样的问题,这是一个典型的最优化问题,求解最优的旋转矩阵 R R R和平移向量 t t t,使误差变得最小。
我们知道,旋转矩阵自身是带有正交且横列式需为1的约束,若把旋转矩阵当作优化变量,这会增加额外的约束条件,使优化变得困难。而通过李群—李代数间的转换关系,我们希望可以把位姿估计变成无约束优化问题,这将简化求解问题。下面我们进行李群—李代数相关知识的介绍和学习。
1 群和李群
1.1 群的引出
在之前的学习中,我们知道了旋转矩阵和变换矩阵的定义。同时,我们说三维旋转矩阵构成了特殊正交群SO(3),变换矩阵构成了特殊欧式群SE(3),让我们再看一下他们的形式: S O ( 3 ) = { R ∈ R 3 × 3 ∣ R R T = I , d e t ( R ) = 1 } . (1.1) \rm{SO(3)}=\{\bm{R}\in \mathbb{R}^{3\times 3}|\bm{RR}^{T}=I,\rm{det}(\bm{R})=1\}.\tag{1.1} SO(3)={R∈R3×3∣RRT=I,det(R)=1}.(1.1) S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } . (1.2) \rm{SE(3)}=\Bigg\{\bm{T}=\begin{bmatrix}\bm{R}&\bm{t} \\ \bm{0}^T&1\end{bmatrix}\in \mathbb{R}^{4\times 4}|\bm{R}\in SO(3),\bm{t}\in \mathbb{R}^3\Bigg\}.\tag{1.2} SE(3)={T=[R0Tt1]∈R4×4∣R∈SO(3),t∈R3}.(1.2)我们通过简单的例子计算可以发现,旋转矩阵和变换矩阵对加法运算是不封闭的。即对于两个任意的旋转矩阵 R 1 \bm{R}_1 R1和 R 2 \bm{R}_2 R2,按照矩阵加法运算法则得到的新的矩阵,不再满足旋转矩阵的定义: R 1 + R 2 ∉ S O ( 3 ) , T 1 + T 2 ∉ S E ( 3 ) (1.3) \bm{R}_1+\bm{R}_2\notin \rm{SO(3)},\bm{T}_1+\bm{T}_2\notin \rm{SE(3)}\tag{1.3} R1+R2∈/SO(3),T1+T2∈/SE(3)(1.3)换一种说法,我们也可以说没有找到一种合适的加法运算的定义,或者常规的矩阵加法对这两个集合不封闭。不过,他们的乘法运算是较好的,并且只有这一种。 S O ( 3 ) \rm{SO(3)} SO(3)和 S E ( 3 ) \rm{SE}(3) SE(3)关于乘法是封闭的: R 1 R 2 ∈ S O ( 3 ) , T 1 T 2 ∈ S E ( 3 ) . (1.4) \bm{R}_1\bm{R}_2\in \rm{SO(3)},\bm{T}_1\bm{T}_2\in \rm{SE(3)}.\tag{1.4} R1R2∈SO(3),T1T2∈SE(3).(1.4)同时,我们也可以对任何一个旋转矩阵或变换矩阵在乘法的意义上求逆。而乘法所对应的是旋转或者变换的复合,两个旋转矩阵相乘表示做了两次旋转。对于这种只有一个(合适的/保证集合封闭)运算的集合,我们称之为群。
1.2 群的性质及李群定义
群(group)的官方定义为:群是一种集合加上一种运算的代数结构。若把集合记作 A A A,运算记作 ⋅ \cdot ⋅,群就可以记作 G = ( A , ⋅ ) G=(A,\cdot) G=(A,⋅)。群的运算需要满足以下条件:
- 封闭性 ∀ a 1 , a 2 ∈ A , a 1 ⋅ a 2 ∈ A . {\forall}a_1,a_2\in A,\quad a_1\cdot a_2 \in A. ∀a1,a2∈A,a1⋅a2∈A.
- 结合律 ∀ a 1 , a 2 , a 3 ∈ A , ( a 1 ⋅ a 2 ) ⋅ a 3 = a 1 ⋅ ( a 2 ⋅ a 3 ) . {\forall}a_1,a_2,a_3\in A,\quad(a_1\cdot a_2)\cdot a_3=a_1\cdot (a_2\cdot a_3). ∀a1,a2,a3∈A,(a1⋅a2)⋅a3=a1⋅(a2⋅a3).
- 幺元 ∃ a 0 ∈ A , s . t . ∀ ∈ A , a 0 ⋅ a = a ⋅ a 0 = a . {\exist}a_0\in A,\quad\rm{s.t.}{\forall}\in A,\quad a_0\cdot a=a \cdot a_0=a. ∃a0∈A,s.t.∀∈A,a0⋅a=a⋅a0=a.
- 逆 ∀ a ∈ A , ∃ a − 1 ∈ A , s . t . a ⋅ a − 1 = a 0 . {\forall}a\in A,\quad{\exist}a^{-1}\in A,\quad\rm{s.t.}\quad a \cdot a^{-1}=a_0. ∀a∈A,∃a−1∈A,s.t.a⋅a−1=a0.
通过以上的条件,我们可以验证旋转/变换矩阵和矩阵乘法都构成群。矩阵中常见的群有:
- 一般线性群 G L ( n ) {GL}(n) GL(n) \quad 指 n × n n \times n n×n的可你矩阵,它们对矩阵乘法成群。
- 特殊正交群 S O ( n ) {SO}(n) SO(n) \quad 即所谓的旋转矩阵群,最为常见的是 S O ( 2 ) \rm{SO}(2) SO(2)和 S O ( 3 ) \rm{SO}(3) SO(3)。
- 特殊欧式群 S E ( n ) {SE}(n) SE(n) \quad 即 n n n维欧式变换,如 S E ( 2 ) \rm{SE}(2) SE(2)和 S E ( 3 ) \rm{SE}(3) SE(3)。
下面介绍一下李群。
李群是指具有连续(光滑)性质的群。整数群
Z
\mathbb{Z}
Z是离散的群没有连续性质,所以不是李群。而
S
O
(
n
)
\rm{SO}(n)
SO(n)和
S
E
(
n
)
\rm{SE}(n)
SE(n)由于在实数空间上是连续的,所以我们能够直观地想象一个刚体能够连续地在空间中运动,因此它们都是李群。
S
O
(
n
)
\rm{SO}(n)
SO(n)和
S
E
(
n
)
\rm{SE}(n)
SE(n)对相机姿态估计尤其重要,我们主要讨论这两个李群。
在视觉SLAM十四讲中,作者从旋转矩阵引出李代数,从简单的
S
O
(
3
)
\rm{SO}(3)
SO(3)进行讨论,引出
S
O
(
3
)
\rm{SO}(3)
SO(3)上面的李代数
s
o
(
3
)
\mathfrak{so}(3)
so(3)。
2 李代数
2.1 李代数的引出
对于任意的旋转矩阵 R \bm{R} R,满足: R R T = I (2.1) \bm{R}\bm{R}^T=\bm{I}\tag{2.1} RRT=I(2.1)当 R \bm{R} R是某个相机的旋转时,它将是一个随时间连续变化的时间函数: R ( t ) \bm{R}(t) R(t),这时仍然满足: R ( t ) R T ( t ) = I (2.2) \bm{R}(t)\bm{R}^T(t)=\bm{I}\tag{2.2} R(t)RT(t)=I(2.2)同时对等式两边对时间求导,得到 R ˙ ( t ) R T ( t ) + R ( t ) R ˙ ( t ) T = 0 ⟺ R ˙ ( t ) R T ( t ) = − ( R ˙ ( t ) R T ( t ) ) T (2.3) \dot{\bm{R}}(t)\bm{R}^T(t)+\bm{R}(t)\dot{\bm{R}}(t)^T=0 \quad \Longleftrightarrow \quad \dot{\bm{R}}(t)\bm{R}^T(t)=-(\dot{\bm{R}}(t)\bm{R}^T(t))^T\tag{2.3} R˙(t)RT(t)+R(t)R˙(t)T=0⟺R˙(t)RT(t)=−(R˙(t)RT(t))T(2.3)显然, R ˙ ( t ) R T ( t ) \dot{\bm{R}}(t)\bm{R}^T(t) R˙(t)RT(t)是一个反对称矩阵。在之前旋转变换的知识中,引入的 ∧ ^{\land} ∧符号,可以将向量变成反对称矩阵。同理,我们也可以找到一个唯一与反对称矩阵对应的矩阵,我们可以用 ∨ ^\vee ∨这个符号表示: a ∧ = A = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] , A ∨ = a (2.4) a^{\land}=\bm{A}=\begin{bmatrix}0&-a_3&a_2 \\ a_3&0&-a_1 \\ -a_2&a_1&0\end{bmatrix},\quad \bm{A}^{\vee}=a\tag{2.4} a∧=A= 0a3−a2−a30a1a2−a10 ,A∨=a(2.4)对于反对称矩阵 R ˙ ( t ) R T ( t ) \dot{\bm{R}}(t)\bm{R}^T(t) R˙(t)RT(t),我们同样可以找到一个三维向量 ϕ ( t ) ∈ R 3 \bm{\phi}(t)\in\mathbb{R}^3 ϕ(t)∈R3与之对应: R ˙ ( t ) R T ( t ) = ϕ ( t ) ∧ . (2.5) \dot{\bm{R}}(t)\bm{R}^T(t)=\bm{\phi}(t)^{\land}.\tag{2.5} R˙(t)RT(t)=ϕ(t)∧.(2.5)等式两边右乘 R ( t ) \bm{R}(t) R(t),有: R ˙ ( t ) = ϕ ( t ) ∧ R ( t ) = [ 0 − ϕ 3 ϕ 2 ϕ 3 0 − ϕ 1 − ϕ 2 ϕ 1 0 ] R ( t ) (2.6) \dot{\bm{R}}(t)=\bm{\phi}(t)^{\land}\bm{R}(t)=\begin{bmatrix}0&-\phi_3&\phi_2 \\ \phi_3&0&-\phi_1 \\ -\phi_2&\phi_1&0\end{bmatrix}\bm{R}(t)\tag{2.6} R˙(t)=ϕ(t)∧R(t)= 0ϕ3−ϕ2−ϕ30ϕ1ϕ2−ϕ10 R(t)(2.6)显然,对旋转矩阵每次求导,只需要左乘一个 ϕ ( t ) ∧ \bm{\phi}(t)^{\land} ϕ(t)∧即可。现在我们考虑 t 0 = 0 t_0=0 t0=0的情况,设此时的旋转矩阵 R ( 0 ) = I \bm{R}(0)=\bm{I} R(0)=I。根据导数的定义,我们将 R ( t ) \bm{R}(t) R(t)在 t = 0 t=0 t=0附近一阶泰勒展开: R ( t ) ≈ R ( t 0 ) + R ˙ ( t 0 ) ( t − t 0 ) = I + ϕ ( t 0 ) ∧ ( t ) . (2.7) \bm{R}(t) \approx\bm{R}(t_0)+\dot{\bm{R}}(t_0)(t-t_0)=\bm{I}+\bm{\phi}(t_0)^{\land}(t).\tag{2.7} R(t)≈R(t0)+R˙(t0)(t−t0)=I+ϕ(t0)∧(t).(2.7)这里, ϕ \bm{\phi} ϕ反映了 R \bm{R} R的导数性质,我们称它在 S O ( 3 ) \rm{SO}(3) SO(3)原点附近的正切空间(Tangent Space)上。在 t 0 t_0 t0附近,设 ϕ \bm{\phi} ϕ保持为常数 ϕ ( t 0 ) = ϕ 0 \bm{\phi}(t_0)=\bm{\phi}_0 ϕ(t0)=ϕ0。根据式(2.6),有: R ˙ ( t ) = ϕ ( t 0 ) ∧ R ( t ) = ϕ 0 R ( t ) . (2.8) \dot{\bm{R}}(t)=\bm{\phi}(t_0)^{\land}\bm{R}(t)=\phi_0\bm{R}(t).\tag{2.8} R˙(t)=ϕ(t0)∧R(t)=ϕ0R(t).(2.8)上式为一个关于 R \bm{R} R的微分方程,初值为 R ( 0 ) = I \bm{R}(0)=\bm{I} R(0)=I,解得: R ( t ) = e x p ( ϕ 0 ∧ t ) . (2.9) \bm{R}(t)=\rm{exp}(\bm{\phi}^{\land}_0t).\tag{2.9} R(t)=exp(ϕ0∧t).(2.9)这个结果说明在 t = 0 t=0 t=0附近,旋转矩阵可以由 e x p ( ϕ 0 ∧ t ) \rm{exp}(\bm{\phi}^{\land}_0t) exp(ϕ0∧t)计算出来。同时,旋转矩阵 R \bm{R} R与另一个反对称矩阵 ϕ 0 ∧ t \bm{\phi}^{\land}_0t ϕ0∧t之间通过指数关系联系,这里有两个问题需要我们来解释:
- 给定某时刻的 R \bm{R} R,有一个对应的 ϕ \bm{\phi} ϕ,它描述了 R \bm{R} R在某时刻局部的导数关系,而 ϕ \bm{\phi} ϕ正是对应到 S O ( 3 ) \rm{SO}(3) SO(3)上的李代数 s o ( 3 ) \mathfrak{so}(3) so(3)。
- 给定某个向量
ϕ
\bm{\phi}
ϕ时,矩阵指数
e
x
p
(
ϕ
∧
)
\rm{exp}(\bm{\phi}^{\land})
exp(ϕ∧)如何计算得到?反之,给定
R
\bm{R}
R时,能否有相反的运算来计算
ϕ
\bm{\phi}
ϕ?这正是李群与李代数之间的指数/对数映射关系。
下面我们通过李代数的定义及 s o ( 3 ) \mathfrak{so}(3) so(3)和 s e ( 3 ) \mathfrak{se}(3) se(3)两个李代数的介绍来引出李群与李代数之间的指数/对数映射关系。
2.2 李代数的定义
在《旋量代数与李群、李代数》这本书中,给出了李代数的概念如下:
李代数是研究李群、微分流形和微小变换等几何体的代数工具. 瞬时旋量是李群
S
E
(
3
)
\rm{SE}(3)
SE(3)的射影李代数的元素。在20 世纪40 年代德国数学家与理论物理学家
H
e
r
m
a
n
n
W
e
y
l
Hermann Weyl
HermannWeyl(1944) 引进术语"李代数" 之前,李代数一直被称作"无穷小群",即群元素非常接近恒等, 为李群流形在单位元处的切空间。
每个李群都有与之对应的李代数。李代数描述了李群的局部性质,准确地说,是单位圆附近的正切空间。一般的李代数定义如下:
李代数由一个集合
V
\mathbb{V}
V、一个数域
F
\mathbb{F}
F和一个二元运算[,]组成。如果他们满足以下几条性质,则称
(
V
,
F
,
[
,
]
)
(\mathbb{V},\mathbb{F},[,])
(V,F,[,])为一个李代数,记作
g
\mathfrak{g}
g。它具有以下性质:
- 封闭性 ∀ X , Y ∈ V , [ X , Y ] ∈ V . \quad {\forall}\bm{X},\bm{Y}\in \mathbb{V},[\bm{X},\bm{Y}]\in \mathbb{V}. ∀X,Y∈V,[X,Y]∈V.
- 双线性 ∀ X , Y , Z ∈ V , a , b ∈ F , \quad {\forall}\bm{X},\bm{Y},\bm{Z}\in \mathbb{V},a,b\in \mathbb{F}, ∀X,Y,Z∈V,a,b∈F,有 [ a X + b Y , Z ] = a [ X , Z ] + b [ Y , Z ] , [ Z , a X + b Y ] = a [ Z , X ] + b [ Z , Y ] . [a\bm{X}+b\bm{Y},\bm{Z}]=a[\bm{X},\bm{Z}]+b[\bm{Y},\bm{Z}], \quad [\bm{Z},a\bm{X}+b\bm{Y}]=a[\bm{Z},\bm{X}]+b[\bm{Z},\bm{Y}]. [aX+bY,Z]=a[X,Z]+b[Y,Z],[Z,aX+bY]=a[Z,X]+b[Z,Y].
- 自反性 ∀ X ∈ V , [ X , X ] = 0 \quad {\forall}\bm{X}\in \mathbb{V},[\bm{X},\bm{X}]=0 ∀X∈V,[X,X]=0.
- 雅可比等价 ∀ X , Y , Z ∈ V , [ X , [ Y , Z ] ] + [ Z , [ X , Y ] ] + [ Y , [ Z , X ] ] = 0. \quad {\forall}\bm{X},\bm{Y},\bm{Z}\in \mathbb{V},[\bm{X},[\bm{Y},\bm{Z}]]+[\bm{Z},[\bm{X},\bm{Y}]]+[\bm{Y},[\bm{Z},\bm{X}]]=0. ∀X,Y,Z∈V,[X,[Y,Z]]+[Z,[X,Y]]+[Y,[Z,X]]=0.
这里的二元运算被称为李括号。和群中简单的二元运算相比,李括号表达了两个元素的差异,它不遵循结合律,而是要求元素和自己做李括号最后为零的性质。一个很简单的例子是三维向量 R 3 \mathbb{R}^3 R3上定义的叉乘(外积、叉积)是一种李括号,所以 g = ( R 3 , R , × ) \mathfrak{g}=(\mathbb{R}^3,\mathbb{R},\times) g=(R3,R,×)构成了一个李代数。
2.3 李代数 s o ( 3 ) \mathfrak{so}(3) so(3)和 s e ( 3 ) \mathfrak{se}(3) se(3)
2.3.1 李代数 s o ( 3 ) \mathfrak{so}(3) so(3)
我们在上一节引入的 ϕ \bm{\phi} ϕ实际上是一种李代数, S O ( 3 ) \rm{SO}(3) SO(3)对应的李代数是定义在 R 3 \mathbb{R}^3 R3上的向量,我们可以记作 ϕ \bm{\phi} ϕ。根据上一节的推导,每个 ϕ \bm{\phi} ϕ都可以生成一个反对称矩阵: Φ = ϕ ∧ = [ 0 − ϕ 3 ϕ 2 ϕ 3 0 − ϕ 1 − ϕ 2 ϕ 1 0 ] ∈ R 3 × 3 . (2.10) \Phi=\bm{\phi}^{\land}=\begin{bmatrix}0&-\phi_3&\phi_2 \\ \phi_3&0&-\phi_1 \\ -\phi_2&\phi_1&0\end{bmatrix}\in \mathbb{R}^{3 \times 3}.\tag{2.10} Φ=ϕ∧= 0ϕ3−ϕ2−ϕ30ϕ1ϕ2−ϕ10 ∈R3×3.(2.10)在此定义下的两个向量 ϕ 1 \bm{\phi}_1 ϕ1, ϕ 2 \bm{\phi}_2 ϕ2的李括号为 [ ϕ 1 , ϕ 2 ] = ( Φ 1 Φ 2 − Φ 2 Φ 1 ) ∧ . (2.11) [\bm{\phi}_1,\bm{\phi}_2]=(\Phi_1\Phi_2-\Phi_2\Phi_1)^{\land}.\tag{2.11} [ϕ1,ϕ2]=(Φ1Φ2−Φ2Φ1)∧.(2.11)由于向量 ϕ \bm{\phi} ϕ和反对称矩阵一一对应的事实,我们可以在不引起歧义的情况下,说 s o ( 3 ) \mathfrak{so}(3) so(3)的元素是三维向量或三维反对称矩阵,不加区别: s o ( 3 ) = { ϕ ∈ R 3 , Φ = ϕ ∧ ∈ R 3 × 3 } . (2.12) \mathfrak{so}(3)=\{\phi \in \mathbb{R}^3,\Phi=\bm{\phi}^{\land}\in\mathbb{R}^{3 \times 3} \}.\tag{2.12} so(3)={ϕ∈R3,Φ=ϕ∧∈R3×3}.(2.12)现在,我们清楚了 s o ( 3 ) \mathfrak{so}(3) so(3)的内容:它们是一个由三维向量组成的集合,每个向量都对应一个反对称矩阵,可以用来表达旋转矩阵的导数。它和 S O ( 3 ) \rm{SO}(3) SO(3)之间的关系通过指数映射: R = e x p ( ϕ ∧ ) (2.13) \bm{R}=\rm{exp}(\bm{\phi}^{\land})\tag{2.13} R=exp(ϕ∧)(2.13)指数映射将在第三节介绍。
2.3.2 李代数 s e ( 3 ) \mathfrak{se}(3) se(3)
对于 S E ( 3 ) \rm{SE}(3) SE(3),它对应的李代数为 s e ( 3 ) \mathfrak{se}(3) se(3)。与 s o ( 3 ) \mathfrak{so}(3) so(3)相似, s e ( 3 ) \mathfrak{se}(3) se(3)位于 R 6 \mathbb{R}^6 R6空间中: s e ( 3 ) = { ξ = [ ρ ϕ ] ∈ R 6 , ρ ∈ R 3 , ϕ ∈ s o ( 3 ) , ξ ∧ = [ ϕ ∧ ρ 0 T 0 ] ∈ R 4 × 4 } . (2.14) \mathfrak{se}(3)=\Bigg\{ \bm{\xi}=\begin{bmatrix}\bm{\rho} \\ \bm{\phi}\end{bmatrix}\in \mathbb{R}^6,\bm{\rho} \in \mathbb{R}^3,\bm{\phi} \in \mathfrak{so}(3),\bm{\xi}^{\land}=\begin{bmatrix}\bm{\phi}^{\land}&\bm{\rho} \\ 0^T&0\end{bmatrix}\in \mathbb{R}^{4 \times 4} \Bigg\}.\tag{2.14} se(3)={ξ=[ρϕ]∈R6,ρ∈R3,ϕ∈so(3),ξ∧=[ϕ∧0Tρ0]∈R4×4}.(2.14)我们可以用 ξ \bm{\xi} ξ来表示 s e ( 3 ) \mathfrak{se}(3) se(3)的每个元素,它是一个六维向量。它的前三维是平移,但不同于之前变换矩阵中的平移,记作 ρ \bm{\rho} ρ;后三维是旋转,记作 ϕ \bm{\phi} ϕ,实质上是 s o ( 3 ) \mathfrak{so}(3) so(3)元素。在 s o ( 3 ) \mathfrak{so}(3) so(3)中,我们同样使用 ∧ ^{\land} ∧符号,表示将一个六维向量转换成四维矩阵,不在表示反对称: ξ ∧ = [ ϕ ∧ ρ 0 T 0 ] ∈ R 4 × 4 . (2.15) \bm{\xi}^{\land}=\begin{bmatrix}\bm{\phi}^{\land}&\bm{\rho} \\ 0^T&0\end{bmatrix}\in \mathbb{R}^{4 \times 4}.\tag{2.15} ξ∧=[ϕ∧0Tρ0]∈R4×4.(2.15)我们这里仍然使用 l a n d ^{land} land和 ∨ ^{\vee} ∨符号来表示"从向量到矩阵"和"从矩阵到向量"的关系,来保持和 s o ( 3 ) \mathfrak{so}(3) so(3)的一致性。并且仍然是一一对应的关系。我们可以简单的把 s e ( 3 ) \mathfrak{se}(3) se(3)理解为"由一个平移加上一个 s o ( 3 ) \mathfrak{so}(3) so(3)元素构成的向量"(注意:这里的 ρ \bm{\rho} ρ不是直接的平移)。类似于 s o ( 3 ) \mathfrak{so}(3) so(3)的李括号,李代数 s e ( 3 ) \mathfrak{se}(3) se(3)的李括号: [ ξ 1 , ξ 2 ] = ( ξ 1 ∧ ξ 2 ∧ − ξ 2 ∧ ξ 1 ∧ ) ∧ . (2.16) [\bm{\xi}_1,\bm{\xi}_2]=(\bm{\xi}_1^{\land}\bm{\xi}_2^{\land}-\bm{\xi}_2^{\land}\bm{\xi}_1^{\land})^{\land}.\tag{2.16} [ξ1,ξ2]=(ξ1∧ξ2∧−ξ2∧ξ1∧)∧.(2.16)
3 指数与对数映射
现在我们来解决第二个问题,即如何计算 e x p ( ϕ ∧ ) \rm{exp}(\bm{\phi}^{\land}) exp(ϕ∧)?我们可以看出,这是一个矩阵的指数,在李群和李代数中称为指数映射(Exponential Map)。我们首先讨论 s o ( 3 ) \mathfrak{so}(3) so(3)上的指数映射。
3.1 S O ( 3 ) \rm{SO}(3) SO(3)上的指数映射
我们知道,任意一个矩阵的指数映射可以由泰勒展开表示,但是只有当收敛的情况下才会有结果,其结果仍然是一个矩阵:
e
x
p
(
A
)
=
∑
n
=
0
∞
1
n
!
A
n
.
(3.1)
\rm{exp}(\bm{A})=\sum^{\infty}_{n=0}\frac{1}{n!}\bm{A}^n.\tag{3.1}
exp(A)=n=0∑∞n!1An.(3.1)对于
s
o
(
3
)
\mathfrak{so}(3)
so(3)中的任意元素
ϕ
\bm{\phi}
ϕ,可以按照同样的定义表示它的指数映射:
e
x
p
(
ϕ
∧
)
=
∑
n
=
0
∞
1
n
!
(
ϕ
∧
)
n
.
(3.2)
\rm{exp}(\bm{\phi}^{\land})=\sum^{\infty}_{n=0}\frac{1}{n!}({\bm{\phi}^{\land}})^n.\tag{3.2}
exp(ϕ∧)=n=0∑∞n!1(ϕ∧)n.(3.2)这里的矩阵无穷次幂计算起来非常麻烦,这里采用一种计算指数映射的简便方法。我们通过定义旋转向量的方式,定义
ϕ
\bm{\phi}
ϕ(三维向量),定义他的模长为
θ
\theta
θ,方向为
a
\bm{a}
a(长度为1的单位向量,
∥
a
∥
=
1
,
a
1
2
+
a
2
2
+
a
3
2
=
1
\Vert \bm{a} \Vert=1,a^2_1+a^2_2+a^2_3=1
∥a∥=1,a12+a22+a32=1),所以有
ϕ
=
θ
a
\bm{\phi}=\theta\bm{a}
ϕ=θa。对于
a
∧
\bm{a}^{\land}
a∧,我们有以下两条性质:
a
∧
a
∧
=
[
−
a
2
2
−
a
3
2
a
1
a
2
a
1
a
3
a
1
a
2
−
a
1
2
−
a
3
2
a
2
a
3
a
1
a
3
a
2
a
3
−
a
1
2
−
a
2
2
]
=
a
a
T
−
I
.
(3.3)
\bm{a}^{\land}\bm{a}^{\land}=\begin{bmatrix}-a^2_2-a^2_3 & a_1a_2 & a_1a_3 \\ a_1a_2 & -a^2_1-a^2_3 & a_2a_3 \\ a_1a_3 & a_2a_3 & -a^2_1-a^2_2\end{bmatrix}=\bm{a}\bm{a}^T-\bm{I}.\tag{3.3}
a∧a∧=
−a22−a32a1a2a1a3a1a2−a12−a32a2a3a1a3a2a3−a12−a22
=aaT−I.(3.3)
a
∧
a
∧
a
∧
=
a
∧
(
a
a
T
−
I
)
=
−
a
∧
.
(3.4)
\bm{a}^{\land}\bm{a}^{\land}\bm{a}^{\land}=\bm{a}^{\land}(\bm{a}\bm{a}^T-\bm{I})=-\bm{a}^{\land}.\tag{3.4}
a∧a∧a∧=a∧(aaT−I)=−a∧.(3.4)便于理解,下面我们来简单推导一下这个性质
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
2
2
−
a
3
2
a
1
a
2
a
1
a
3
a
1
a
2
−
a
1
2
−
a
3
2
a
2
a
3
a
1
a
3
a
2
a
3
−
a
1
2
−
a
2
2
]
\bm{a}^{\land}\bm{a}^{\land}=\begin{bmatrix}0&-a_3&a_2 \\ a_3&0&-a_1 \\ -a_2&a_1&0\end{bmatrix}\begin{bmatrix}0&-a_3&a_2 \\ a_3&0&-a_1 \\ -a_2&a_1&0\end{bmatrix}=\begin{bmatrix}-a^2_2-a^2_3 & a_1a_2 & a_1a_3 \\ a_1a_2 & -a^2_1-a^2_3 & a_2a_3 \\ a_1a_3 & a_2a_3 & -a^2_1-a^2_2\end{bmatrix}
a∧a∧=
0a3−a2−a30a1a2−a10
0a3−a2−a30a1a2−a10
=
−a22−a32a1a2a1a3a1a2−a12−a32a2a3a1a3a2a3−a12−a22
=
[
a
1
2
−
1
a
1
a
2
a
1
a
3
a
1
a
2
a
2
2
−
1
a
2
a
3
a
1
a
3
a
2
a
3
a
3
2
−
1
]
=
[
a
1
2
a
1
a
2
a
1
a
3
a
1
a
2
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
]
=\begin{bmatrix}a^2_1-1 & a_1a_2 & a_1a_3 \\ a_1a_2 & a^2_2-1 & a_2a_3 \\ a_1a_3 & a_2a_3 & a^2_3-1\end{bmatrix}=\begin{bmatrix}a^2_1 & a_1a_2 & a_1a_3 \\ a_1a_2 & a^2_2 & a_2a_3 \\ a_1a_3 & a_2a_3 & a^2_3\end{bmatrix}-\begin{bmatrix}1&0&0 \\ 0&1&0 \\ 0&0&1\end{bmatrix}
=
a12−1a1a2a1a3a1a2a22−1a2a3a1a3a2a3a32−1
=
a12a1a2a1a3a1a2a22a2a3a1a3a2a3a32
−
100010001
=
[
a
1
a
2
a
3
]
[
a
1
a
2
a
3
]
−
[
1
0
0
0
1
0
0
0
1
]
=
a
a
T
−
I
(3.4)
=\begin{bmatrix}a_1 \\ a_2 \\ a_3\end{bmatrix}\begin{bmatrix}a_1& a_2&a_3\end{bmatrix}-\begin{bmatrix}1&0&0 \\ 0&1&0 \\ 0&0&1\end{bmatrix}=\bm{a}\bm{a}^T-\bm{I}\tag{3.4}
=
a1a2a3
[a1a2a3]−
100010001
=aaT−I(3.4)
a
∧
a
∧
a
∧
=
a
∧
(
a
a
T
−
I
)
=
[
0
−
a
3
a
2
a
3
0
−
a
1
−
a
2
a
1
0
]
[
−
a
2
2
−
a
3
2
a
1
a
2
a
1
a
3
a
1
a
2
−
a
1
2
−
a
3
2
a
2
a
3
a
1
a
3
a
2
a
3
−
a
1
2
−
a
2
2
]
\bm{a}^{\land}\bm{a}^{\land}\bm{a}^{\land}=\bm{a}^{\land}(\bm{a}\bm{a}^T-\bm{I})=\begin{bmatrix}0&-a_3&a_2 \\ a_3&0&-a_1 \\ -a_2&a_1&0\end{bmatrix}\begin{bmatrix}-a^2_2-a^2_3 & a_1a_2 & a_1a_3 \\ a_1a_2 & -a^2_1-a^2_3 & a_2a_3 \\ a_1a_3 & a_2a_3 & -a^2_1-a^2_2\end{bmatrix}
a∧a∧a∧=a∧(aaT−I)=
0a3−a2−a30a1a2−a10
−a22−a32a1a2a1a3a1a2−a12−a32a2a3a1a3a2a3−a12−a22
=
[
0
−
a
3
a
2
a
3
0
−
a
1
−
a
2
a
1
0
]
[
a
1
2
−
1
a
1
a
2
a
1
a
3
a
1
a
2
a
2
2
−
1
a
2
a
3
a
1
a
3
a
2
a
3
a
3
2
−
1
]
=
[
0
a
3
−
a
2
−
a
3
0
a
1
a
2
−
a
1
0
]
=
−
a
∧
(3.5)
=\begin{bmatrix}0&-a_3&a_2 \\ a_3&0&-a_1 \\ -a_2&a_1&0\end{bmatrix}\begin{bmatrix}a^2_1-1 & a_1a_2 & a_1a_3 \\ a_1a_2 & a^2_2-1 & a_2a_3 \\ a_1a_3 & a_2a_3 & a^2_3-1\end{bmatrix}=\begin{bmatrix}0&a_3&-a_2 \\ -a_3&0&a_1 \\ a_2&-a_1&0\end{bmatrix}=-\bm{a}^{\land}\tag{3.5}
=
0a3−a2−a30a1a2−a10
a12−1a1a2a1a3a1a2a22−1a2a3a1a3a2a3a32−1
=
0−a3a2a30−a1−a2a10
=−a∧(3.5)显然,这两个式子可以为我们处理
a
∧
\bm{a}^{\land}
a∧的高阶项问题。我们可以把指数映射写成:
e
x
p
(
ϕ
∧
)
=
e
x
p
(
θ
a
∧
)
=
∑
n
=
0
∞
1
n
!
(
θ
a
∧
)
n
\rm{exp}(\bm{\phi}^{\land})=\rm{exp}(\theta\bm{a}^{\land})=\sum^{\infty}_{n=0}\frac{1}{n!}(\theta\bm{a}^{\land})^n
exp(ϕ∧)=exp(θa∧)=n=0∑∞n!1(θa∧)n
=
I
+
θ
a
∧
+
1
2
!
θ
2
a
∧
a
∧
+
1
3
!
θ
3
a
∧
a
∧
a
∧
+
1
4
!
θ
4
(
a
∧
)
4
+
⋯
=\bm{I}+\theta\bm{a}^{\land}+\frac{1}{2!}\theta^2\bm{a}^{\land}\bm{a}^{\land}+\frac{1}{3!}\theta^3\bm{a}^{\land}\bm{a}^{\land}\bm{a}^{\land}+\frac{1}{4!}\theta^4(\bm{a}^{\land})^4+\cdots
=I+θa∧+2!1θ2a∧a∧+3!1θ3a∧a∧a∧+4!1θ4(a∧)4+⋯
=
a
a
T
−
a
∧
a
∧
+
θ
a
∧
+
1
2
!
θ
2
a
∧
a
∧
−
1
3
!
θ
3
a
∧
−
1
4
!
θ
4
(
a
∧
)
2
+
⋯
=\bm{aa}^T-\bm{a}^{\land}\bm{a}^{\land}+\theta\bm{a}^{\land}+\frac{1}{2!}\theta^2\bm{a}^{\land}\bm{a}^{\land}-\frac{1}{3!}\theta^3\bm{a}^{\land}-\frac{1}{4!}\theta^4(\bm{a}^{\land})^2+ \cdots
=aaT−a∧a∧+θa∧+2!1θ2a∧a∧−3!1θ3a∧−4!1θ4(a∧)2+⋯
=
a
a
T
+
(
θ
−
1
3
!
θ
3
+
1
5
!
θ
5
−
⋯
)
⏟
s
i
n
θ
的泰勒展开式
a
∧
−
(
1
−
1
2
!
θ
2
+
1
4
!
θ
4
−
⋯
)
⏟
c
o
s
θ
的泰勒展开式
a
∧
a
∧
=\bm{aa}^T+\underbrace{(\theta-\frac{1}{3!}\theta^3+\frac{1}{5!}\theta^5- \cdots)}_{sin\theta的泰勒展开式}\bm{a}^{\land}-\underbrace{(1-\frac{1}{2!}\theta^2+\frac{1}{4!}\theta^4- \cdots)}_{cos\theta的泰勒展开式}\bm{a}^{\land}\bm{a}^{\land}
=aaT+sinθ的泰勒展开式
(θ−3!1θ3+5!1θ5−⋯)a∧−cosθ的泰勒展开式
(1−2!1θ2+4!1θ4−⋯)a∧a∧
=
a
∧
a
∧
+
I
+
s
i
n
θ
a
∧
−
c
o
s
θ
a
∧
a
∧
=
(
1
−
c
o
s
θ
)
a
∧
a
∧
+
I
+
s
i
n
θ
a
∧
=\bm{a}^{\land}\bm{a}^{\land}+\bm{I}+sin\theta\bm{a}^{\land}-cos\theta\bm{a}^{\land}\bm{a}^{\land}=(1-cos\theta)\bm{a}^{\land}\bm{a}^{\land}+\bm{I}+sin\theta\bm{a}^{\land}
=a∧a∧+I+sinθa∧−cosθa∧a∧=(1−cosθ)a∧a∧+I+sinθa∧
=
c
o
s
θ
I
+
(
1
−
c
o
s
θ
)
a
a
T
+
s
i
n
θ
a
∧
.
(3.6)
=cos\theta\bm{I}+(1-cos\theta)\bm{aa}^T+sin\theta\bm{a}^{\land}.\tag{3.6}
=cosθI+(1−cosθ)aaT+sinθa∧.(3.6) 这里
1
4
!
θ
4
(
a
∧
)
4
=
1
4
!
θ
4
a
∧
a
∧
a
∧
a
∧
=
(
a
a
T
−
I
)
a
∧
=
−
(
a
∧
)
2
\frac{1}{4!}\theta^4(\bm{a}^{\land})^4=\frac{1}{4!}\theta^4\bm{a}^{\land}\bm{a}^{\land}\bm{a}^{\land}\bm{a}^{\land}=(\bm{aa}^T-\bm{I})\bm{a}^{\land}=-(\bm{a}^{\land})^2
4!1θ4(a∧)4=4!1θ4a∧a∧a∧a∧=(aaT−I)a∧=−(a∧)2,其他高阶项同理进行降阶变形。
我们可以看到,式(3.6)我们很熟悉:
e
x
p
(
θ
a
∧
)
=
c
o
s
θ
I
+
(
1
−
c
o
s
θ
)
a
a
T
+
s
i
n
θ
a
∧
.
(3.7)
\rm{exp}(\theta\bm{a}^{\land})=cos\theta\bm{I}+(1-cos\theta)\bm{aa}^T+sin\theta\bm{a}^{\land}.\tag{3.7}
exp(θa∧)=cosθI+(1−cosθ)aaT+sinθa∧.(3.7)它和我们之前推导的罗德里格斯公式,如出一辙。这个结果表明,
s
o
(
3
)
\mathfrak{so}(3)
so(3)实际上就是由之前学习的旋转向量组成的空间,指数映射即罗德里格斯公式。通过它们,我们可以把
s
o
(
3
)
\mathfrak{so}(3)
so(3)中任意一个向量对应到一个位于
S
O
(
3
)
\rm{SO}(3)
SO(3)中的旋转向量。若定义的是对数映射,也可以把
S
O
(
3
)
\rm{SO}(3)
SO(3)中的元素对应到
s
o
(
3
)
\mathfrak{so}(3)
so(3)中:
ϕ
=
l
n
(
R
)
∨
=
{
∑
n
=
0
∞
(
−
1
)
n
n
+
1
(
R
−
I
)
n
+
1
}
∨
.
(3.8)
\bm{\phi}=\rm{ln}(\bm{R})^{\vee}=\Bigg\{\sum^{\infty}_{n=0}\frac{(-1)^n}{n+1}(\bm{R-I})^{n+1}\Bigg\}^{\vee}.\tag{3.8}
ϕ=ln(R)∨={n=0∑∞n+1(−1)n(R−I)n+1}∨.(3.8)类似的方法,我们可以利用之前学的第三讲的知识"旋转矩阵到旋转向量的转换",通过迹的性质求解转角和转轴,很容易求出来。
3.2 S E ( 3 ) \rm{SE}(3) SE(3)上的指数映射
同理,
s
e
(
3
)
\mathfrak{se}(3)
se(3)上的指数映射也可以由式(3.1)展开,下面我们进行结果的推导:
e
x
p
(
ξ
∧
)
=
∑
n
=
0
∞
1
n
!
(
ξ
∧
)
n
=
∑
n
=
0
∞
1
n
!
[
ϕ
∧
ρ
0
T
0
]
n
\rm{exp}(\bm{\xi}^{\land})=\sum^{\infty}_{n=0}\frac{1}{n!}(\bm{{\xi}^{\land}})^n=\sum^{\infty}_{n=0}\frac{1}{n!}\begin{bmatrix}\bm{\phi}^{\land}&\bm{\rho} \\ \bm{0}^T&0\end{bmatrix}^n
exp(ξ∧)=n=0∑∞n!1(ξ∧)n=n=0∑∞n!1[ϕ∧0Tρ0]n
=
I
+
∑
n
=
1
∞
1
n
!
[
ϕ
∧
ρ
0
T
0
]
n
.
(3.9)
=\bm{I}+\sum^{\infty}_{n=1}\frac{1}{n!}\begin{bmatrix}\bm{\phi}^{\land}&\bm{\rho} \\ \bm{0}^T&0\end{bmatrix}^n.\tag{3.9}
=I+n=1∑∞n!1[ϕ∧0Tρ0]n.(3.9)
[
ϕ
∧
ρ
0
T
0
]
2
=
[
ϕ
∧
ρ
0
T
0
]
[
ϕ
∧
ρ
0
T
0
]
=
[
(
ϕ
∧
)
2
ϕ
∧
ρ
0
T
0
]
.
(3.10)
\begin{bmatrix}\bm{\phi}^{\land}&\bm{\rho} \\ \bm{0}^T&0\end{bmatrix}^2=\begin{bmatrix}\bm{\phi}^{\land}&\bm{\rho} \\ \bm{0}^T&0\end{bmatrix}\begin{bmatrix}\bm{\phi}^{\land}&\bm{\rho} \\ \bm{0}^T&0\end{bmatrix}=\begin{bmatrix}(\bm{\phi}^{\land})^2&\bm{\phi}^{\land}\bm{\rho} \\ \bm{0}^T&0\end{bmatrix}.\tag{3.10}
[ϕ∧0Tρ0]2=[ϕ∧0Tρ0][ϕ∧0Tρ0]=[(ϕ∧)20Tϕ∧ρ0].(3.10)
[
ϕ
∧
ρ
0
T
0
]
3
=
[
(
ϕ
∧
)
2
ϕ
∧
ρ
0
T
0
]
[
ϕ
∧
ρ
0
T
0
]
=
[
(
ϕ
∧
)
3
(
ϕ
∧
)
2
ρ
0
T
0
]
(3.11)
\begin{bmatrix}\bm{\phi}^{\land}&\bm{\rho} \\ \bm{0}^T&0\end{bmatrix}^3=\begin{bmatrix}(\bm{\phi}^{\land})^2&\bm{\phi}^{\land}\bm{\rho} \\ \bm{0}^T&0\end{bmatrix}\begin{bmatrix}\bm{\phi}^{\land}&\bm{\rho} \\ \bm{0}^T&0\end{bmatrix}=\begin{bmatrix}(\bm{\phi}^{\land})^3&(\bm{\phi}^{\land})^2\bm{\rho} \\ \bm{0}^T&0\end{bmatrix}\tag{3.11}
[ϕ∧0Tρ0]3=[(ϕ∧)20Tϕ∧ρ0][ϕ∧0Tρ0]=[(ϕ∧)30T(ϕ∧)2ρ0](3.11)由(3.9-3.11)可知:
I
+
∑
n
=
1
∞
1
n
!
[
ϕ
∧
ρ
0
T
0
]
n
=
I
+
∑
n
=
1
∞
1
n
!
[
(
ϕ
∧
)
n
(
ϕ
∧
)
n
−
1
ρ
0
T
0
]
\bm{I}+\sum^{\infty}_{n=1}\frac{1}{n!}\begin{bmatrix}\bm{\phi}^{\land}&\bm{\rho} \\ \bm{0}^T&0\end{bmatrix}^n=\bm{I}+\sum^{\infty}_{n=1}\frac{1}{n!}\begin{bmatrix}(\bm{\phi}^{\land})^n&\bm{(\phi}^{\land})^{n-1}\bm{\rho} \\ \bm{0}^T&0\end{bmatrix}
I+n=1∑∞n!1[ϕ∧0Tρ0]n=I+n=1∑∞n!1[(ϕ∧)n0T(ϕ∧)n−1ρ0]
=
[
∑
n
=
0
∞
1
n
!
(
ϕ
∧
)
n
∑
n
=
1
∞
1
n
!
(
ϕ
∧
)
n
−
1
ρ
0
T
1
]
=
[
∑
n
=
0
∞
1
n
!
(
ϕ
∧
)
n
∑
n
=
0
∞
1
(
n
+
1
)
!
(
ϕ
∧
)
n
ρ
0
T
1
]
(3.12)
=\begin{bmatrix}\sum^{\infty}_{n=0}\frac{1}{n!}(\bm{\phi}^{\land})^n&\sum^{\infty}_{n=1}\frac{1}{n!}(\bm{\phi}^{\land})^{n-1}\bm{\rho} \\ \bm{0}^{\small{T}}&1\end{bmatrix}=\begin{bmatrix}\sum^{\infty}_{n=0}\frac{1}{n!}(\bm{\phi}^{\land})^n&\sum^{\infty}_{n=0}\frac{1}{(n+1)!}{(\bm{\phi}^{\land})^{n} \rho} \\ \bm{0}^{\small{T}}&1\end{bmatrix}\tag{3.12}
=[∑n=0∞n!1(ϕ∧)n0T∑n=1∞n!1(ϕ∧)n−1ρ1]=[∑n=0∞n!1(ϕ∧)n0T∑n=0∞(n+1)!1(ϕ∧)nρ1](3.12)这里,第二项中令
n
=
n
−
1
n=n-1
n=n−1,这时候求和的初值和分母都会改变,这样就和左上角的元素进行了求和类型的统一。
然后,我们最后进行整理:
[
∑
n
=
0
∞
1
n
!
(
ϕ
∧
)
n
∑
n
=
0
∞
1
(
n
+
1
)
!
(
ϕ
∧
)
n
ρ
0
T
1
]
≜
[
R
J
ρ
0
T
1
]
=
T
.
(3.13)
\begin{bmatrix}\sum^{\infty}_{n=0}\frac{1}{n!}(\bm{\phi}^{\land})^n&\sum^{\infty}_{n=0}\frac{1}{(n+1)!}{(\bm{\phi}^{\land})^{n} \rho} \\ \bm{0}^{\small{T}}&1\end{bmatrix}\triangleq\begin{bmatrix}\bm{R}&\bm{J\rho}\\ \bm{0}^T&1\end{bmatrix}=\bm{T}.\tag{3.13}
[∑n=0∞n!1(ϕ∧)n0T∑n=0∞(n+1)!1(ϕ∧)nρ1]≜[R0TJρ1]=T.(3.13)和推导
s
o
(
3
)
\mathfrak{so}(3)
so(3)类似,我们令
ϕ
=
θ
a
\bm{\phi}=\theta\bm{a}
ϕ=θa,
a
\bm{a}
a为单位向量,对指数矩阵进行泰勒展开得:
∑
n
=
0
∞
1
(
n
+
1
)
!
(
ϕ
∧
)
n
=
I
+
1
2
!
θ
a
∧
+
1
3
!
θ
2
(
a
∧
)
2
+
1
4
!
θ
3
(
a
∧
)
3
+
1
5
!
θ
4
(
a
∧
)
4
+
⋯
\sum^{\infty}_{n=0}\frac{1}{(n+1)!}({\bm{\phi}^{\land})^{n}}=\bm{I}+\frac{1}{2!}\theta\bm{a}^{\land}+\frac{1}{3!}\theta^2\bm({a}^{\land})^2+\frac{1}{4!}\theta^3\bm({a}^{\land})^3+\frac{1}{5!}\theta^4\bm({a}^{\land})^4+\cdots
n=0∑∞(n+1)!1(ϕ∧)n=I+2!1θa∧+3!1θ2(a∧)2+4!1θ3(a∧)3+5!1θ4(a∧)4+⋯
=
1
θ
(
1
2
!
θ
2
+
1
4
!
θ
4
+
⋯
)
⏟
(
1
−
c
o
s
θ
)
的泰勒展开
)
(
a
∧
)
+
1
θ
(
1
3
!
θ
3
−
1
5
!
θ
5
+
⋯
)
⏟
(
θ
−
s
i
n
θ
)
的泰勒展开
(
a
∧
)
2
+
I
=\frac{1}{\theta}\underbrace{(\frac{1}{2!}\theta^2+\frac{1}{4!}\theta^4+ \cdots)}_{(1-cos\theta)的泰勒展开})(\bm{a}^{\land})+\frac{1}{\theta}\underbrace{(\frac{1}{3!}\theta^3-\frac{1}{5!}\theta^5+ \cdots)}_{(\theta-sin\theta)的泰勒展开}(\bm{a}^{\land})^2+\bm{I}
=θ1(1−cosθ)的泰勒展开
(2!1θ2+4!1θ4+⋯))(a∧)+θ1(θ−sinθ)的泰勒展开
(3!1θ3−5!1θ5+⋯)(a∧)2+I
=
1
θ
(
1
−
c
o
s
θ
)
(
a
∧
)
+
θ
−
s
i
n
θ
θ
(
a
a
T
−
I
)
+
I
=\frac{1}{\theta}(1-cos\theta)(\bm{a}^{\land})+\frac{\theta-sin\theta}{\theta}(\bm{aa}^T-\bm{I})+\bm{I}
=θ1(1−cosθ)(a∧)+θθ−sinθ(aaT−I)+I
=
s
i
n
θ
θ
I
+
(
1
−
s
i
n
θ
θ
)
a
a
T
+
1
−
c
o
s
θ
θ
a
∧
=
d
e
f
J
(3.14)
=\frac{sin\theta}{\theta}\bm{I}+(1-\frac{sin\theta}{\theta})\bm{aa}^T+\frac{1-cos\theta}{\theta}\bm{a}^{\land}\xlongequal{\rm{def}}\bm{J}\tag{3.14}
=θsinθI+(1−θsinθ)aaT+θ1−cosθa∧defJ(3.14)从式(3.13)可以看出,
ξ
\bm{\xi}
ξ的指数映射左上角的
R
\bm{R}
R式我们熟知的
S
O
(
3
)
\rm{SO}(3)
SO(3),与
s
e
(
3
)
\mathfrak{se}(3)
se(3)中的旋转部分
ϕ
\bm{\phi}
ϕ对应。右下角的
J
\bm{J}
J由式(3.14)给出:
J
=
s
i
n
θ
θ
I
+
(
1
−
s
i
n
θ
θ
)
a
a
T
+
1
−
c
o
s
θ
θ
a
∧
(3.15)
\bm{J=}\frac{sin\theta}{\theta}\bm{I}+(1-\frac{sin\theta}{\theta})\bm{aa}^T+\frac{1-cos\theta}{\theta}\bm{a}^{\land}\tag{3.15}
J=θsinθI+(1−θsinθ)aaT+θ1−cosθa∧(3.15)这个结果和罗德里格斯公式相似,但不完全一样,平移部分经过指数映射之后,发生了一次以
J
\bm{J}
J为系数矩阵的线性变换。后面我们将经常用到这个
J
\bm{J}
J。
同理,我们也可以推导出对数映射,同时根据变换矩阵
T
\bm{T}
T求
s
e
(
3
)
\mathfrak{se}(3)
se(3)上对应的向量有更容易的办法:根据左上角的
R
\bm{R}
R计算旋转向量,右上角的
t
\bm{t}
t满足:
t
=
J
ρ
(3.16)
\bm{t}=\bm{J\rho}\tag{3.16}
t=Jρ(3.16)这里
J
J
J可以由
ϕ
\bm{\phi}
ϕ得到,因此
ρ
\bm{\rho}
ρ也可以由式(3.16)这个线性方程组求解。
下面我们对李群李代数之间的转换关系进行总结,关系如图。
参考资料:
视觉SLAM十四讲:从理论到实践(第2版)(ISBN:9787121369421)
旋量代数与李群、李代数 戴建生(ISBN:9787040318456)