【SLAM】02_李群与李代数

提示：本笔记主要以SLAM实用为目的，故省略我认为不需要的数学推导。需要详细数学推导的读者请阅读 《视觉SLAM十四讲》第四讲

1. 李群与李代数概念

1.1 群

群(group)是 一种集合（记作$A$） 加上 一种运算 （记作 $\cdot$ ）的代数结构，群可记作 $G=(A,\cdot )$

具备以下性质，简称“封结幺逆”

我们需要接触到的群有：

• 特殊正交群$\mathrm{S}\mathrm{O}(\mathrm{n})$：也就是所谓的旋转矩阵群，即旋转矩阵$R$，其中$\mathrm{S}\mathrm{O}(2)$和$\mathrm{S}\mathrm{O}(3)$最为常见
• 特殊欧氏群$\mathrm{S}\mathrm{E}(\mathrm{n})$：也就是前面提到的n维欧氏变换，即变换矩阵$T$,如$\mathrm{S}\mathrm{E}(2)$和$\mathrm{S}\mathrm{E}(3)$

1.2 李群

李群是指具有连续（光滑）性质的群。
刚体在空间中的运动为连续的，故$\mathrm{S}\mathrm{O}(3)$与$\mathrm{S}\mathrm{E}(3)$都是李群，这两个李群对于摄像机姿态估计比较重要。

但是，$\mathrm{S}\mathrm{O}(3)$与 $\mathrm{S}\mathrm{E}(3)$ 有着更丰富的几何性，没有角运动，所以难以计算取根函数，求导等运算。

1.3 李代数

1.3.1 李代数的引入

任意旋转矩阵$R$满足：$R{R}^T=I$，因为旋转会随时间连续变化，故也可$\mathbf{R}(t)\mathbf{R}(t{)}^{\mathrm{T}}=\mathbf{I}$

经过变形之后可以得到：
$$\dot{R}(t)=\varphi (t{)}^{\wedge }R(t)=\left[\begin{array}{ccc}0& -{\varphi }_3& {\varphi }_2\\ {\varphi }_3& 0& -{\varphi }_1\\ -{\varphi }_2& {\varphi }_1& 0\end{array}\right]R(t)$$

可以看到，每对旋转矩阵求一次导数，只需左乘一个${\varphi }^{\wedge }(t)$矩阵即可。

设 $t_0=0$, 并设此时旋转矩阵为 $R(0)=I$ 把 $R(t)$ 在 $0$ 附近进行泰勒展开，保留一阶：
$$\mathbf{R}(t)\approx \mathbf{R}\left(t_0\right)+\dot{\mathbf{R}}\left(t_0\right)\left(t-t_0\right)=I+\varphi (t_0{)}^{\wedge }(t).$$

则 $\varphi$ 反映了某一时刻 $R$ 在局部的导数关系, 对应李群$\mathrm{S}\mathrm{O}(3)$的李代数$\mathfrak{s}\mathfrak{o}(3)$。

在$t_0$附近，设$\varphi$保持为常数$\varphi (t_0)={\varphi }_0$，将关于$R$的微分方程
$$\dot{\mathbf{R}}(t)=\mathbf{\varphi }(t_0{)}^{\wedge }\mathbf{R}(t)={\mathbf{\varphi }}_0^{\wedge }\mathbf{R}(t)$$

解得：

$$\mathbf{R}(t)=\exp \left({\mathbf{\varphi }}_0^{\wedge }t\right)$$
说明旋转矩阵可以由$\exp \left({\varphi }_0^{\wedge }t\right)$计算出来。
这正是李群与李代数之间的指数/对数映射。

后文主要使用$\mathrm{S}\mathrm{O}(3)$加上平移矢量的方式来推导后续的运动方程、滤波器等关系。

1.3.2 李代数的定义

每个李群都有与之对应的李代数。李代数描述了李群的局部性质，准确地说，是单位元附近的正切空间。

李代数$(\mathbb{V},\mathbb{F},[,])$由一个集合$\mathbb{V}$、一个数域 $\mathbb{F}$ 和一个二元运算$[,]$组成，记作$\mathfrak{g}$。

则$\mathfrak{s}\mathfrak{o}(3)$含义为：
$$\mathfrak{s}\mathfrak{o}(3)=\left\{\mathbf{\varphi }\in {\mathbb{R}}^3,\mathbf{\Phi }={\mathbf{\varphi }}^{\wedge }\in {\mathbb{R}}^{3\times 3}\right\}$$
它们是一个由三维向量组成的集合，每个向量对应一个反对称矩阵，可以用于表达旋转矩阵的导数。

它与$\mathrm{S}\mathrm{O}(3)$的关系由指数映射给定：$R=\exp ({\varphi }^{\wedge })$

至于$\mathfrak{s}\mathfrak{e}(3)$可以简单的理解为“由一个平移加上一个$\mathrm{S}\mathrm{O}(3)$元素构成的向量”

1.4 小结

三维旋转构成了三维旋转群 $\mathrm{S}\mathrm{O}(3)$,其对应的李代数为 $\mathfrak{s}\mathfrak{o}(3)$;
三维变换构成了三维变换群$\mathrm{S}\mathrm{E}(3)$, 对应的李代数为 $\mathfrak{s}\mathfrak{e}(3)$。

李代数元素到李群元素的映射为指数映射，其中$\mathrm{S}\mathrm{O}(3)$至 $\mathfrak{s}\mathfrak{o}(3)$的指数映射为

$$\mathbf{R}=\exp ({\mathbf{\varphi }}^{\wedge })$$

$\varphi$表示李代数$\mathfrak{s}\mathfrak{o}(3)$。

具体计算过程由罗德里格斯公式给出。反向的对数映射记作

$$\varphi =\log (\mathbf{R}{)}^{\vee }$$

2. $\mathrm{S}\mathrm{O}(3)$上的BCH线性近似式

2.1 BCH公式

Baker-Campbell-Hausdorff(BCH)公式公式是一个用来在李群和李代数之间建立联系的数学工具。这个公式描述了如何将李代数中的加法操作与相应的李群上的乘法操作联系起来。

在李代数中两个元素相加，相对应李群的元素并不是简单的相乘。

李代数$X$与$Y$，加法$X+Y$，李群上的元素$e^X$与$e^Y$（指数映射），其乘积并不是$e^X\cdot e^Y$，因为李群为矩阵并不是标量。

BCH给出了李代数上操作加法小量与李群上乘小量之间的关系，它告诉我们当处理两个矩阵指数之积的时候，它们会产生一些由李括号组成的余项。

李括号是李代数的二元运算$[,]$

具体来说，李代数$X$与$Y$相加，对应的李群(矩阵)乘积为：

$$Z=X+Y+\frac{1}{2}[X,Y]+\frac{1}{12}[X,[X,Y]]+\frac{1}{12}[Y,[Y,X]]+\cdots$$
$[X,Y]$即李括号。

BCH 公式的重要性在于它允许我们从李代数（一个相对简单的结构）推导出李群的行为（一个更复杂的结构）。

2.2 BCH线性近似式

BCH线性近似表达：
$$\ln {\left(\exp \left({\varphi }_1^{\wedge }\right)\exp \left({\varphi }_2^{\wedge }\right)\right)}^{\vee }\approx \{\begin{array}{ll}{\mathbf{J}}_l({\mathbf{\varphi }}_2{)}^{-1}{\mathbf{\varphi }}_1+{\mathbf{\varphi }}_2& \text{当}{\mathbf{\varphi }}_1\text{为小量},\\ {\mathbf{J}}_r({\mathbf{\varphi }}_1{)}^{-1}{\mathbf{\varphi }}_2+{\mathbf{\varphi }}_1& \text{当}{\mathbf{\varphi }}_2\text{为小量}.\end{array}$$

分为左乘近似与右乘近似两种，以左乘为例：

$${\mathbf{J}}_l(\theta a)=\mathbf{J}=\frac{\sin \theta }{\theta }\mathbf{I}+\left(1-\frac{\sin \theta }{\theta }\right)\mathbf{a}{\mathbf{a}}^{\mathrm{T}}+\frac{1-\cos \theta }{\theta }{\mathbf{a}}^{\wedge }$$
$${\mathbf{J}}_l^{-1}(\theta a)=\frac{\theta }{2}\cot \frac{\theta }{2}\mathbf{I}+\left(1-\frac{\theta }{2}\cot \frac{\theta }{2}\right)\mathbf{a}{\mathbf{a}}^{\mathrm{T}}-\frac{\theta }{2}{\mathbf{a}}^{\wedge }$$

而右雅各比矩阵仅需要对自变量取负号即可：
${\mathbf{J}}_r(\mathbf{\varphi })={\mathbf{J}}_l(-\mathbf{\varphi })$

(1) $\mathrm{S}\mathrm{O}(3)\Rightarrow \mathfrak{s}\mathfrak{o}(3)$

• 在李群中$\mathrm{S}\mathrm{O}(3)$，某个旋转矩阵$R$，其对应李代数为$\varphi$

• 一个微小旋转矩阵，记为$\Delta R$ ，其对应李代数$\Delta \varphi$

旋转矩阵进行微小旋转后，即对$R$左乘$\Delta R$，李群方面结果为$\Delta R\cdot R$

根据BCH公式近似，李代数方面结果为：$J_1^{-1}(\varphi )\Delta \varphi +\varphi$

合并后：

$$\Delta R\cdot R=\exp (\Delta {\varphi }^{\wedge })\exp ({\varphi }^{\wedge })=\exp ((J_1^{-1}(\varphi )\Delta \varphi +\varphi {)}^{\wedge })$$

(2) $\mathfrak{s}\mathfrak{o}(3)\Rightarrow \mathrm{S}\mathrm{O}(3)$

李代数方面进行加法$\Delta \varphi +\varphi$，则李群方面对应的乘法为：

$$\exp ((\Delta \varphi +\varphi {)}^{\wedge })=\{\begin{array}{ll}\exp ((J_l(\varphi )\Delta \varphi {)}^{\wedge })\exp ({\varphi }^{\wedge })& 左雅各比\\ \exp ({\varphi }^{\wedge })\exp ((J_r(\varphi )\Delta \varphi {)}^{\wedge })& 右雅各比\end{array}$$

3. 扰动模型

如果在李群上左乘或右乘增量(无论是用旋转矩阵表示，还是用四元数表示)，则其对应的李代数上就会存在一个对应的增量，而这两个增量之间，由于BCH公式的存在，在一阶线性近似意义下，会存在一个雅可比矩阵。

实际运用中经常会构建与位姿有关的函数，然后讨论该函数关于位姿的导数，以调整当前的估计值。

一般用李代数来解决求导问题：

• 李代数表示姿态，根据李代数加法对李代数进行求导
• 李群左乘或右乘微小扰动，然后对该扰动求导，称为左扰动和右扰动模型

3.1 李代数求导

对空间点$p$进行旋转，得到$Rp$，计算旋转之后点的坐标相对于旋转的导数：
$$\frac{\partial \left(\mathbf{R}\mathbf{p}\right)}{\partial \mathbf{\varphi }}={\left(-\mathbf{R}\mathbf{p}\right)}^{\wedge }{\mathbf{J}}_l$$

其中，$\varphi$为旋转矩阵$R$的李代数，这里存在复杂的$J_l$。

3.2 扰动模型（左乘）

扰动模型提供了更简单的导数计算方式。

对$R$进行一次扰动$\Delta R$，这个扰动由李代数元素 $\phi$ 描述，看结果相对于扰动的变化率即计算 $\mathbf{R}\mathbf{p}$（即 $\mathbf{R}$ 和 $\mathbf{p}$ 的组合）相对于 $\mathbf{\phi }$ 的变化率。

对$\phi$求导：
$$\frac{\partial \left(\mathbf{R}\mathbf{p}\right)}{\partial \mathbf{\phi }}=\underset{\mathbf{\phi }\to 0}{\lim }\frac{\exp \left({\mathbf{\phi }}^{\wedge }\right)\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}}{\phi }$$

经过$\exp ({\mathbf{\phi }}^{\wedge })$一阶泰勒展开、叉乘变换，得：
$$\frac{\partial \left(\mathbf{R}\mathbf{p}\right)}{\partial \mathbf{\phi }}=-(Rp{)}^{\wedge }$$

叉乘变换 $a^{\wedge }bc=a\times b\cdot c=-b\cdot c\times a$

4. 学习李群与李代数的原因

此处需要以后验证更新：

1. 描述机器人的运动和姿态
   在SLAM中，机器人的运动通常涉及到旋转和平移。李群提供了一种自然的框架来描述这些变换。例如，SE(2)群（二维特殊欧几里得群）用于描述二维空间中的平移和旋转，而SE(3)群（三维特殊欧几里得群）则用于描述三维空间中的平移和旋转。这些群的元素可以表示为机器人的位姿（位置和方向）。

2. 处理不确定性和噪声
   在实际应用中，机器人的传感器数据（如激光雷达、摄像头等）通常包含噪声。李群和李代数允许我们以一种数学上严谨的方式处理这些不确定性。通过使用李代数，我们可以在概率框架下（如卡尔曼滤波器）对机器人的运动和观测进行建模，从而估计机器人的位姿和地图。

3. 优化问题
   SLAM的核心是一个优化问题，目标是最小化机器人位姿和地图估计的误差。李群和李代数提供了一种有效的数学工具来表达和优化这些位姿。在优化过程中，我们通常需要计算雅可比矩阵（Jacobian），这是李群和李代数中的一个重要概念，它描述了函数对输入参数的敏感性。

4. 运动模型和观测模型
   在SLAM中，运动模型描述了机器人从一个位姿移动到另一个位姿的过程，而观测模型描述了传感器如何从环境特征中获取信息。李群和李代数为这些模型提供了一种紧凑且直观的表示方法，使得模型的数学表达和计算更加高效。

5. 算法的鲁棒性
   李群和李代数的引入使得SLAM算法更加鲁棒，能够处理复杂的运动和观测情况。例如，通过使用李群，我们可以更容易地处理机器人在非欧几里得空间（如球面或曲面）上的运动。

6. 计算效率
   李群和李代数的运算通常比传统的矩阵运算更高效。在SLAM中，这可以显著提高算法的计算速度，尤其是在处理大规模数据集时。