SLAM十四讲 - - 第四讲之李群与李代数1
《SLAM十四讲》系列文章链接
SLAM十四讲 - - 第三讲之可视化演示程序逐行讲解
纯纯新手一个，记录自己学习后的理解，欢迎大家交流讨论。
目录
一、为什么要学李群与李代数？
??在SLAM中相机的位姿是未知的，当我们采集到了观测数据后，我们要解决“什么样的相机位姿最符合当前的观测数据”这样一个问题。说人话就是：有了一堆相机观测的数据，如何准确地（最小化误差）计算出相机的位姿。一种典型的方法是：把这个问题转换成优化问题，求解最优的旋转矩阵
R
R
R和平移矩阵
t
t
t，使得观测误差最小化。
??求解旋转矩阵
R
R
R和平移矩阵
t
t
t和我们李群与李代数有什么关系呢？是这样的，在处理优化问题时，我们尝尝会构建一个损失函数（也就是一个误差函数），在最小化这个损失函数的时候，由于旋转矩阵自身是带约束的（正交矩阵：矩阵转置=矩阵的逆），如果直接使用旋转矩阵
R
R
R作为优化变量的话，会引入额外的约束，使优化变的困难。我们可以通过李群与李代数之间的转换关系，把位姿估计问题转化为无约束的优化问题，简化求解的方式。
二、李群与李代数
1.李群
??群（group）是一种集合和一种运算。我们把集合记作A，运算记作·，那么群可以记作G=(A,·)。群需要满足以下条件：
??
1.
封
闭
性
:
?
a
1
,
a
2
∈
A
,
a
1
?
a
2
∈
A
.
1. 封闭性: \quad \forall a_{1}, a_{2} \in A, \quad a_{1} \cdot a_{2} \in A .
1.封闭性:?a1?,a2?∈A,a1??a2?∈A.
??
2.
结
合
律
:
?
a
1
,
a
2
,
a
3
∈
A
,
(
a
1
?
a
2
)
?
a
3
=
a
1
?
(
a
2
?
a
3
)
.
2. 结合律: \quad \forall a_{1}, a_{2}, a_{3} \in A, \quad\left(a_{1} \cdot a_{2}\right) \cdot a_{3}=a_{1} \cdot\left(a_{2} \cdot a_{3}\right) .
2.结合律:?a1?,a2?,a3?∈A,(a1??a2?)?a3?=a1??(a2??a3?).
??
3.
么
元
:
?
a
0
∈
A
,
s
.
t
.
?
a
∈
A
,
a
0
?
a
=
a
?
a
0
=
a
.
3. 么元: \quad \quad \exists a_{0} \in A, \quad s.t. \quad \forall a \in A, \quad a_{0} \cdot a=a \cdot a_{0}=a .
3.么元:?a0?∈A,s.t.?a∈A,a0??a=a?a0?=a.
??
4.
逆
:
?
a
∈
A
,
?
a
?
1
∈
A
,
s
.
t
.
a
?
a
?
1
=
a
0
.
4. 逆: \quad \quad\quad \forall a \in A, \quad \exists a^{-1} \in A, \quad s.t. \quad a \cdot a^{-1}=a_{0} .
4.逆:?a∈A,?a?1∈A,s.t.a?a?1=a0?.
??在第三讲中我们学习到三维旋转矩阵构成了特殊正交群SO(3)，变换矩阵构成了特殊欧式群SE(3)，他们写起来就像这样：
S
O
(
3
)
=
{
R
∈
R
3
×
3
∣
R
R
T
=
I
,
det
?
(
R
)
=
1
}
,
?
S
E
(
3
)
=
{
T
=
[
R
t
0
T
1
]
∈
R
4
×
4
∣
R
∈
S
O
(
3
)
,
t
∈
R
3
}
.
\begin{array}{l} \mathrm{SO}(3)=\left\{\boldsymbol{R} \in \mathbb{R}^{3 \times 3} \mid \boldsymbol{R} \boldsymbol{R}^{\mathrm{T}}=\boldsymbol{I}, \operatorname{det}(\boldsymbol{R})=1\right\}, \\~\\ \mathrm{SE}(3)=\left\{\boldsymbol{T}=\left[\begin{array}{cc} \boldsymbol{R} & \boldsymbol{t} \\ \mathbf{0}^{\mathrm{T}} & 1 \end{array}\right] \in \mathbb{R}^{4 \times 4} \mid \boldsymbol{R} \in \mathrm{SO}(3), \boldsymbol{t} \in \mathbb{R}^{3}\right\} . \end{array}
SO(3)={R∈R3×3∣RRT=I,det(R)=1},?SE(3)={T=[R0T?t1?]∈R4×4∣R∈SO(3),t∈R3}.???不难观察到SO(3)和SE(3)对矩阵加法是不封闭的，而对矩阵乘法是封闭的，也即：
R
1
+
R
2
?
S
O
(
3
)
,
T
1
+
T
2
?
S
E
(
3
)
?
R
1
R
2
∈
S
O
(
3
)
,
T
1
T
2
∈
S
E
(
3
)
\boldsymbol{R}_{1}+\boldsymbol{R}_{2} \notin \mathrm{SO}(3), \quad \boldsymbol{T}_{1}+\boldsymbol{T}_{2} \notin \mathrm{SE}(3)\\ ~\\ \boldsymbol{R}_{1} \boldsymbol{R}_{2} \in \mathrm{SO}(3), \quad \boldsymbol{T}_{1} \boldsymbol{T}_{2} \in \mathrm{SE}(3)
R1?+R2?∈/?SO(3),T1?+T2?∈/?SE(3)?R1?R2?∈SO(3),T1?T2?∈SE(3)??所以严格的说，旋转矩阵和矩阵乘法构成特殊正交群SO(n)，变换矩阵和矩阵乘法构成特殊欧式群SE(3)。
??群结构保证了在群上的运算具有良好的运算。其中，李群指的是具有连续(光滑)性质的群。直观的来说，刚体能够在空间上连续的运动，所以我们说SO(n)和SE(3)是李群。
2.李代数
2.1引出李代数
??这部分公式敲起来麻烦，这里就直接贴上手写的推导。
??简单的来说，就是经过简单推导可以发现旋转矩阵
R
R
R有一个性质：
R
˙
(
t
)
R
(
t
)
T
\dot{\boldsymbol{R}}(t) \boldsymbol{R}(t)^{\mathrm{T}}
R˙(t)R(t)T是一个反对称矩阵，记作：
R
˙
(
t
)
R
(
t
)
T
=
?
(
t
)
∧
\dot{\boldsymbol{R}}(t) \boldsymbol{R}(t)^{\mathrm{T}}=\boldsymbol{\phi}(t)^{\wedge}
R˙(t)R(t)T=?(t)∧??其中，
?
(
t
)
\phi(t)
?(t)是一个向量，符号^是第三讲中学习的将向量转换为反对称矩阵的符号。
??再经过一阶泰勒展开和简单的化简我们得到：
R
˙
(
t
)
=
?
0
∧
R
(
t
)
\dot{\boldsymbol{R}}(t)=\phi_{0}^{\wedge} \boldsymbol{R}(t)
R˙(t)=?0∧?R(t)??可以发现，在
t
t
t时刻附近，每次对矩阵进行求导只需要左乘一个
?
\phi
?^。这说明向量
?
\phi
?反映了
R
R
R的局部导数性质，所以也称它在SO(3)的正切空间上。
??根据初始值和微分方程求解可以得到：
R
(
t
)
=
exp
?
(
?
0
∧
t
)
\boldsymbol{R}(t)=\exp \left(\phi_{0}^{\wedge} t\right)
R(t)=exp(?0∧?t)??到这里，我们不得不思考两个问题：
?? 1. 给定某时刻的矩阵
R
R
R我们就能计算对应的向量
?
\phi
?，那么
?
\phi
?向量有什么含义呢？
?
\phi
?正是对应对应到SO(3)上的李代数
s
o
(
3
)
\mathfrak{s o}(3)
so(3)。
?? 2. 给定某时刻的向量
?
\phi
?，矩阵指数
exp
?
(
?
∧
)
\exp \left(\phi^{\wedge}\right)
exp(?∧)如何计算呢(也就是计算矩阵
R
R
R)？反过来，给定
R
R
R时是否有相反的运算来计算向量
?
\phi
?？这正是李群与李代数之间的指数和对数映射。
2.2李代数的定义
??每个李群都有与之对应的李代数，李代数描述了李群的局部性质，准确的说，是单位元附近的正切空间。李代数的数学定义如下：
??李代数由一个集合
V
\mathbb{V}
V、一个数域
F
\mathbb{F}
F和一个二元运算
[
,
]
组
成
。
如
果
它
们
满
足
以
下
几
条
性
质
，
则
称
[,]组成。如果它们满足以下几条性质，则 称
[,]组成。如果它们满足以下几条性质，则称
(
V
,
F
,
[
,
]
)
(\mathbb{V},\mathbb{F},[,])
(V,F,[,])为一个李代数， 记作
g
\mathfrak{g}
g。
??
1.
封
闭
性
?
X
,
Y
∈
V
,
[
X
,
Y
]
∈
V
.
1. 封闭性 \quad \forall \boldsymbol{X}, \boldsymbol{Y} \in \mathbb{V},[\boldsymbol{X}, \boldsymbol{Y}] \in \mathbb{V} .
1.封闭性?X,Y∈V,[X,Y]∈V.
??
2.
双
线
性
?
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
]
.
2. 双线性 \quad \forall \boldsymbol{X}, \boldsymbol{Y}, \boldsymbol{Z} \in \mathbb{V}, a, b \in \mathbb{F} , 有 [a \boldsymbol{X}+b \boldsymbol{Y}, \boldsymbol{Z}]=a[\boldsymbol{X}, \boldsymbol{Z}]+b[\boldsymbol{Y}, \boldsymbol{Z}], \quad[\boldsymbol{Z}, a \boldsymbol{X}+b \boldsymbol{Y}]=a[\boldsymbol{Z}, \boldsymbol{X}]+b[\boldsymbol{Z}, \boldsymbol{Y}] .
2.双线性?X,Y,Z∈V,a,b∈F,有[aX+bY,Z]=a[X,Z]+b[Y,Z],[Z,aX+bY]=a[Z,X]+b[Z,Y].
??
3.
自
反
性
?
X
∈
V
,
[
X
,
X
]
=
0.
3. 自反性 \quad \forall \boldsymbol{X} \in \mathbb{V},[\boldsymbol{X}, \boldsymbol{X}]=\mathbf{0} .
3.自反性?X∈V,[X,X]=0.
??
4.
雅
可
比
等
价
?
X
,
Y
,
Z
∈
V
,
[
X
,
[
Y
,
Z
]
]
+
[
Z
,
[
X
,
Y
]
]
+
[
Y
,
[
Z
,
X
]
]
=
0.
4. 雅可比等价 \quad \forall \boldsymbol{X}, \boldsymbol{Y}, \boldsymbol{Z} \in \mathbb{V},[\boldsymbol{X},[\boldsymbol{Y}, \boldsymbol{Z}]]+[\boldsymbol{Z},[\boldsymbol{X}, \boldsymbol{Y}]]+[\boldsymbol{Y},[\boldsymbol{Z}, \boldsymbol{X}]]=\mathbf{0} .
4.雅可比等价?X,Y,Z∈V,[X,[Y,Z]]+[Z,[X,Y]]+[Y,[Z,X]]=0.
??其中二元运算符称为李括号。举个例子，三维向量 R 3 \mathbb{R}^{3} R3上定义的叉乘 × \times ×是一种李括号(证明过程)，因此 g = ( R 3 × 3 , R , × ) \mathfrak{g}=(\mathbb{R}^{3\times3},\mathbb{R},\times) g=(R3×3,R,×)构成了一个李代数。
2.2.1 李代数 s o ( 3 ) \mathfrak{so}(3) so(3)
??之前提到的
?
\phi
?，事实上是一种李代数。SO(3)对应的李代数是定义在
R
3
\mathbb{R}^{3}
R3上的向量，记作
?
\phi
?。 根据前面的推导， 每个
?
\phi
?都可以生成一个反对称矩阵：
Φ
=
?
∧
=
[
0
?
?
3
?
2
?
3
0
?
?
1
?
?
2
?
1
0
]
∈
R
3
×
3
\boldsymbol{\Phi}=\phi^{\wedge}=\left[\begin{array}{ccc} 0 & -\phi_{3} & \phi_{2} \\ \phi_{3} & 0 & -\phi_{1} \\ -\phi_{2} & \phi_{1} & 0 \end{array}\right] \in \mathbb{R}^{3 \times 3}
Φ=?∧=???0?3???2????3?0?1???2???1?0????∈R3×3??在此定义下，两个向量
?
1
\phi_{1}
?1?和
?
2
\phi_{2}
?2?的李括号为：(证明过程)
[
?
1
,
?
2
]
=
(
Φ
1
Φ
2
?
Φ
2
Φ
1
)
∨
\left[\phi_{1}, \phi_{2}\right]=\left(\boldsymbol{\Phi}_{1} \boldsymbol{\Phi}_{2}-\boldsymbol{\Phi}_{2} \boldsymbol{\Phi}_{1}\right)^{\vee}
[?1?,?2?]=(Φ1?Φ2??Φ2?Φ1?)∨??由于向量
?
\phi
?与反对称矩阵是一一对应的 ，在不引起歧义的清况下，就说
s
o
(
3
)
\mathfrak{so}(3)
so(3)的元素是三维向量或者三维反对称矩阵，不加区别：
s
o
=
{
?
∈
R
3
,
Φ
=
?
∧
∈
R
3
×
3
}
\mathfrak{so}=\left\{\phi \in \mathbb{R}^{3}, \Phi=\phi^{\wedge} \in \mathbb{R}^{3 \times 3}\right\}
so={?∈R3,Φ=?∧∈R3×3}
??总结来说，
s
o
(
3
)
\mathfrak{so}(3)
so(3)的内容是一个由三维向量组成的集合，每个向量对应一个反对称矩阵， 可以用于表达旋转矩阵的导数。它与SO(3)的关系由指数映射给定(具体计算第三节中说)：
R
=
exp
?
(
?
∧
)
\boldsymbol{R}=\exp \left(\phi_{}^{\wedge}\right)
R=exp(?∧?)
2.2.2 李代数 s e ( 3 ) \mathfrak{se}(3) se(3)
??与之前类似，SE(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
}
\mathfrak{s e}(3)=\left\{\xi=\left[\begin{array}{l} \rho \\ \phi \end{array}\right] \in \mathbb{R}^{6}, \rho \in \mathbb{R}^{3}, \phi \in \mathfrak{s o}(3), \xi^{\wedge}=\left[\begin{array}{ll} \phi^{\wedge} & \rho \\ 0^{\mathrm{T}} & 0 \end{array}\right] \in \mathbb{R}^{4 \times 4}\right\}
se(3)={ξ=[ρ??]∈R6,ρ∈R3,?∈so(3),ξ∧=[?∧0T?ρ0?]∈R4×4}??
s
e
(
3
)
\mathfrak{se}(3)
se(3)中元素记作
ξ
\xi
ξ，是一个六维向量。前三维为平移(但含义与变换矩阵中的平移不同，分析见后)，记作
ρ
\rho
ρ；后三维为旋转，记作
?
\phi
?，实质上是
s
o
(
3
)
\mathfrak{so}(3)
so(3)元素。＾符号进行简单的拓展，表示将一个六维向最转换成四维矩阵：
ξ
∧
=
[
?
∧
ρ
0
T
0
]
∈
R
4
×
4
\xi^{\wedge}=\left[\begin{array}{ll} \phi^{\wedge} & \rho \\ 0^{\mathrm{T}} & 0 \end{array}\right] \in \mathbb{R}^{4 \times 4}
ξ∧=[?∧0T?ρ0?]∈R4×4??类似的，两个向量
ξ
1
\xi_{1}
ξ1?和
ξ
2
\xi_{2}
ξ2?的李括号为：
[
ξ
1
,
ξ
2
]
=
(
ξ
1
∧
ξ
2
∧
?
ξ
2
∧
ξ
1
∧
)
∨
\left[\boldsymbol{\xi}_{1}, \boldsymbol{\xi}_{2}\right]=\left(\boldsymbol{\xi}_{1}^{\wedge} \boldsymbol{\xi}_{2}^{\wedge}-\boldsymbol{\xi}_{2}^{\wedge} \boldsymbol{\xi}_{1}^{\wedge}\right)^{\vee}
[ξ1?,ξ2?]=(ξ1∧?ξ2∧??ξ2∧?ξ1∧?)∨
3.指数与对数映射
??矩阵指数问题，在李群和李代数中称为指数映射。同样的，这里面的公式比较琐碎，这里只放简单的框架性推导，其中涉及的结论性公式的证明过程见此链接。
3.1 SO(3)与 s o ( 3 ) \mathfrak{so}(3) so(3)
3.2 SE(3)与 s e ( 3 ) \mathfrak{se}(3) se(3)
??如果不想看推导过程，只要能够看懂带图就行。这个图总结的非常全面！高博真是太厉害了！
4.李代数求导与扰动模型
4.1 基础
??前面我们说：我们可以通过李群与李代数之间的转换关系，把位姿估计问题转化为无约束的优化问题，简化求解的方式。而在优化过程中，对损失函数进行求导是非常必要的(第6讲会介绍)。求导过程中的复合求导不可避免的要处理矩阵乘法，那么当在 SO(3) 中完成两个矩阵乘法时，李代数
s
o
(
3
)
\mathfrak{so}(3)
so(3)发生了什么改变呢？
??这里直接给出结论，两个李代数指数映射的由Baker-Campbell-Hausdorff公式(BCH公式）给出。老规矩，直接上手写的：
??推导中，左乘BCH近似雅可比
J
l
\boldsymbol{J}_{l}
Jl?形式：
J
l
=
J
=
sin
?
θ
θ
I
+
(
1
?
sin
?
θ
θ
)
a
a
T
+
1
?
cos
?
θ
θ
a
∧
\boldsymbol{J}_{l}=\boldsymbol{J}=\frac{\sin \theta}{\theta} \boldsymbol{I}+\left(1-\frac{\sin \theta}{\theta}\right) \boldsymbol{a} \boldsymbol{a}^{\mathrm{T}}+\frac{1-\cos \theta}{\theta} \boldsymbol{a}^{\wedge}
Jl?=J=θsinθ?I+(1?θsinθ?)aaT+θ1?cosθ?a∧??它的逆为：
J
l
?
1
=
θ
2
cot
?
θ
2
I
+
(
1
?
θ
2
cot
?
θ
2
)
a
a
T
?
θ
2
a
∧
\boldsymbol{J}_{l}^{-1}=\frac{\theta}{2} \cot \frac{\theta}{2} \boldsymbol{I}+\left(1-\frac{\theta}{2} \cot \frac{\theta}{2}\right) \boldsymbol{a} \boldsymbol{a}^{\mathrm{T}}-\frac{\boldsymbol{\theta}}{2} \boldsymbol{a}^{\wedge}
Jl?1?=2θ?cot2θ?I+(1?2θ?cot2θ?)aaT?2θ?a∧??右乘雅可比仅需要对自变量取负号即可：
J
r
(
?
)
=
J
l
(
?
?
)
\boldsymbol{J}_{r}(\boldsymbol{\phi})=\boldsymbol{J}_{l}(-\boldsymbol{\phi})
Jr?(?)=Jl?(??)。
??对于李代数
s
e
(
3
)
\mathfrak{se}(3)
se(3)，也有近似的BCH公式，由于其雅克比矩阵形式复杂，实际中未使用，所以跳过。
4.2 李代数导数与扰动模型
??下面来讨论一个带有李代数的函数，以及关于该李代数求导的问题。直接上手写笔记。
??按照思路一：李代数求导，推导过程见此链接。
??按照思路二：扰动模型(左乘)，推导过程见此链接。
??SE(3)上的李代数求导直接给结论，推导过程见此链接。
