李群李代数
1.群
1.常见群
- 三维旋转矩阵构成了特殊正交群(Special Orthogonal Group)
SO(3) = \left \{
\mathbf{R \in \mathbb{R}^{3 \times 3} |
RR^{T}=I}, det(\mathbf{R}) = 1
\right \}
- 三维变换矩阵构成了特殊欧氏群(Special Euclidean Group)
SE(3)=\left \{
\mathbf{T} = \begin{bmatrix}
\mathbf{R} && \mathbf{t} \\
\mathbf{0} && 1
\end{bmatrix} \in \mathbb{R}^{4\times 4} |
\mathbf{R} \in \mathbf{SO(3)},
\mathbf{t} \in \mathbb{R}^{3}
\right \}
- 一般线性群GL(n)：指n\times n的可逆矩阵，它们对矩阵乘法成群。
2.群定义
群(group)是一种集合加上一种运算的代数结构。记集合为\mathbf{A}，运算为\cdot，那么当运算满足以下性质时，称\mathbf(A,\cdot)成群：
- (1) 封闭性：\forall a_{1},a_{2} \in A,\ a_{1}a_{2}\in A
- (2) 结合律：\forall a_{1}, a_{2}, a_{3} \in A, \ (a_{1}\cdot a_{2})\cdot a_{3}=a_{1}\cdot (a_{2}\cdot a_{3})
- (3) 幺元：\exists a_{0} \in A,\ s.t.\ a_{0}\cdot a = a \cdot a_{0} = a
- (4) 逆：\forall a \in A, \ \exists a^{-1}\in A,\ s.t.\ a\cdot a^{-1}=a_{0}
2.李群与李代数
2.1 李群
- 具有连续(光滑)性质的群；
- 既是群也是流行；
- 直观上看，一个刚体能够连续地在空间中运动，故SO(3)和SE(3)都是李群；
- 但是SO(3)和SE(3)只是定义良好地乘法，没有加法，所以难以进行取极限，求导等操作。
2.2 李代数
- 与李群对应的一种结构，位于向量空间；
- 通常记作小写so(3)和se(3)；
- 事实上是李群单位元处的正切空间；
2.3 旋转矩阵到李代数推导
考虑任意旋转矩阵\mathbf{R}，满足\mathbf{RR^{T}=I}，令\mathbf{R}随时间变化(连续运动)，有：\mathbf{R}(t)\mathbf{R}(t)^{T}=\mathbf{I}，两边对时间求导：
\dot{\mathbf{R}}(t)\mathbf{R}(t)^{T}+\mathbf{R}(t)\dot{\mathbf{R}}(t)^{T} = 0 \\
\rightarrow \dot{\mathbf{R}}(t)\mathbf{R}(t)^{T} = -(\dot{\mathbf{R}}(t)\mathbf{R}(t)^{T})^{T}
可以看出这是一个反对称矩阵，记：
\dot{\mathbf{R}}(t)\mathbf{R}(t)^{T}=\phi(t)^{\wedge} \\
\rightarrow \dot{\mathbf{R}}(t) = \phi(t)^{\wedge}\mathbf{R}(t)
单位元附近：
t_{0} = 0,\mathbf{R}(0)=\mathbf{I} \\
\mathbf{R(t)} \approx \mathbf{R}(t_{0}) + \dot{\mathbf{R}}(t_{0})(t-t_{0}) \\
=\mathbf{R(t_{0})} + \phi(t_{0})^{\wedge}\mathbf{R}(t_{0})(t-t_{0}) \\
=\mathbf{R(0)} + \phi_{0}^{\wedge}\mathbf{R(0)}t
= \mathbf{I} + \phi(t_{0})^{\wedge}t \\
= \mathbf{I + \phi_{0}^{\wedge}}t
在t_{0}附近，假设\phi不变，有微分方程：
\dot{\mathbf{R}}(t) = \phi(t_{0})^{\wedge}\mathbf{R}(t) = \phi_{0}^{\wedge}\mathbf{R}(t)
已知\mathbf{R}(0)=\mathbf{I}，解之得：
\mathbf{R}(t)=exp(\phi_{0}^{\wedge}t)
该式说明，对任意t，都可以找到一个\mathbf{R}和一个\phi 的对应关系，该关系称为指数映射(Exponential Map)，这里\phi称为\mathbf{SO(3)}对应的李代数so(3)。
2.4 李代数性质
每个李群都有与之对应的李代数，李代数描述了李群单位元附近的正切空间性质。李代数由一个集合\mathbb{V}，一个数域\mathbb{F}和一个二元运算[,]组成，若它们满足以下几条性质，称(\mathbb{V}, \mathbb{F}, [,])为一个李代数，记作\mathbf{g}：
- (1) 封闭性： \forall \mathbf{X,Y \in\mathbb{V}, [X,Y]\in \mathbb{V}}；
- (2) 双线性：\forall \mathbf{X,Y,Z} \in \mathbb{V}, a,b\in\mathbb{F}，有：
[a\mathbf{X}+b\mathbf{Y},\mathbf{Z}]=a\mathbf{[X,Z]} + b\mathbf{[Y,Z]} \\
[\mathbf{Z}, a\mathbf{X}+b\mathbf{Y}]=a\mathbf{[Z,X]} + b\mathbf{[Z,Y]}
- (3) 自反性：\forall \mathbf{X \in \mathbb{V}, [X,X]}=0
- (4) 雅可比等价：\forall \mathbf{X,Y,Z \in \mathbb{V}},\mathbf{[X,[Y,Z]] + [Y, [Z,X] + [Z, [X,Y]]]}= 0
二元运算[,]被称为李括号(Lie Bracket)，直观来说，李括号表达了两个元素的差异。李代数so(3):
so(3) = \left \{
\phi \in \mathbb{R}^{3},\Phi=\phi^{\wedge}\in \mathbb{R}^{3\times 3}
\right \} \\
\Phi=\phi^{\wedge}=\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}
李括号定义如下，就满足上面性质：
[\phi_{1}, \phi_{2}]=(\Phi_{1}\Phi_{2} - \Phi_{2}\Phi_{1})^{\vee}
同理，\mathbf{SE(3)}也有李代数se(3)：
se(3)=\left \{
\xi=\begin{bmatrix}
\rho \\
\phi
\end{bmatrix} \in \mathbb{R}^{6}, \phi \in so(3),
\xi ^{\wedge} = \begin{bmatrix}
\phi^{\wedge} && \rho \\
\mathbf{0}^{T} && 0
\end{bmatrix} \in \mathbb{R}^{4 \times 4}
\right \}
上尖尖不再是反对称矩阵，但仍然保留记法：
\xi ^{\wedge} = \begin{bmatrix}
\phi^{\wedge} && \rho \\
\mathbf{0}^{T} && 0
\end{bmatrix} \in \mathbb{R}^{4 \times 4}
李括号：
[\xi_{1}, \xi_{2}]=(\xi_{1}^{\wedge}\xi_{2}^{\wedge} - \xi_{2}^{\wedge}\xi_{1}^{\wedge})^{\vee}
3.指数映射和对数映射
\mathbf{R=exp(\phi^{\wedge})}
\phi = \theta a
a^{\wedge}a^{\wedge} = \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_{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}
\end{bmatrix} \\
aa^{T} = \begin{bmatrix}
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}
\end{bmatrix} \\
a_{1}^{2} + a_{2}^{2} + a_{3}^{2} = 1 \\
\rightarrow a^{\wedge}a^{\wedge} = aa^{T} - \mathbf{I} \\
a^{\wedge}a^{\wedge}a^{\wedge}=a^{\wedge}(aa^{T} - \mathbf{I}) \\
= a^{\wedge}aa^{T} - a^{\wedge}\mathbf{I} = - a^{\wedge}
exp(\phi^{\wedge})=exp(\theta a^{\wedge}) = \sum_{n=0}^{\infty}\frac{1}{n!}(\theta a^{\wedge})^{n} \\
= I + \theta a^{\wedge} + \frac{1}{2!}\theta^{2}(a^{\wedge})^{2} + \frac{1}{3!}\theta^{3}(a^{\wedge})^{3} + \frac{1}{4!}\theta^{4}(a^{\wedge})^{4} + \dots \\
= aa^{T} - a^{\wedge}a^{\wedge} + \theta a^{\wedge} + \frac{1}{2!}\theta^{2}(a^{\wedge})^{2} - \frac{1}{3!}\theta^{3}a^{\wedge}-\frac{1}{4!}\theta^{4}(a^{\wedge})^{2} + \dots \\
= aa^{T} + (\theta - \frac{1}{3!}\theta^{3} + \frac{1}{5!}\theta^{5}-\dots)a^{\wedge} - (1-\frac{1}{2!}\theta^{2} + \frac{1}{4!}\theta^{4}-\dots)a^{\wedge}a^{\wedge} \\
= a^{\wedge}a^{\wedge} + \mathbf{I} + sin\theta a^{\wedge} - cos\theta a^{\wedge}a^{\wedge} \\
= (1-cos\theta)a^{\wedge}a^{\wedge} + \mathbf{I} + sin\theta a^{\wedge} \\
= cos\theta \mathbf{I} + (1-cos\theta)aa^{T} + sin\theta a^{\wedge}
指数映射就是罗德里格斯公式：so(3)的物理意义就是旋转向量，反之，给定旋转矩阵时，也能求李代数：
\phi = ln(\mathbf{R})^{\vee} = (\sum_{n=0}{\infty}\frac{n+1}{(-1)^{n}}(\mathbf{R-I}^{n+1}))^{\vee}
但实际上没必要这么求，在讲解旋转向量时，介绍了矩阵到向量的转换关系：
\theta = \arccos(\frac{tr(\mathbf{R} - 1)}{2}),\mathbf{R}n=n \\
Rn=n \rightarrow Rn=\lambda n, \lambda = 1
至此，说明了SO(3)和so(3)之间的对应关系。
se(3)到SE(3)的指数映射：
exp(\xi^{\wedge})=\begin{bmatrix}
\sum_{n=0}^{\infty}\frac{1}{n!}(\phi^{\wedge})^{n} && \sum_{n=0}^{\infty}\frac{1}{(n+1)!}(\phi^{\wedge})^{n}\rho \\
\mathbf{0}^{T} && 1
\end{bmatrix} \\
= \begin{bmatrix}
\mathbf{R} && \mathbf{J}\rho \\
\mathbf{0}^{T} && 1
\end{bmatrix} = \mathbf{T} \\
\mathbf{J} = \frac{\sin\theta}{\theta}\mathbf{I} + (1-\frac{\sin\theta}{\theta})aa^{T} + \frac{1-\cos\theta}{\theta}a^{\wedge}
它们之间的转换关系可以由如下图表示：
4.李代数求导与扰动模型
4.李代数求导与扰动模型
4.1 BCH公式与近似形式
使用李代数一大动机就是为了进行优化，而优化过程中导数是非常必要的信息。现在考虑一下，当我们在SO(3)上完成两个矩阵乘法时，李代数中\mathbf{so(3)}上发生了什么变化？反过来说，当so(3)上做两个李代数加法时，SO(3)上是否对应着两个矩阵成绩？若成立，相当于
exp(\phi_{1}^{\wedge})exp(\phi_{2}^{\wedge})=exp((\phi_{1}+\phi_{2})^{\wedge})
若\phi_{1},\phi_{2}为标量，那么显然成立，但此处计算是矩阵的指数函数，而非标量的指数，换言之，我们在研究下式是否成立：
ln(exp(\mathbf{A})exp(\mathbf{B}))=\mathbf{A+B}?
很遗憾，该式在矩阵时并不成立。两个李代数指数映射乘积的完整形式，由Baker-Campbell-Hauselorff公式(BCH公式)给出。由于完整形式较复杂，我们给出它展开式的前几项：
ln(exp(\mathbf{A})exp(\mathbf{B}))=\mathbf{A + B} + \frac{1}{2}\begin{bmatrix}
\mathbf{A,B}
\end{bmatrix} +
\frac{1}{12}\begin{bmatrix}
\mathbf{A},\begin{bmatrix}
\mathbf{A,B}
\end{bmatrix}
\end{bmatrix} -
\frac{1}{12}\begin{bmatrix}
\mathbf{B},\begin{bmatrix}
\mathbf{A,B}
\end{bmatrix}
\end{bmatrix} + \dots
其中\begin{bmatrix} \end{bmatrix}为李括号，BCH公式告诉我们，当处理两个矩阵指数之积时，它们会产生一个由李括号组成的余项。特别地，考虑SO(3)上李代数ln(exp(\phi_{1}^{\wedge})exp(\phi_{2}^{\wedge}))^{\vee}，当\phi_{1}或\phi_{2}为小量时，小量二次以上的项都可以被忽略掉，此时BCH拥有线性近似表达：
ln(exp(\phi_{1}^{\wedge})exp(\phi_{2}^{\wedge}))^{\vee}=
\left\{\begin{matrix}
\mathbf{J}(\phi_{2})^{-1}\phi_{1} + \phi_{2} & if \ \phi_{1}\ is\ small \\
\mathbf{J}(\phi_{1})^{-1}\phi_{2} + \phi_{1} & if \ \phi_{2}\ is\ small
\end{matrix}\right.
对于第一个近似为例，该式告诉我们，当对一个旋转矩阵\mathbf{R}_{2}(李代数为\phi_{2})左乘一个微小的旋转矩阵\mathbf{R}_{1}(李代数为\phi_{1})时，可以近似地看作，在原有地李代数\phi_{2}上，加上一项\mathbf{J}(\phi_{2})^{-1}\phi_{1}。同理，第二个近似描述了一个微小位移地情况，于是，李代数在BCH近似下，分成了左乘近似和右乘近似两种，使用时需要注意，使用的是左乘模型还是右乘模型。
以左乘模型为例，左乘BCH近似雅可比\mathbf{J}_{1}事实上就是：
\mathbf{J}_{1} = \mathbf{J} = \frac{\sin\theta}{\theta}\mathbf{I} + (1-\frac{\sin\theta}{\theta})aa^{T} + \frac{1-\cos\theta}{\theta}a^{\wedge}
它的逆为：
\mathbf{J}_{1}^{-1} = \frac{\theta}{2}\cot\frac{\theta}{2}+(1-\frac{\theta}{2}\cot\frac{\theta}{2})\mathbf{aa^{T}}-\frac{\theta}{2}\mathbf{a}^{\wedge}
而右乘雅可比仅需要对自变量取负号即可：
\mathbf{J}_{r}(\phi) = \mathbf{J}_{l}(-\phi)
这样就可以讨论李群乘法与李代数加法的关系了。BCH近似的意义：假定对某个旋转\mathbf{R}，对应李代数为\phi，给它左乘一个微笑旋转，记作\Delta\mathbf{ R}，对应李代数为\Delta\phi，那么李群上得到结果就是\Delta \mathbf{R}\cdot \mathbf{R}，而在李代数上，根据BCH近似，为：\mathbf{J}_{1}^{-1}(\phi)\Delta\phi + \phi，合并起来，可以简单写成：
exp(\Delta\phi^{\wedge})exp(\phi\wedge)=exp((\phi + \mathbf{J}_{l}^{-1}(\phi)\Delta\phi)^{\wedge})
反之，若在李代数上进行加法，让一个\phi加上\Delta\phi，那么可以近似为李群上带左右雅可比的乘法：
exp((\phi+\Delta\phi)^{\wedge})=exp((\mathbf{J}_{l}\Delta\phi)^{\wedge})exp(\phi^{\wedge})=exp(\phi^{\wedge})exp((\mathbf{J}_{r}\Delta\phi))
4.2 SO(3)李代数上求导
在SLAM种，要估计一个相机的位置和姿态，该位姿是由SO(3)上的旋转矩阵和SE(3)上的变换矩阵描述，假设某个时刻相机位姿为T，它观察到一个世界坐标位于p的点，产生一个观测数据z，那么由位姿变换关系知：
z=\mathbf{T}p+w
由于观测噪声w的存在，z往往不可能准确地满足z=\mathbf{T}p关系，所以通常会计算理想值与实际数据之间误差：
e=z-\mathbf{T}p
假设一共有N个这样地路标点和观测，就有了N个上面的式子，那么对于相机的位姿估计，相当云寻找一个最优\mathbf{T}，使得整体误差最小化：
\min_{\mathbf{T}}\mathbf{J(T)}=\sum_{i=1}^{N} \left \| z_{i} - \mathbf{T}p_{i} \right \|
计算此类问题，需要计算目标函数\mathbf{J} 关于变换矩阵\mathbf{T}的导数，以调整当前的估计值。若将\mathbf{T}当作一个普通矩阵来处理优化，就必须对它加以约束，而从李代数角度来说，由于李代数由向量组成，具有良好的加法运算，因此使用李代数解决导数问题思路分两种：
- (1) 用李代数表示姿态，然后根据李代数加法对李代数求导；
- (2) 对李群左乘或右乘微小扰动，然后对该扰动求导，称为左扰动或右扰动模型。
第一种方式对应到李代数的求导模型，而第二种则对应扰动模型。
4.3 李代数求导
首先，考虑SO(3)上的情况，假设对一个空间点p进行了旋转，得到\mathbf{R}p，现在，要计算旋转后点的坐标相对于旋转的导数：
\frac{\partial (\mathbf{R}p)}{\partial \mathbf{R}}
由于SO(3)没有加法，所以该导数无法按照导数定义进行计算，设\mathbf{R}对应李代数为\phi，我们转而计算：
\frac{\partial (exp(\phi^{\wedge})p)}{\partial \phi} \\
=\lim_{\delta\phi\rightarrow 0}\frac{exp((\phi+\delta\phi)^{\wedge})p-exp(\phi^{\wedge})p}{\delta\phi} \\
= \lim_{\delta\phi\rightarrow 0}\frac{exp((\mathbf{J}_{l}\delta\phi)^{\wedge})exp(\phi^{\wedge})p-exp(\phi^{\wedge})p}{\delta\phi} \\
\approx \lim_{\delta\phi\rightarrow 0}\frac{(\mathbf{I}+(\mathbf{J}_{l}\delta\phi)^{\wedge})exp(\phi^{\wedge})p-exp(\phi^{\wedge})p}{\delta\phi} \\
= \lim_{\delta\phi\rightarrow 0}\frac{(\mathbf{J}_{l}\delta\phi)^{\wedge}exp(\phi^{\wedge})p}{\delta\phi} \\
= \lim_{\delta\phi\rightarrow 0}\frac{-(exp(\phi^{\wedge})p)^{\wedge}\mathbf{J}_{l}\delta\phi}{\delta\phi} \\
= -(\mathbf{R}p)^{\wedge}\mathbf{J}_{l}
由于这里还是含有形式复杂的\mathbf{J}_{l}，我们不太希望计算它，而下面扰动模型提供了更简单的导数计算方式。
4.4 扰动模型(左乘)
另一种求导方式，是对\mathbf{R}进行一次扰动\Delta \mathbf{R}，这个扰动可以乘在左边也可以是右边，最后结果会有一点微小差异，这里以左扰动为例，设左扰动\Delta \mathbf{R}对应李代数为\varphi ，然后对\varphi求带：
\frac{\partial (\mathbf{R}p)}{\partial \varphi} = \lim_{\varphi \rightarrow 0}\frac{exp(\varphi^{\wedge})exp(\phi^{\wedge})p-exp(\phi^{\wedge})p}{\varphi}
对上式求导比上面更为简单：
\frac{\partial (\mathbf{R}p)}{\partial \varphi} = \lim_{\varphi \rightarrow 0}\frac{exp(\varphi^{\wedge})exp(\phi^{\wedge})p-exp(\phi^{\wedge})p}{\varphi} \\
\approx \lim_{\varphi\rightarrow 0} \frac{(1 + \varphi^{\wedge})exp(\phi^{\wedge})p-exp(\phi^{\wedge})p}{\varphi} \\
=\lim_{\varphi\rightarrow 0} \frac{ \varphi^{\wedge}\mathbf{R}p}{\varphi}
= \lim_{\varphi\rightarrow 0} \frac{ -(\mathbf{R}p)^{\wedge}\varphi}{\varphi}=-(\mathbf{R}p)^{\wedge}
对于SE(3)上的扰动模型，假设某空间点p经过一次变换\mathbf{T}(对于李代数为\xi)，得到\mathbf{T}p，给\mathbf{T}左乘一个扰动\Delta \mathbf{T}=exp(\delta\xi^{\wedge})，设扰动项的李代数为\delta \xi = \begin{bmatrix}\delta\rho,\delta\phi \end{bmatrix}，那么：
\frac{\partial(\mathbf{T}p)}{\partial\delta\xi} = \lim_{\delta\xi\rightarrow 0}\frac{exp(\delta\xi^{\wedge})exp(\xi^{\wedge})p - exp(\xi^{\wedge})p}{\delta\phi} \\
\approx \lim_{\delta\xi\rightarrow 0}\frac{(\mathbf{I} + \delta\xi^{\wedge})exp(\xi^{\wedge})p - exp(\xi^{\wedge})p}{\delta\phi} \\
= \lim_{\delta\xi\rightarrow 0}\frac{\delta\xi^{\wedge}exp(\xi^{\wedge})p}{\delta\phi}\\
= \lim_{\delta\xi\rightarrow 0}\frac{
\begin{bmatrix}
\delta\phi^{\wedge} && \delta\rho \\
\mathbf{0}^{T} && 0
\end{bmatrix}
\begin{bmatrix}
\mathbf{R}p + t \\
1
\end{bmatrix}
}
{\delta\phi} \\
\lim_{\delta\xi\rightarrow 0}\frac{
\begin{bmatrix}
\delta\phi^{\wedge}(\mathbf{R}p + t) + \delta\rho \\
0
\end{bmatrix}
}
{\delta\phi} \\
= \begin{bmatrix}
\mathbf{I} && -(\mathbf{R}p+t)^{\wedge} \\
\mathbf{0}^{T} && \mathbf{0}^{T}
\end{bmatrix} \doteq (\mathbf{T}p)^{\odot}
最后结果定义为一个算符^{\odot}，它将一个齐次坐标的空间点变换成一个4\times 6的矩阵。
参考资料
- [1] 高翔,张涛,刘毅,等.视觉SLAM十四讲:从理论到实践[M].电子工业出版社,2019.
