视觉SLAM第四讲

因为在 SLAM 中位姿是未知的，而我们需要解决形如“什么样的相机位姿最符合当前观测数据”这样的问题。一种典型的方式是把它构建成一个优化问题，求解最优的$\mathrm{R}、\mathrm{t}$，使得误差最小化。如前所言，旋转矩阵自身是带有约束的（正交且行列式为 1）。它们作为优化变量时，会引入额外的约束，使优化变得困难。通过李群—李代数间的转换关系，我们希望把位姿估计变成无约束的优化问题，简化求解方式。

李群、李代数

李群

旋转矩阵也好，变换矩阵也好，它们对加法是不封闭的，SO(3)和SE(3)关于乘法是封闭的，对于这种只有一个（良好的）运算的集合，我们称之为群。群（Group）是一种集合加上一种运算的代数结构。我们把集合记作 A，运算记作 ·，那么群可以记作G = (A, ·)。群要求这个运算满足以下几个条件：

三维旋转矩阵构成了特殊正交群SO(3)，而变换矩阵构成了特殊欧氏群SE(3)。

李群是指具有连续（光滑）性质的群。像整数群Z那样离散的群没有连续性质，所以不是李群。而SO(n)和SE(n)在实数空间上是连续的。

李代数

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

李代数由一个集合 V，一个数域 F 和一个二元运算 [, ] 组成。如果它们满足以下几条性质，则称(V, F, [, ]) 为一个李代数，记作 g。

其中二元运算被称为李括号。从表面上来看，李代数所需要的性质还是挺多的。相比于群中的较为简单的二元运算，李括号表达了两个元素的差异。它不要求结合律，而要求元素和自己做李括号之后为零的性质。作为例子，三维向量${\mathrm{R}}^3$上定义的叉积$\times$是一种李括号，因此$\mathrm{g}=({\mathrm{R}}^3,\mathrm{R},\times )$构成了一个李代数。读者可以尝试将叉积的性质代入到上面四条性质中。

李群李代数间的指数、对数映射

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

推导：

$\mathbf{R}=\cos \theta \mathbf{I}+(1-\cos \theta )\mathbf{n}{\mathbf{n}}^{\mathrm{T}}+\sin \theta {\mathbf{n}}^{\wedge }$

概述图：

BCH

BCH 公式

其中 [] 为李括号。BCH 公式告诉我们，当处理两个矩阵指数之积时，它们会产生一些由李括号组成的余项。特别地，考虑 SO(3) 上的李代数$\ln {\left(\exp \left({\varphi }_1^{\wedge }\right)\exp \left({\varphi }_2^{\wedge }\right)\right)}^{\vee }$，当 ${\varphi }_1$ 或 ${\varphi }_2$ 为小量时，小量二次以上的项都可以被忽略掉。此时，BCH 拥有线性近似表达：

BCH 近似的意义

假定对某个旋转$\mathbf{R}$，对应的李代数为$\varphi$。我们给它左乘一个微小旋转，记作$\Delta \mathbf{R}$，对应的李代数为$\Delta \varphi$。那么，在李群上，得到的结果就是$\Delta \mathbf{R}\cdot \mathbf{R}$，而在李代数上，根据 BCH 近似，为$J_l^{-1}(\varphi )\Delta \varphi +\varphi$。合并起来，可以简单地写成：

反之，如果我们在李代数上进行加法，让一个$\varphi$加上$\Delta \varphi$，那么可以近似为李群上带左右雅可比的乘法：

SO(3)李代数上的求导

下面来讨论一个带有李代数的函数，如何关于该李代数求导的问题。该问题有很强的实际背景。在 SLAM 中，我们要估计一个相机的位置和姿态，该位姿是由 SO(3) 上的旋转矩阵或 SE(3) 上的变换矩阵描述的。不妨设某个时刻小萝卜的位姿为$T$。它观察到了一个世界坐标位于$p$的点，产生了一个观测数据$z$。那么，由坐标变换关系知：

$z=Tp+w$

其中$w$为随机噪声。由于它的存在，$z$往往不可能精确地满足$z=Tp$的关系。所以，我们通常会计算理想的观测与实际数据的误差：

$e=z-Tp$

假设一共有 N 个这样的路标点和观测，于是就有 N 个上式。那么，对小萝卜的位姿估计，相当于是寻找一个最优的$T$，使得整体误差最小化：

${\min }_{\mathbf{T}}J(\mathbf{T})={\sum }_{i=1}^N{\Vert {\mathbf{z}}_i-\mathbf{T}{\mathbf{p}}_i\Vert }_2^2$

求解此问题，需要计算目标函数$J$关于变换矩阵$T$的导数。我们把具体的算法留到后面再讲。这里的重点是，我们经常会构建与位姿有关的函数，然后讨论该函数关于位姿的导数，以调整当前的估计值。然而，SO(3), SE(3) 上并没有良好定义的加法，它们只是群。如果我们把$T$当成一个普通矩阵来处理优化，那就必须对它加以约束。而从李代数角度来说，由于李代数由向量组成，具有良好的加法运算。因此，使用李代数解决求导问题的思路分为两种：

1. 用李代数表示姿态，然后根据李代数加法来对李代数求导。
2. 对李群左乘或右乘微小扰动，然后对该扰动求导，称为左扰动和右扰动模型。第一种方式对应到李代数的求导模型，而第二种则对应到扰动模型。下面来讨论这两种思路的异同。

李代数加法求导

首先，考虑 SO(3) 上的情况。假设我们对一个空间点 $p$ 进行了旋转，得到了$Rp$。现在，要计算旋转之后点的坐标相对于旋转的导数，我们非正式地记为：

$\frac{\partial \mathbf{Rp}}{\partial \mathbf{R}}$

由于 SO(3) 没有加法，所以该导数无法按照导数的定义进行计算。设$R$对应的李代数为 $\varphi$，我们转而计算：

$\frac{\partial \exp ({\mathbf{\varphi }}^{\wedge })\mathbf{p}}{\partial \mathbf{\varphi }}$

按照导数的定义，有：

第 2 行的近似为 BCH 线性近似，第 3 行为泰勒展开舍去高阶项后的近似（但由于取了极限，可以写等号），第 4 行至第 5 行将反对称符号看作叉积，交换之后变号。

不过，由于这里仍然含有形式比较复杂的$J_l$，我们不太希望计算它。而下面要讲的扰动模型则提供了更简单的导数计算方式。

李代数的扰动模型

扰动模型（左乘）

另一种求导方式是对 $R$ 进行一次扰动 $\Delta R$，看结果相对于扰动的变化率。这个扰动可以乘在左边也可以乘在右边，最后结果会有一点儿微小的差异，我们以左扰动为例。设左扰动 $\Delta R$ 对应的李代数为 $\phi$。然后，对 $\phi$ 求导，即：

该式的求导比上面更为简单：

相比于直接对李代数求导，省去了一个雅可比$J_l$的计算。这使得扰动模型更为实用。

习题

p5.证明：

$\mathrm{R}{\mathrm{p}}^{\wedge }{\mathrm{R}}^{\mathrm{T}}=(\mathrm{Rp}{)}^{\wedge }$

证明：$\Rightarrow (\mathrm{Rp}{)}^{\wedge }\mathrm{n}=(\mathrm{Rp})\times \left({\mathrm{RR}}^{-1}\mathrm{n}\right)=\mathrm{R}\left[\mathrm{p}\times \left({\mathrm{R}}^{-1}\mathrm{n}\right)\right]={\mathrm{Rp}}^{\wedge }{\mathrm{R}}^{\top }\mathrm{n}$

$\Rightarrow \mathrm{R}{\mathrm{p}}^{\wedge }{\mathrm{R}}^{\mathrm{T}}=(\mathrm{Rp}{)}^{\wedge }$

第二个等号是因为叉乘的旋转不变性：向量间的叉乘后旋转等于向量分别旋转后再叉乘。

p6.证明：

$\mathrm{R}\exp ({\mathrm{p}}^{\wedge }){\mathrm{R}}^{\mathrm{T}}=\exp ((\mathrm{Rp}{)}^{\wedge })$

该式称为SO(3) 上的伴随性质。

证明：$\mathrm{Rexp}\left({\mathrm{p}}^{\wedge }\right){\mathrm{R}}^{\mathrm{T}}=\mathrm{R}{\sum }_{\mathrm{n}=0}^{\infty }\frac{{\mathrm{p}}^{\wedge \mathrm{n}}}{\mathrm{n}!}{\mathrm{R}}^{\mathrm{T}}={\sum }_{\mathrm{n}=0}^{\infty }\frac{{\left(\mathrm{R}{\mathrm{p}}^{\wedge }{\mathrm{R}}^{\mathrm{T}}\right)}^{\mathrm{n}}}{\mathrm{n}!}=\exp \left(\mathrm{R}{\mathrm{p}}^{\wedge }{\mathrm{R}}^{\mathrm{T}}\right)=\exp \left((\mathrm{Rp}{)}^{\wedge }\right)$

第二个等号由旋转矩阵的正交性易得。