视觉 SLAM 十四讲 —— 第四讲李群和李代数

最新推荐文章于 2024-09-26 19:28:01 发布

GondorFu

最新推荐文章于 2024-09-26 19:28:01 发布

阅读量768

点赞数

分类专栏：三维重建

本文链接：https://blog.csdn.net/a40850273/article/details/109126420

版权

三维重建专栏收录该内容

10 篇文章

订阅专栏

视觉 SLAM 十四讲 —— 第四讲李群和李代数

在 SLAM 中的主要问题是基于当前的观测找到最符合观测的相机位姿。一种典型的方式是把它构建成一个优化问题，求解最优的旋转矩阵 $\textbf{R}$ 和平移向量 $\textbf{t}$ ，使量测误差最小。

但旋转矩阵本身具有一定约束（正交且行列式为 1），这使得直接求解旋转矩阵十分困难。不过通过李群和李代数的转换关系，可以将上述问题转换成无约束优化问题，从而简化求解。

李群和李代数基础

群（Group）是一种集合加上一种运算的代数结构。群要求这个运算满足（封闭性，结合律，幺元，逆）。

旋转矩阵 $SO(n) = \begin{Bmatrix} {\textbf{R}\in \mathbb{R}^{n\times n} |\textbf{R}\textbf{R}^T=\textbf{I}, det(\textbf{R}))=1} \end{Bmatrix}$ 和变换矩阵

$SE(3)=\begin{Bmatrix} {\textbf{T}=\begin{bmatrix} \textbf{R} & \textbf{t} \\ \textbf{0}^T & 1 \end{bmatrix}\in \mathbb{R}^{4\times 4} | \textbf{R} \in SO{3}, \textbf{t} \in \mathbb{R} ^ 3\end{Bmatrix}$ 对于对应的乘法操作就满足上述定义，因此它们都是一个群。

李群进一步规定其还具有连续（光滑）的性质。SO(3) 和 SE(3) 在实数空间上是连续的。我们可以直观的想象一个刚体能够连续地在空间中运动，所以它们都是李群。

而每个李群都有对应的李代数。李代数描述里李群的局部性质。李代数定义如下

旋转矩阵（李群）对应李代数推导

由于任意旋转矩阵都是单位正交矩阵，即酉矩阵 $\textbf{R}(t)\textbf{R}(t)^T=\textbf{I}$ ，进一步对求导

其中是一个反对称矩阵，而操作 $\cdot ^ \wedge$ 将一个向量转换为一个矩阵，具体对应数值参考 4.8 式。

当 t_0 =0 并且 $\textbf{R}(0)=\textbf{I}$ 时，

可以看出 $\phi(t)^\wedge$ 描述了 $\textbf{R}$ 的导数性质。同时，如果 $\phi(t)$ 在 t_0 附近保持不变，那么

由此建立了旋转矩阵与 $\phi$ 的一一对应关系。

根据前面的推导，每个 $\phi$ 都可以生成对应的反对称矩阵

根据定义，如此定义的李括号，保证了 $\phi$ 构成了一个李代数

而这对李群（旋转矩阵）和李代数（ $\phi$ ）满足指数对应关系。

同理变换矩阵对应的李代数如下

指数与对数映射

从上式 4.14 中，可以发现旋转矩阵的李群和李代数满足指数关系。而基于泰勒展开可以得到

其中将 $\phi=\theta a$ 表示成了模长乘单位向量的形式。可以看出上式与罗德里格式公式

十分相似。而由于罗德里格式公式描述的是旋转向量到旋转矩阵的变换关系，因此 so(3) 李代数就是由所谓的旋转向量组成的空间，而指数公式就是罗德里格斯公式。

同理，从旋转矩阵到对应李代数满足如下对数映射关系

其求解可以参考第三讲中旋转向量的求解进行。

针对变换矩阵不再做细节介绍，直接给出下图

李代数求导与扰动模型

BCH公式与近似公式

利用李代数的一大动机就是进行优化，而在优化过程中导数是非常必要的信息。因此需要弄清在李群上的乘法（旋转的叠加）操作在李代数上效果和李代数上的加法是否对应了李群上的乘积。

根据 BCH 公式

其中 [] 为李括号。从公式可以看出，两个矩阵的指数乘积会产生很多的李括号余项。不过，如果两个李代数中的某一个为小量，那么能够得到以下的近似结果

其中

由此可以得出以下结论

对于变换矩阵也有类似结论

李代数上求导

针对 SLAM 问题，我们经常会构建与位姿有关的函数，然后讨论该函数关于位姿的导数，以调整当前的估计值。然而 SO(3) 和 SE(3) 上并没有良好的定义的加法，它们只是群。如果只当作普通的矩阵进行求解，那么又必须添加很多约束。而从李代数的角度，由于李代数上定义了良好的加法运算，因此使用李代数解决求导问题存在两种思路：

1、用李代数表示位姿，然后根据李代数加法来对李代数求导。

2、对李群左乘或右乘微笑扰动，然后对该扰动求导，称之为左扰动和右扰动模型。

从上面两个公式中可以看出，第二种方法比第一种方法省去了复杂的雅可比矩阵的求解，计算更为简单，因此在实际应用中更为常用。

最后给出变换矩阵第二种方法的求导过程

相似变换群与李代数

之前已经介绍过单目的尺度不确定性。如果在单目 SLAM 中使用 SE(3) 表示位姿，那么由于尺度不确定性与尺度漂移，整个 SLAM 过程中的尺度会发生变化，这在 SE(3) 中未能体现出来。因此，在单目情况下我们一般会显式地把尺度因子表达出来。对于空间一点 $\textbf{p}$ ，在相机坐标系下的相似变换能够表示为