视觉SLAM中的数学基础 第三篇 李群与李代数
前言
在SLAM中，除了表达3D旋转与位移之外，我们还要对它们进行估计，因为SLAM整个过程就是在不断地估计机器人的位姿与地图。为了做这件事，需要对变换矩阵进行插值、求导、迭代等操作。例如，在经典ICP问题中，给定了两组3D点，我们要计算它们之间的变换矩阵。假设第一组的3D点为$mathbf{P}={ mathbf{p}_i | i = [1,2, ldots, N] }$，第二组3D点为$mathbf{Q}={ mathbf{q}_i | i = [1,2, ldots, N] }$，那我们实际要做的事情是求一个欧氏变换$mathbf{T}$，使得$mathbf{T}$满足：
[egin{equation}
forall i, quad mathbf{q}_i = mathbf{T} mathbf{p}_i
end{equation}]
注意这里使用了齐次坐标表示。通常，这许多个匹配过的点是通过特征匹配得到的，构成了一个超定方程。而由于噪声的存在，这个方程往往是无解的。因此我们转而计算一个最小二乘：
[egin{equation}
mathop {min }limits_{mathbf{T}} uleft( {mathbf{T}}
ight) = sumlimits_{i = 1}^N {{{left| {{mathbf{q}_i} - mathbf{T} {mathbf{p}_i}}
ight|}^2}}
end{equation}]
这时问题就来了：如果用迭代方式求解这个优化时（尽管可以不用迭代方式来求），如何求目标函数$u$相对于$mathbf{T}$的导数呢？首先，$mathbf{T}$只有6 个自由度，最好能够在一个六维空间表达它，那么$u(mathbf{T})$相对于这个六维空间的导数（雅可比矩阵）是一个$6 imes 6$的矩阵。其次，$mathbf{T}$对于乘法是封闭的，但对加法不封闭，即任意两个变换矩阵相加后并不是一个变换矩阵，这主要是因为旋转矩阵对加法是不封闭的。
出于这两个原因，我们希望有更好的数学工具帮助我们做这些事，而李群与李代数理论正好提供了这样的工具。李群与李代数广泛地用于机器人与计算机视觉领域，并在机器人动力学推导上占据重要地位。不过，由于SLAM不涉及过多的动力学推导。我们重点介绍它在SLAM中相关的几个重要的结果，而略去许多数学性质的证明。特别地，重点介绍$SO(3)$和$SE(3)$这两个李群与对应的李代数。
李代数基础
首先，我们来讨论较为简单的三维旋转群。为了说明它的结构，首先介绍群的概念。
群
群（Group）是一种集合加上一种运算的代数结构，记作$(A,cdot)$。其中$A$代表集合，$cdot$是定义在该集合上的二元运算。那么，如果这个运算满足以下几个条件，则称$G=(A, cdot)$为群。
- 封闭性: $ quad forall a_1, a_2, quad a_1 cdot a_2 in A$
- 结合律: $ quad forall a_1, a_2, a_3, quad (a_1 cdot a_2) cdot a_3 = a_1 cdot ( a_2 cdot a_3) $
- 幺元: $ quad exists a_0 in A, quad s.t. quad forall a in A, quad a_0 cdot a = a cdot a_0 = a $
- 逆: $ quad forall a in A, quad exists a^{-1} in A, quad s.t. quad a cdot a^{-1} = a_0 $
读者可以记作“封结幺逆”（谐音凤姐咬你），并可以把一些常见的群放进去验证。例如整数的加法（幺元为0），去掉0后的有理数的乘法（幺元为1）。对于矩阵，可以找到一些常见的矩阵群，例如：
- 一般线性群$GL(n)$ 指$n imes n$的可逆矩阵，它们对矩阵乘法成群。
- 特殊正交群$SO(n)$ 也就是所谓的旋转矩阵群，其中$SO(2)$ 和$SO(3)$最为常见。正式的记法是：
[egin{equation}
SO(n) = { mathbf{R} in mathbb{R}^{n imes n} | mathbf{R R}^T = mathbf{I}, det(mathbf{R})=1 }
end{equation}]
- 特殊欧氏群$SE(n)$ 也就是前面提到的$n$维欧氏变换，如$SE(2)$和$SE(3)$。这里给出$SE(3)$的记法：
[egin{equation}
SE(3) = left{ mathbf{T} = left[ {egin{array}{*{20}{c}}
mathbf{R} & mathbf{t} \
{{mathbf{0}^T}} & 1
end{array}}
ight]
in mathbb{R}^{4 imes 4} | mathbf{R} in SO(3), mathbf{t} in mathbb{R}^3
ight}
end{equation}]
群结构保证了在群上的运算具有良好的性质，而群论则研究群的各种结构和性质，但我们在此不多加介绍。感兴趣的读者可以参考任意一本近世代数教材。
李群是指具有连续性质的群。并且，一般连续群上的运算还是无限可微，乃至解析的（解析比无限可微更强，它还要求任意点邻域的泰勒展开都收敛）。这个问题在20世纪初被称为希尔伯特第五问题，并已得到了解决。而李群，则指实数空间上的连续群。常见的李群包括上边提到的$GL(n), SO(n), SE(n)$，以及其他的如酉群$U(n)$，辛群$Sp(2n)$等等。
三维旋转群$SO(3)$
三维旋转群$SO(3)$是特殊正交群$SO(n)$在$n=3$时的特例，它们可以用来描述三维空间的旋转，其元素都是$3 imes3$ 的正交且行列式为$+1$的矩阵。假设有这样一个矩阵$mathbf{R}$，满足$mathbf{R} mathbf{R}^T＝mathbf{I}$。现在，考虑它随时间发生变化，即从$mathbf{R}$ 变成了$mathbf{R}(t)$，仍有$mathbf{R}(t) mathbf{R}(t) ^T = mathbf{I}$。在等式两边对时间求导，得到：
[egin{equation}
mathbf{dot{R}} (t) mathbf{R} {(t)^T} + mathbf{R} (t) mathbf{dot{R}} {(t)^T} = 0
end{equation}]
于是：
[egin{equation}
mathbf{dot{R}} (t) mathbf{R} {(t)^T} = - left( mathbf{dot{R}} (t) mathbf{R} {(t)^T}
ight)^T
end{equation}]
可以看出$mathbf{dot{R}} (t) mathbf{R} {(t)^T}$是一个反对称矩阵。注意到对于任意一个$3 imes 3$的反对称矩阵，我们记它为$mathbf{A}$。由于$mathbf{A}^T=-mathbf{A}$，所以它主对角线元素必为$0$，而非对角线元素则只有三个自由度。我们可以把它对应到一个向量$mathbf{a}=[a_1, a_2, a_3]^T$中去：
[egin{equation}
{mathbf{a}^ wedge } = mathbf{A} = left[ {egin{array}{*{20}{c}}
0&{ - {a_3}}&{{a_2}}\
{{a_3}}&0&{ - {a_1}}\
{ - {a_2}}&{{a_1}}&0
end{array}}
ight]
end{equation}]
其中$^{wedge}$符号表示由向量转换为矩阵，反之我们也可以用符号$^{vee}$定义由矩阵转换为向量的方式：
[egin{equation}
{ mathbf{A}^ vee } = mathbf{a}
end{equation}]
注意到这样定义的好处之一，是它与叉积的兼容性。我们可以直接把矩阵与任意向量的乘积$mathbf{A} mathbf{b} $写成 $mathbf{a} imes mathbf{b}$。读者可以自行验证这个兼容性。除此之外，这样定义的向量还有一些较好的性质，后文会提到。
现在，由于$mathbf{dot{R}} (t) mathbf{R} {(t)^T}$是一个反对称矩阵，我们可以找到一个三维向量$mathbf{phi} (t) in mathbb{R}^3$与之对应。于是有：
[egin{equation}
mathbf{ dot{R} } (t) mathbf{R}(t)^T = mathbf{phi} (t) ^ {wedge}
end{equation}]
左右各右乘$mathbf{R}(t)$，由于$mathbf{R}$为正交阵，有：
[egin{equation}
mathbf{ dot{R} } (t) = mathbf{phi} (t)^{wedge} mathbf{R}(t) =
left[ {egin{array}{*{20}{c}}
0&{ - {phi _3}}&{{phi _2}}\
{{phi _3}}&0&{ - {phi _1}}\
{ - {phi _2}}&{{phi _1}}&0
end{array}}
ight] mathbf{R} (t)
end{equation}]
可以看到，每对旋转矩阵求一次导数，只需左乘一个$mathbf{phi}$矩阵即可。由于$mathbf{phi}$反映了$mathbf{R}$的导数性质，故称它在$SO(3)$的正切空间(tangent space)上。同时，将上式类比于一个关于$mathbf{R}$的微分方程，可得：
[egin{equation}
label{eq:so3ode}
mathbf{R}(t) = exp left( mathbf{phi} (t) ^wedge
ight) mathbf{R}(t_0)
end{equation}]
由此我们可以引出两个概念。（1）求$mathbf{phi}$的方法以及它的结构？——$mathbf{phi}$是对应到$SO(3)$上的李代数$mathfrak{so}(3)$；（2）$exp( mathbf{phi})$如何计算？——李群与李代数间的指数/对数映射。下面我们一一加以介绍。
什么是李代数
对于$SO(3)$和$SE(3)$，李代数可定义于李群的正切空间上，描述了李群中元素局部性质，分别把它们记作小写的$mathfrak{so}(3)$和$mathfrak{se}(3)$。首先，给出通用的李代数的定义。
李代数由一个集合$mathbb{V}$，一个数域$mathbb{F}$和一个二元运算$[]$组成。如果它们满足以下几条性质，称$(mathbb{V}, mathbb{F}, [])$ 为一个李代数，记作$mathfrak{g}$。
- 封闭性 $forall mathbf{X}, mathbf{Y} in mathbb{V}, [mathbf{X} mathbf{Y}] in mathbb{V}$
- 双线性 $forall mathbf{X,Y,Z} in mathbb{V}, a,b in mathbb{F}, $ 有 $$ [amathbf{X}+bmathbf{Y}, mathbf{Z}] = a[mathbf{X}mathbf{Z}] + b [ mathbf{Y} mathbf{Z} ] quad [mathbf{Z}, a mathbf{X}+bmathbf{Y}] = a [mathbf{Z} mathbf{X} ]+ b [mathbf{ZY}] $$
- 自反性 $forall mathbf{X} in mathbb{V}, [mathbf{X} mathbf{X}] = mathbf{0}$
- 雅可比等价 $forall mathbf{X,Y,Z} in mathbb{V}, [mathbf{X}, [mathbf{YZ}] ] + [mathbf{Z}, [mathbf{YX}] ] + [mathbf{Y}, [mathbf{ZX}]] $
从表面上来看，李代数所需要的性质还是挺多的。其中二元运算被称为李括号。相比于群中的较为简单的二元运算，李括号表达了两个集合元素的差异。它不要求结合律，而满足反对称性，以及元素和自己做李括号之后为零的性质。作为类比，三维向量$mathbb{R}^3$ 上定义的叉积$ imes$是一种李括号，因此$mathfrak{g} = (mathbb{R}^3, mathbb{R}, imes)$构成了一个李代数。读者可以尝试将叉积的性质代入到上面四条性质中。
三维旋转群与对应的李代数
$SO(3)$对应的李代数是定义在$mathbb{R}^3$上的向量，我们记作$mathbf{phi}$（注意这是个向量，虽然希腊字母的粗体不明显）。根据前面的推导，每个$mathbf{phi}$都可以生成一个反对称矩阵：
[egin{equation}
label{eq:phi}
mathbf{Phi} = mathbf{phi}^{wedge} = left[ {egin{array}{*{20}{c}}
0&{ - {phi _3}}&{{phi _2}}\
{{phi _3}}&0&{ - {phi _1}}\
{ - {phi _2}}&{{phi _1}}&0
end{array}}
ight] in mathbb{R}^{3 imes 3}
end{equation}]
在此定义下，两个向量$mathbf{phi}_1, mathbf{phi}_2$的李括号为：
[egin{equation}
[mathbf{phi}_1, mathbf{phi}_2] = mathbf{ Phi }_1 mathbf{ Phi }_2 - mathbf{ Phi }_2 mathbf{ Phi }_1
end{equation}]
读者可以去验证该定义下的李括号满足上面的几条性质。由于$mathbf{phi}$ 与反对称矩阵关系很紧密，在不引起歧义的情况下，就说$mathfrak{so}(3)$的元素是3维向量或者3维反对称矩阵，不加区别：
[egin{equation}
mathfrak{so}(3) = left{ Phi = mathbf{phi^wedge} in mathbb{R}^{3 imes 3} | mathbf{phi} in mathbb{R}^3
ight}
end{equation}]
反对称矩阵有一些重要的性质，重点包括以下两条：
[egin{equation}
mathbf{phi} mathbf{phi}^T = mathbf{phi}^{wedge} mathbf{phi}^{wedge} + | mathbf{phi} |^2 mathbf{I}_{3 imes 3}
end{equation}]
当$mathbf{phi}$为单位向量时，进而有：
[egin{equation}
mathbf{phi} mathbf{phi}^T = mathbf{phi}^{wedge} mathbf{phi}^{wedge} + mathbf{I}1
end{equation}]
以及
[egin{equation}
mathbf{phi}^{wedge} mathbf{phi}^{wedge} mathbf{phi}^{wedge} = - mathbf{phi}^{wedge}
end{equation}]
这两条性质读者也可以自行验证，我们在指数映射中会用到。
至此，我们已清楚了$mathfrak{so}(3)$的结构。它们是一个由三维向量组成的集合，每个向量对应到一个反对称矩阵，可以表达旋转矩阵的导数。现在来考虑$exp ( mathbf{phi}^{wedge} )$是如何计算的，为此我们引入指数映射。
指数映射
首先，回忆任意矩阵的指数映射。它可以写成一个泰勒展开，但是只有在收敛的情况下才会有结果，其结果仍是一个矩阵。
[egin{equation}
exp(mathbf{A}) = sumlimits_{n = 0}^infty {frac{1}{{n!}}{ mathbf{A}^n}}
end{equation}]
同样地，对$mathfrak{so}(3)$中任意一元素$mathbf{phi}$，我们亦可按此方式定义它的指数映射：
[egin{equation}
exp(mathbf{phi}^wedge) = sumlimits_{n = 0}^infty {frac{1}{{n!}}{ (mathbf{phi}^{wedge})^n}}
end{equation}]
现在我们来仔细看看它的含义。由于$mathbf{phi}$是三维向量，我们可以定义它的模长和它的方向，分别记作$ heta$和$mathbf{a}$（注意这里记号是有含义的，此时$mathbf{a}$是一个单位长度的向量），那么按照上式，可以推出如下公式，注意中间使用了上面讲到了两个反对称矩阵的性质：
[egin{align*}
exp left( {{mathbf{phi} ^ wedge }}
ight) &= exp left( { heta {mathbf{a}^ wedge }}
ight) = sumlimits_{n = 0}^infty {frac{1}{{n!}}{{left( { heta {mathbf{a}^ wedge }}
ight)}^n}} \
&= mathbf{I} + heta {mathbf{a}^ wedge } + frac{1}{{2!}}{ heta ^2}{mathbf{a}^ wedge }{mathbf{a}^ wedge } + frac{1}{{3!}}{ heta ^3}{mathbf{a}^ wedge }{mathbf{a}^ wedge }{mathbf{a}^ wedge } + frac{1}{{4!}}{ heta ^4}{left( {{mathbf{a}^ wedge }}
ight)^4} + ...\
&= mathbf{a} {mathbf{a}^T} - {mathbf{a}^ wedge }{mathbf{a}^ wedge } + heta {mathbf{a}^ wedge } + frac{1}{{2!}} heta {mathbf{a}^ wedge }{mathbf{a}^ wedge } - frac{1}{{3!}}{ heta ^3}{mathbf{a}^ wedge } + frac{1}{{4!}}{ heta ^4}{left( {{mathbf{a}^ wedge }}
ight)^4} + ...\
&= mathbf{a}{mathbf{a}^T} + left( { heta - frac{1}{{3!}}{ heta ^3} + frac{1}{{5!}}{ heta ^5} - ...}
ight){mathbf{a}^ wedge } - left( {1 - frac{1}{{2!}}{ heta ^2} + frac{1}{{4!}}{ heta ^4} - ...}
ight){mathbf{a}^ wedge }{mathbf{a}^ wedge }\
&= {mathbf{a}^ wedge }{mathbf{a}^ wedge } + mathbf{I} + sin heta {mathbf{a}^ wedge } - cos heta {mathbf{a}^ wedge }{mathbf{a}^ wedge }\
&= (1 - cos heta ){mathbf{a}^ wedge }{mathbf{a}^ wedge } + I + sin heta {mathbf{a}^ wedge }\
&= cos heta mathbf{I} + (1 - cos heta )mathbf{a}{mathbf{a}^T} + sin heta {mathbf{a}^ wedge } \
end{align*}]
最后我们得到了一个似曾相识的式子：
[egin{equation}
exp( heta mathbf{a} ) = cos heta mathbf{I} + (1 - cos heta )mathbf{a}{mathbf{a}^T} + sin heta {mathbf{a}^ wedge }
end{equation}]
回忆前一节内容，它和罗德里格斯公式（参观本系列第一篇）如出一辄。这表明，$mathfrak{so}(3)$实际上就是由所谓的旋转向量组成的空间。特别地，当转轴取一定顺序时，李代数$mathfrak{so}(3)$还会变为对应的欧拉角。通过罗德里格斯公式或者指数映射，我们把$mathbb{R}^3$ 中的一个向量对应到了一个位于$SO(3)$中的3D旋转。
反之，如果定义对数映射，我们也能把$SO(3)$中的元素对应到$mathfrak{so}(3)$中：
[egin{equation}
mathbf{phi} = ln {left( mathbf{R}
ight)^ vee } = {left( {sumlimits_{n = 0}^infty {frac{{{{left( { - 1}
ight)}^n}}}{{n + 1}}{{left( { mathbf{R} - mathbf{I}}
ight)}^{n + 1}}} }
ight)^ vee }
end{equation}]
其中$^vee$表示从反对称矩阵到向量的对应关系，为$^wedge$的逆运算。
读者可能会问，指数映射性质如何呢？它是一个双射吗？很遗憾，它只是一个满射。每个$SO(3)$中的元素，都可以找到$mathfrak{so}(3)$中至少一个与之对应；但是可能存在多个$mathfrak{so}(3)$中的元素，对应到同一个$SO(3)$元素上。至少对于旋转角$ heta$，我们知道它具有周期性。
$SO(3)$与$mathfrak{so}(3)$的结论似乎在我们意料之中。它和我们前面讲的旋转向量与旋转矩阵很相似，而指数映射即是罗德里格斯公式。旋转向量可以视为旋转矩阵的导数，指导如何在旋转矩阵中进行微积分运算。
三维欧氏群与对应的李代数
下面我们来介绍三维欧氏群$SE(3)$以及对应的李代数$mathfrak{se}(3)$。有了前面的基础，我们可以直接介绍它们的结构及运算了。$SE(3)$的结构已经在前面介绍群的时候给出：
[egin{equation}
SE(3) = left{ mathbf{T} = left[ {egin{array}{*{20}{c}}
mathbf{R} & mathbf{t} \
{{mathbf{0}^T}} & 1
end{array}}
ight]
in mathbb{R}^{4 imes 4} | mathbf{R} in SO(3), mathbf{t} in mathbb{R}^3
ight}
end{equation}]
每个变换矩阵有六个四由度，故对应的李代数位于$mathbb{R}^6$中：
[egin{equation}
mathfrak{se}(3) = left{ mathbf{ Xi } = mathbf{xi}^wedge in mathbb{R}^{4 imes 4} | mathbf{xi} in mathbb{R}^6
ight}
end{equation}]
但是$^wedge$不再对应到一个反对称关系，而是：
[egin{equation}
mathbf{xi}^wedge = {left[ egin{array}{l}
mathbf{
ho} \
mathbf{phi}
end{array}
ight]^ wedge } = left[ {egin{array}{*{20}{c}}
{{mathbf{phi} ^ wedge }}&mathbf{
ho} \
{{mathbf{0}^T}}&0
end{array}}
ight] = mathbf{Xi}
end{equation}]
可以看到，$mathbf{xi}$ 的前三维为旋转向量，后三维为平移向量，其定义也十分的直观。该李代数对应于微分方程：
[egin{equation}
mathbf{dot{T}}(t) = mathbf{xi}^wedge(t) mathbf{T}(t)
end{equation}]
因此
[egin{equation}
mathbf{T}(t) = exp ( mathbf{xi}(t)^wedge ) mathbf{T}(t)
end{equation}]
那么$mathfrak{se}(3)$上的指数映射如何呢？略加推导可得：
[egin{align}
exp left( {{ mathbf{xi} ^ wedge }}
ight) &= left[ {egin{array}{*{20}{c}}
{sumlimits_{n = 0}^infty {frac{1}{{n!}}{{left( {{mathbf{phi} ^ wedge }}
ight)}^n}} }&{sumlimits_{n = 0}^infty {frac{1}{{left( {n + 1}
ight)!}}{{left( {{mathbf{phi} ^ wedge }}
ight)}^n} mathbf{
ho} } }\
{{mathbf{0}^T}}&1
end{array}}
ight] \
&= left[ {egin{array}{*{20}{c}}
mathbf{Phi} &{mathbf{J
ho} } \
{{mathbf{0}^T}}&1
end{array}}
ight]
end{align}]
左上角的$mathbf{Phi}$是我们熟知的$mathfrak{so}(3)$中的元素，前文已经介绍过了。而右上角的$mathbf{J}$则可整理为（设$mathbf{phi}= hetamathbf{a}$）：
[egin{equation}
mathbf{J} = frac{{sin heta }}{ heta } mathbf{I} + left( {1 - frac{{sin heta }}{ heta }}
ight) mathbf{a} { mathbf{a}^T} + frac{{1 - cos heta }}{ heta }{ mathbf{a}^ wedge }
end{equation}]
因此我们就得到了$mathfrak{se}(3)$的指数映射的关系。 其对数映射亦可类比推得。
小结
最后，我们对之前介绍的李群李代数进行一个简单的小结。概而言之，李群有以下两个重要用处：
- 李代数表达的正切空间，具有和对应李群相同的自由度。
- 指数映射能把正切空间中任意向量正好映射到原李群。
下篇中，我们将教大家用Eigen和Sophus库处理变换矩阵与李代数。敬请期待。
参考资料
[1]. Yi Ma, An Invitation to 3D Vision. 2001.
[2]. Timothy D. Barfoot, State Estimation for Robotics: A Matrix-Lie-Group Approach, 2015.
如果你觉得我的博客有帮助，可以进行几块钱的小额赞助，帮助我把博客写得更好。
