【视觉SLAM十四讲】李群和李代数

群

群实际上是一种集合 + 一种运算的代数结构，可以理解为一个光滑的弯曲空间。如 $G=(A,\cdot )$ ，群具有以下几个特性

1. 封闭性：$\forall a_1,a_2\in A,a_1\cdot 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,\text{s.t.}\forall a\in A,a_0\cdot a=a\cdot a_0=a$
4. 逆：$\forall a\in A,\exists a^{-1}\in A,\text{s.t.}a\cdot a^{-1}=a_0$

李代数

为什么需要李代数

在 SLAM 位姿优化中，直接处理旋转矩阵会面临正交性约束难题，所依我们需要引入李代数，将优化过程放到无约束的李代数空间中进行

从上面的推导中，我们可以得出几个重要的结论

1. 对旋转矩阵求导，只需对 $R(t)$ 左乘一个反对称矩阵 ${\varphi }^{\wedge }$
2. 我们最终得到的 $R(t)=exp({\varphi }_0^{\wedge }t)$ ，揭示了李群和李代数的转换关系

定义

李代数描述了李群的局部性质，即李群单位元附近的正切空间。李代数由**一个集合 + 一个数域 + 一个二元运算符号（李括号）**组成（如 $\mathfrak{g}=(\mathbb{V},\mathbb{F},[,])$），集合具有如下几个特性

1. 封闭性：$\forall X,Y\in \mathbb{V},[X,Y]\in \mathbb{V}$
2. 双线性：$\forall X,Y,Z\in \mathbb{V},a,b\in \mathbb{F},$ 有 $[aX+bY,Z]=a[X,Z]+b[Y,Z],[X+a,Y]=[X,Y]+[a,Y]$
3. 自反性：$\forall X\in \mathbb{V},[X,X]=0$
4. 雅可比等价：$\forall X,Y,Z\in \mathbb{V},[X,[Y,Z]]+[Z,[X,Y]]+[Y,[Z,X]]=0$

在接下来的部分，我们将加入几个新的表述，$\mathrm{SO}(3),\mathfrak{so}(3)$ 将代表旋转部分的李群和李代数，而$\mathrm{SE}(3),\mathfrak{se}(3)$ 将代表变换部分的李群和李代数。

李代数 $\mathfrak{so}(3)$

前面推导出来的 $\varphi$ 其实就是 $\mathfrak{so}(3)$ ，可以用旋转向量或者旋转向量对应的反对称矩阵表示，二者没有区别
s o ( 3 ) = { ϕ ∈ R 3 , Φ = ϕ ∧ ∈ R 3 × 3 } \mathfrak{so}(3)=\{\phi \in \mathbb{R}^3,\Phi=\phi^\wedge \in \mathbb{R}^{3\times3}\} so(3)={ϕ∈R3,Φ=ϕ∧∈R3×3}
此时，李括号可表示为：$[{\varphi }_1,{\varphi }_2]=({\Phi }_1{\Phi }_2-{\Phi }_2{\Phi }_1{)}^{\vee }$ ，可以理解为量化了两种旋转操作的非交换性

李代数 $\mathfrak{se}(3)$

$$\mathfrak{se}(3)=\left\{\xi =\left[\begin{array}{c}\rho \\ \varphi \end{array}\right]\in {\mathbb{R}}^6,\rho \in {\mathbb{R}}^3,\varphi \in \mathfrak{so}(3),{\xi }^{\wedge }=\left[\begin{array}{c}{\varphi }^{\wedge }\rho \\ 0^{T}0\end{array}\right]\in {\mathbb{R}}^{4\times 4}\right\}$$

$\mathfrak{se}(3)$ 的元素是由三维平移向量 $\rho$ 和三维旋转向量$\varphi$（也就是 $\mathfrak{so}(3)$ ）组成的六维向量

此时，李括号可表示为：$[{\xi }_1,{\xi }_2]=({\xi }_1^{\wedge }{\xi }_2^{\wedge }-{\xi }_2^{\wedge }{\xi }_1^{\wedge }{)}^{\vee }$ ，或者说 $[({\omega }_1,{\upsilon }_1),({\omega }_2,{\upsilon }_2)]=({\omega }_1\times {\omega }_2,{\omega }_1\times {\upsilon }_2-{\omega }_2\times {\upsilon }_1)$
注意，不同于 $\mathfrak{se}(3)$ ，这并不是单纯表示物理运动的先后顺序差异，而是通过旋转与旋转、旋转与平移的非交换性共同显现而出的差异

指数映射与对数映射

$\mathrm{SO}(3)\to \mathfrak{so}(3)$

上面已经推导过了一次，我们知道从 $\mathrm{SO}(3)$ 到 $\mathfrak{so}(3)$ 只需经历一次指数映射（这里的 t 只是参数，实际优化中无需使用）
$$R(t)=\exp ({\varphi }^{\wedge }t)$$
我们在前面已经知道了 ${\varphi }^{\wedge }$ 其实就是旋转向量的反对称矩阵形式，我们可定义其为由旋转角 $\theta$ 和旋转轴 $a$ 组成的形式（模长是 $\theta$ ，方向是 $a$）

这里比较复杂就不推导了，大家感兴趣的自己去看看书就行，直接把结果放出来
$$\exp ({\varphi }^{\wedge }t)=\exp (\theta a^{\wedge })=\cos \theta I+(1-\cos \theta )a{a}^T+\sin \theta a^{\wedge }$$
很眼熟吧，这和我们前面三维空间刚体运动那章推导的罗德里格斯公式几乎一模一样。罗德里格斯公式用于旋转矩阵和旋转向量之间的转换，对于 $\mathrm{SO}(3)$ 转换到 $\mathfrak{so}(3)$ 也是同理

至于指数映射，也不需要复杂的计算，同样用前面提到过的旋转矩阵转旋转向量的式子即可
$$\theta =arccos\frac{tr(R)-1}{2}Rn=n$$

$\mathrm{SE}(3)\to \mathfrak{se}(3)$

这里就不展开讲了，相关的转换看图就好

李代数求导与扰动模型

前面提到过引入李代数是为了优化流程，因为 SLAM 优化的本质就是最小化重投影误差等损失函数，那求导肯定是必不可少的

BCH 公式及其近似形式

在优化结束之后，我们也需要将当前变换和增量变换进行整合（注意！优化针对的是增量，而不是整个变换）

因为不能简单的直接将两个李代数指数映射的乘积定义为它们和的指数映射（因为指数映射非单映射，可能出现多个李代数元素对应同一个李群元素），即
$$exp({\varphi }_1^{\wedge })exp({\varphi }_2^{\wedge })\ne exp(({\varphi }_1+{\varphi }_2{)}^{\wedge })$$
所以我们需要使用 BCH 公式来完成这一步骤，因为原式较长，在 $\mathrm{SO}(3)$ 中，我们通常使用其如下线性近似表达
$$ln(exp({\varphi }_1^{\wedge })exp({\varphi }_2^{\wedge }){)}^{\vee }\approx \{\begin{array}{l}{J}_l({\varphi }_2{)}^{-1}{\varphi }_1+{\varphi }_2若{\varphi }_1为小量\\ J_l({\varphi }_1{)}^{-1}{\varphi }_2+{\varphi }_1若{\varphi }_2为小量\end{array}$$
其中的 $J_l$ 为左雅各比矩阵，与前面 $\mathfrak{se}(3)$ 处提到的一致
$$\begin{gathered} J_l(\varphi )=\frac{\sin \theta }{\theta }+\left(1-\frac{\sin \theta }{\theta }\right)a{a}^T+\frac{1-\cos \theta }{\theta }{a}^{\wedge } \\ {J}_l(\varphi {)}^{-1}=\frac{\theta }{2}\cot \frac{\theta }{2}I+\left(1-\frac{\theta }{2}\cot \frac{\theta }{2}\right)a{a}^T-\frac{\theta }{2}{a}^{\wedge } \end{gathered}$$
右雅各比矩阵也不复杂，只要对自变量取反即可
$$J_r(\varphi )=J_l(-\varphi )$$
举个例子，假如现有某个旋转 $R$ ，要对它左乘微小旋转 $\Delta R$ ，我们可以以两种形式表达

李群乘法，其本质是非线性运算：
$$\exp (\Delta {\varphi }^{\wedge })exp({\varphi }^{\wedge })=\exp ((\varphi +J_l^{-1}(\varphi )\Delta \varphi {)}^{\wedge })$$
或者李代数加法，其本质是线性运算（李代数加法通常用于极微小场景，左乘右乘差别不大）：
$$\exp ((\varphi +\Delta \varphi {)}^{\wedge })=\exp ((J_l\Delta \varphi {)}^{\wedge })\exp ({\varphi }^{\wedge })=\exp ({\varphi }^{\wedge })\exp ((J_r\Delta \varphi {)}^{\wedge })$$
类似的，对于 $SE(3)$ ，也有对应的 BCH 近似
$$\begin{gathered} {\mathcal{J}}_l(\xi )=\left[\begin{array}{cc}{J}_l^{SO(3)}& L\\ 0& J_l^{SO(3)}\end{array}\right] \\ L=\frac{\theta -\sin \theta }{{\theta }^3}{\varphi }^{\wedge }+\frac{1-\cos \theta }{{\theta }^2}{\varphi }^{\wedge }{\varphi }^{\wedge }+\frac{1}{2}I \end{gathered}$$

$\mathfrak{so}(3)求导$

1.BCH 求导

对于 $p^{\prime }=R_{new}p$ ，我们要求旋转后的点对旋转的导数，不过由于 $\mathrm{SO}(3)$ 没有加法，我们要求其转换到李代数空间来进行求导
$$\frac{\partial (Rp)}{\partial (R)}\Rightarrow \frac{\partial (exp({\varphi }^{\wedge })p)}{\partial \varphi }$$

这是比较严谨的方法，不过引入了雅各比矩阵之后，计算量实在太大，一般仅适用于较大扰动和高精度需求场景（实际上现在工程已经很少用了，基本都是扰动模型）

2.扰动模型求导

我们可以对当前变换施加一个小的扰动（通常是左乘，即在世界坐标系下添加扰动，右乘为在相机坐标系上添加扰动），使 $R_{new}=\exp (\delta {\phi }^{\wedge })R$，通过线性化直接求导
$$\frac{\partial (Rp)}{\partial (\varphi )}\Rightarrow \underset{\phi \to 0}{\lim }\frac{\exp ({\phi }^{\wedge })\exp ({\varphi }^{\wedge })p-\exp ({\varphi }^{\wedge })p}{\phi }$$

相较于复杂的 BCH 求导，计算量更小的扰动模型求导更常用

$\mathfrak{se}(3)$ 求导

对于 $p^{\prime }=Tp$ ，我们左乘一个小扰动 $\Delta T=\exp (\delta {\xi }^{\wedge })$ ，其李代数为 $\delta {\xi }^{\wedge }=[\delta \rho ,\delta \varphi {]}^T$

优化全流程

自此，李群和李代数的内容基本结束，接下来将以梯度下降为优化核心，简单介绍下优化的全流程

0.为什么会存在误差

理论上，我们给出的指令是：“先绕 X 轴顺时针转动 $60^{\circ }$ ，再沿着 Y 轴前进 1 m”。这是很明确的指令，并不存在误差，但实际中运动并不如此，机器不会严格按照这个顺序，而是一边旋转一边平移，而我们前面提到过：“$\mathfrak{se}(3)的旋转和平移是具有非交换性的$”，那么这里就出现了误差。这是主要的，其余误差则可能来自于传感器噪声、模型误差等

1.构建损失函数

损失函数一般基于重投影误差，假设有观测像素点 $u_i$ 和对应的 3D 世界点 $P_i([X_i,Y_i,Z_i,1{]}^T)$ ，位姿变换 $T(\xi )=\exp ({\xi }^{\wedge })$ ，投影函数 $\pi$（将 3D 点投影到图像平面），则损失函数可以如下定义
$$\mathrm{Loss}(\xi )=\sum _i||u_i-\pi (T(\xi )\cdot P_i)|{|}^2$$

2.计算梯度

梯度下降的核心就是求 $\frac{\partial \mathrm{Loss}(\xi )}{\partial \xi }$ ，这需要在李代数空间中求导。这里就只用扰动模型来做演示了

先对当前位姿 T 施加小扰动 $\delta \xi$ ，使 $T^{\prime }=\exp (\delta {\xi }^{\wedge })T(\xi )$ ，此时损失函数可近似为（如果不理解这里是怎么变换的，详细的推导我会放在最后）
$$\begin{gathered} \frac{\partial \mathrm{Loss}(\xi )}{\partial \xi }=\sum _i-2(u_i-\pi (T{p}_i){)}^T\frac{\partial \pi (T{p}_i)}{\partial (T{p}_i)}\cdot \frac{\partial (T{p}_i)}{\partial \xi } \\ \frac{\partial (T{p}_i)}{\partial \xi }=\left[\begin{array}{cc}I& -(Rp+t)\\ 0^T& 0^T\end{array}\right] \end{gathered}$$
其中 $p=[P_i^T,1{]}^T$ 是齐次坐标，R、t是当前位姿的旋转、平移部分。$\frac{\partial \pi }{\partial (T{p}_i)}$ 是投影函数的雅可比

3.更新位姿

• 计算更新方向：$\delta \xi =-\alpha \frac{\partial \mathbf{Loss}(\xi )}{\partial \xi }$ （$\alpha$ 是学习率）
• 更新位姿：${\xi }_{new}={\xi }_{old}+\delta \xi$

4.判断收敛和后处理

当损失变化小于阈值时，即可判断为梯度下降已经收敛，此时将李代数 $\xi$ 通过指数映射转换为李群元素 $T=\exp ({\xi }^{\wedge })$ ，得到最终的位姿矩阵

整个的优化过程可以简单通俗地理解为将完整的变换分成很多小份（多次迭代），然后每次计算一个增量加到当前位姿上，并根据新位姿与理想位姿的差距，优化下一个增量，使得实际位姿逐渐逼近理想位姿

附：损失函数偏导的推导