SLAM十四讲_后端1

后端

SLAM 系统的另一个重要模块：后端优化。
前端视觉里程计能给出一个短时间内的轨迹和地图，但由于不可避免的误 差累积，这个地图在长时间内是不准确的。所以，在视觉里程计的基础上，我们还希望构 建一个尺度、规模更大的优化问题，以考虑长时间内的最优轨迹和地图。不过，考虑到精 度与性能的平衡，实际当中存在着许多不同的做法。
视觉里程计只有短暂的记忆，而我们希望整个运动轨迹在较长时 间内都能保持最优的状态。。我们可能会用最新的知识，更新较久远之前的状态——站在“久 远的状态”的角度上看，仿佛是未来的信息告诉它“你应该在哪里”。。所以，在后端优化中，我 们通常考虑一个更长时间内（或所有时间内）的状态估计问题，而且不仅使用过去的信息更 新自己的状态，也会用未来的信息来更新自己，这种处理方式不妨称为“批量的”（Batch）。 否则，如果当前的状态只由过去的时刻决定，甚至只由前一个时刻决定，那不妨称为“渐 进的”（Incremental）。
我们已经知道 SLAM 过程可以由运动方程和观测方程来描述。那么，假设在 t = 0 到 t = N 的时间内，我们有 x0 到 xN 那么多个位姿，并且有 y1,…,yM 那么多个路标。按 照之前的写法，运动和观测方程为：

注意以下几点：

1. 观测方程中，只有当 xk 看到了 yj 时，才会产生观测数据，否则就没有。事实上，在 一个位置通常只能看到一小部分路标。而且，由于视觉 SLAM 特征点数量众多，所 以实际当中观测方程数量会远远大于运动方程的数量。
2. 我们可能没有测量运动的装置，所以也可能没有运动方程。。在这个情况下，有若干种 处理方式：认为确实没有运动方程，或假设相机不动，或假设相机匀速运动。这几种 方式都是可行的。在没有运动方程的情况下，整个优化问题就只由许多个观测方程组 成。这就非常类似于 SfM（Structure from Motion）问题，相当于我们通过一组图像 来恢复运动和结构。与 SfM 中不同的是，SLAM 中的图像有时间上的先后顺序，而 SfM 中允许使用完全无关的图像。

我们知道每个方程都受噪声影响，所以要把这里的位姿 x 和路标 y 看成服从某种概 率分布的随机变量，而不是单独的一个数。。因此，我们关心的问题就变成了：当我拥有某些 运动数据 u 和观测数据 z 时，如何来确定状态量 x,y 的分布？进而，如果得到了新来时 刻的数据之后，那么它们的分布又将发生怎样的变化？在比较常见且合理的情况下，我们 假设状态量和噪声项服从高斯分布——意味着在程序中，只需要储存它们的均值和协方差矩阵即可。均值可看作是对变量最优值的估计，而协方差矩阵则度量了它的不确定性。那 么，问题转变为：当存在一些运动数据和观测数据时，我们如何去估计状态量的高斯分布？

只有运动方程时，相当于我们蒙着眼睛在一个 未知的地方走路。尽管我们知道自己每一步走了多远，但是随着时间增长，我们将对自己 的位置越来越不确定——内心也就越加不安。这说明在输入数据受噪声影响时，我们对位 置方差的估计将越来越大。但是，当我们睁开眼睛时，由于能够不断地观测到外部场景，使 得位置估计的不确定性变小了，我们就会越来越自信。如果用椭圆或椭球直观地表达协方 差阵，那么这个过程有点像是在手机地图软件中走路的感觉。以图 10-1 为例，读者可以想 象，当没有观测数据时，这个圆会随着运动越来越大；而如果有正确观测的话，圆就会缩 小至一定的大小，保持稳定。

在第六讲中，我们介绍了最大似然估计，提到把状态估计转换为最小二乘的做法。。首先，由于位姿和路标点都是待估计的变量，我们改 变一下记号，令 xk 为 k 时刻的所有未知量。它包含了当前时刻的相机位姿与 m 个路标点。

同时，把 k 时刻的所有观测记作 zk.于是，运动方程与观测方程的形式可写得更加简洁。这里不会出现 y，但我们心里要明白这时 x 中已经包含了之前的 y 了：

现在考虑第 k 时刻的情况。我们希望用过去 0 到 k 中的数据，来估计现在的状态分 布：

下标 0 : k 表示从 0 时刻到 k 时刻的所有数据。请注意 zk 来表达所有在 k 时刻的观 测数据，注意它可能不止一个，只是这种记法更加方便。
下面我们来看如何对状态进行估计。按照 Bayes 法则，把 zk 与 xk 交换位置，有:

这里第一项称为似然，第二项称为先验。似然由观测方程给 定，而先验部分，我们要明白当前状态 xk 是基于过去所有的状态估计得来的。至少，它 会受 xk−1 影响，于是按照 xk−1 时刻为条件概率展开：

如果我们考虑更久之前的状态，也可以继续对此式进行展开，但现在我们只关心 k 时 刻和 k−1 时刻的情况。至此，我们给出了贝叶斯估计，，虽然上式还没有具体的概率分布 形式，所以我还没法实际地操作它。对这一步的后续处理，方法上产生了一些分歧。大体 来说，存在若干种选择：
其一是假设马尔可夫性，简单的一阶马氏性认为，k 时刻状态只 与 k−1 时刻状态有关，而与再之前的无关。如果做出这样的假设，我们就会得到以扩展卡 尔曼滤波（EKF）为代表的滤波器方法。在滤波方法中，我们会从某时刻的状态估计，推 导到下一个时刻。
另外一种方法是依然考虑 k 时刻状态与之前所有状态的关系，此时将得 到非线性优化为主体的优化框架。非线性优化的基本知识已经在前文介绍过了。目前视觉 SLAM 主流为非线性优化方法。

线性系统和KF

假设马尔可夫性，当前时刻状态只和上一个时刻有关，则：

这里，由于 k 时刻状态与 k−1 之前的无关，所以就简化成只与 xk−1 和 uk 有关的 形式，与 k 时刻的运动方程对应。第二部分可简化为：

这是考虑到 k 时刻的输入量 uk 与 k−1 时刻的状态无关，所以我们把 uk 拿掉。可 以看到，这一项实际是 k−1 时刻的状态分布。
可 以看到，这一项实际是 k−1 时刻的状态分布。于是，这一系列方程说明了，我们实际在 做的是“如何把 k−1 时刻的状态分布推导至 k 时刻”这样一件事。也就是说，在程序运行期间，我们只要维护一个状态量，对它进行不断地迭代和更新即可。进一步，如果假设 状态量服从高斯分布，那我们只需考虑维护状态量的均值和协方差即可。
我们从形式最简单的线性高斯系统开始，最后会得到卡尔曼滤波器。线性高斯系统是 说，运动方程和观测方程可以由线性方程来描述：

并假设所有的状态和噪声均满足高斯分布。记这里的噪声服从零均值高斯分布（噪声最优值为0）：

为了简洁我省略了 R 和 Q 的下标。。现在，利用马尔可夫性，假设我们知道了 k −1 时刻的后验（在 k−1 时刻看来）状态估计：：ˆ xk−1 和它的协方差 ˆ Pk−1，现在要根据 k 时 刻的输入和观测数据，确定 xk 的后验分布。为区分推导中的先验和后验，我们在记号上 作一点区别：以尖帽子 ˆ xk 表示后验，以横线 ¯ x 表示先验分布，请读者不要混淆。

卡尔曼滤波器的第一步，通过运动方程确定 xk 的先验分布。

这一步称为预测，它显示了如何从上一个时刻的状态，根据输入信息（但是有噪声），推断当前时刻的状态分布。这个分布也就是先验。记这里的：

另一方面，由观测方程，我们可以计算在某个状态下，应该产生怎样的 观测数据：

为了得到后验概率，我们想要计算它们的乘积，
然而，虽然我们知道最后会得到一个关于 xk 的高斯分布，但计算上是有一丁点儿麻 烦的，我们先把结果设为 xk ∼ N(ˆ xk, ˆ Pk)，那么：

这里我们稍微用点讨巧的方法。既然我们已经知道等式两侧都是高斯分布，那就只需 比较指数部分即可，而无须理会高斯分布前面的因子部分。指数部分很像是一个二次型的 配方，我们来推导一下。首先把指数部分展开，有：

为了求左侧的 ˆ xk 和 ˆ Pk，我们把两边展开，并比较 xk 的二次和一次系数。对于二次 系数，有：

该式给出了协方差的计算过程。为了便于后边列写式子，定义一个中间变量：

根据此定义，在式（10.16）左右各乘 ˆ Pk，有：


这里看似有一点儿循环定义的意思。我们由 ˆ Pk 定义了 K，再把 ˆ Pk 写成了 K 的表达式。然而，实际当 中 K 可以不依靠 ˆ Pk 算得。

然后再比较一次项的系数，有：

整理（取系数并转置）得：

于是我们又得到了后验均值的表达。总而言之，上面的两个步骤可以归纳为“预 测”（Predict）和“更新”（Update）两个步骤：

。事实上卡尔曼滤波器有若干种推 导方式，而我们使用的是从概率角度出发的最大后验概率估计的形式。。我们看到，在线性 高斯系统中，卡尔曼滤波器构成了该系统中的最大后验概率估计。而且，由于高斯分布经 过线性变换后仍服从高斯分布，所以整个过程中我们没有进行任何的近似。可以说，卡尔 曼滤波器构成了线性系统的最优无偏估计。

非线性系统和EKF

SLAM 中的运动方程和观测方程通常 是非线性函数，尤其是视觉 SLAM 中的相机模型，需要使用相机内参模型以及李代数表示 的位姿，更不可能是一个线性系统。一个高斯分布，经过非线性变换后，往往不再是高斯 分布，所以在非线性系统中，我们必须取一定的近似，将一个非高斯的分布近似成一个高 斯分布。
我们希望把卡尔曼滤波器的结果拓展到非线性系统中来，称为扩展卡尔曼滤波器（ExtendedKalmanFilter，EKF）。通常的做法是，在某个点附近考虑运动方程以及观测方程的 一阶泰勒展开，只保留一阶项，即线性的部分，然后按照线性系统进行推导。令 k−1 时刻的 均值与协方差矩阵为 ˆ xk−1, ˆ Pk−1。在 k 时刻，我们把运动方程和观测方程，在 ˆ xk−1, ˆ Pk−1 处进行线性化（相当于一阶泰勒展开），有：

记这里的偏导数为：

同样的，对于观测方程，亦有：

那么，在预测步骤中，根据运动方程有：

这些推导和卡尔曼滤波是十分相似的。为方便表述，记这里先验和协方差的均值为

然后，考虑在观测中，我们有：

最后，根据最开始的 Bayes 展开式，可以推导出 xk 的后验概率形式。我们略去中间 的推导过程，只介绍其结果。。简而言之，我们会先定义一个卡尔曼增益 Kk：

在卡尔曼增益的基础上，后验概率的形式为：

卡尔曼滤波器给出了在线性化之后，状态变量分布的变化过程。在线性系统和高斯噪 声下，卡尔曼滤波器给出了无偏最优估计。而在 SLAM 这种非线性的情况下，它给出了单 次线性近似下最大后验估计（MAP）。

EKF 有哪些局限呢？

1. 首先，滤波器方法在一定程度上假设了马尔可夫性，也就是 k 时刻的状态只与 k−1 时刻相关，而与 k −1 之前的状态和观测都无关（或者和前几个有限时间的状态相 关）。这有点像是在视觉里程计中，只考虑相邻两帧关系一样。如果当前帧确实与很 久之前的数据有关（例如回环），那么滤波器就会难以处理这种情况。
   而非线性优化方法则倾向于使用所有的历史数据。它不光考虑邻近时刻的特征点与 轨迹关系，更会把考虑很久之前的状态也考虑进来，称为全体时间上的 SLAM（FullSLAM）。在这种意义下，非线性优化方法使用了更多信息，当然也需要更多的计算。

2. EKF 滤波器仅在 ˆ xk−1 处做了一次线性化，然后就 直接根据这次线性化结果，把后验概率给算了出来。这相当于在说，我们认为该点处的线性化近似，在后验概率处仍然是有效的。。而实际上，当我们离开工作点较远的时 候，一阶泰勒展开并不一定能够近似整个函数，这取决于运动模型和观测模型的非线 性情况。如果它们有强烈的非线性，那线性近似就只在很小范围内成立，不能认为在 很远的地方仍能用线性来近似。这就是 EKF 的非线性误差，是它的主要问题所在。
   在优化问题中，尽管我们也做一阶（最速下降）或二阶（G-N 或 L-M）的近似，但 每迭代一次，状态估计发生改变之后，我们会重新对新的估计点做泰勒展开，而不像 EKF 那样只在固定点上做一次泰勒展开。这就导致优化方法适用范围更广，则在状 态变化较大时亦能适用。

3. 从程序实现上来说，EKF 需要存储状态量的均值和方差，并对它们进行维护和更新。 如果把路标也放进状态的话，由于视觉 SLAM 中路标数量很大，这个存储量是相当 可观的，且与状态量呈平方增长（因为要存储协方差矩阵）。因此，EKF SLAM 普遍 被认为不可适用于大型场景。

由于 EKF 存在这些明显的缺点，我们通常认为，在同等计算量的情况下，非线性优化能取得更好的效果 。下面我们来讨论以非线性优化为主的后端。我们将主要介绍图 优化。

BA与图优化

视觉三维重建

所谓的 Bundle Adjustment¬，是指从视觉重建中提炼出最优的 3D 模型和相机参数（内参数和外参数）。 从每一个特征点反射出来的几束光线（bundles of light rays），在我们把相机姿态和特征点 空间位置做出最优的调整 (adjustment) 之后，最后收束到相机光心的这个过程 [26]，简称 为 BA。

在以图优化框架的视觉 SLAM 算法里，BA 起到了核心作用。它类似于求解只有观测 方程的 SLAM 问题。。在最近几年视觉 SLAM 理论的研究中，BA 算法不仅具有很高的精 度，也开始具备良好的实时性，能够应用于在线计算的 SLAM 场景中。掌握好 BundleAdjustment，深入研究其中的理论和实践细节，是做好视觉 SLAM 的关键。

投影模型和BA代价函数

让我们复习一下整个投影的过程。从一 个世界坐标系中的点 p 出发，把相机的内外参数和畸变都考虑进来，最后投影成像素坐标， 一共需要如下几个步骤：

1. 首先，把世界坐标转换到相机坐标，这里将用到相机外参数 (R,t)：
   
2. 然后，将 P′ 投至归一化平面，得到归一化坐标：
   
3. 对归一化坐标去畸变，得到去畸变后的坐标。这里暂时只考虑径向畸变：
   
4. 最后，根据内参模型，计算像素坐标：
   
   读者应该能领会到，这个过程也就是前面讲的观测方程，之前我们把它抽象地记成：
   
   现在，我们给出了它的详细参数化过程。具体地说，这里的 x 指代此时相机的位姿， 即外参 R,t，它对应的李代数为 ξ。路标 y 即这里的三维点 p，而观加粗样式测数据则是像素坐标 z ∆ = [us,vs]T。以最小二乘的角度来考虑，那么可以列写关于此次观测的误差：
   
   然后，把其他时刻的观测量也考虑进来，我们可以给误差添加一个下标。。设 zij 为在 位姿 ξi 处观察路标 pj 产生的数据，那么整体的**代价函数（Cost Function）**为：
   
   对这个最小二乘进行求解，相当于对位姿和路标同时作了调整，也就是所谓的 BA。

BA的求解

观察在上一节中的观测模型 h(ξ,p)，很容易判断该函数不是线性函数。所以我们希望 使用一些非线性优化手段来优化它。
根据非线性优化的思想，我们应该从某 个的初始值开始，不断地寻找下降方向 ∆x 来找到目标函数的最优解，即不断地求解增量 方程中的增量 ∆x。

尽管误差项都是针对单个位姿和路标点的，但在整体 BA 目 标函数上，我们必须把自变量定义成所有待优化的变量：

相应的，增量方程中的 ∆x 则是对整体自变量的增量。在这个意义下，当我们给自变 量一个增量时，目标函数变为：

其中 Fij 表示整个儿代价函数在当前状态下对相机姿态的偏导数，而 Eij 表示该函数 对路标点位置的偏导。（雅可比矩阵吗？？？–是）
现在，把相机位姿变量放在一起：

并把空间点的变量也放在一起：

简化表达为如下：

需要注意的是，该式从一个由很多个小型二次项之和，变成了一个更整体的样子。这 里的雅可比矩阵 E 和 F 必须是整体目标函数对整体变量的导数，它将是一个很大块的矩 阵，而里头每个小分块，需要由每个误差项的导数 Fij 和 Eij“拼凑”起来。然后，无论 我们使用 G-N 还是 L-M 方法，最后都将面对增量线性方程：

我们知道 G-N 和 L-M 的主要差别在于，这里的 H 是取 JTJ 还 是 JTJ + λI 的形式。由于我们把变量归类成了位姿和空间点两种，所以雅可比矩阵可以 分块为：

那么，以 G-N 为例，则 H 矩阵为：

当然在 L-M 中我们也需要计算这个矩阵。不难发现，因为考虑了所有的优化变量，这 个线性方程的维度将非常大，包含了所有的相机位姿和路标点。尤其是在视觉 SLAM 中， 一个图像就会提出数百个特征点，大大增加了这个线性方程的规模。如果直接对 H 求逆 来计算增量方程，由于矩阵求逆是复杂度为 O(n3) 的操作 [71]，这是非常消耗计算资源的。 幸运地是，这里的 H 矩阵是有一定的特殊结构的。利用这个特殊结构，我们可以加速求 解过程。

稀疏性和边缘化

21 世纪视觉 SLAM 的一个重要进展是认识到了矩阵 H 的稀疏结构，并发现该结构 可以自然、显式地用图优化来表示 。本节将详细讨论一下该矩阵稀疏结构。
H 矩阵的稀疏性是由雅可比 J(x) 引起的。考虑这些代价函数当中的其中一个 eij。注 意到，这个误差项只描述了在 ξi 看到 pj 这件事，只涉及到第 i 个相机位姿和第 j 个路标 点，对其余部分的变量的导数都为 0。所以该误差项对应的雅可比矩阵有下面的形式：

其中 02×6 表示维度为 2×6 的 0 矩阵，同理 02×3 也是一样。该误差项对相机姿态的 偏导 ∂eij/∂ξi 的维度为 2×6，对路标点的偏导 ∂eij/∂pj 维度是 2×3。这个误差项的雅 可比矩阵，除了这两处为非零块之外，其余地方都为零。这体现了该误差项与其他路标和 轨迹无关的特性。那么，它对增量方程有何影响呢？H 矩阵为什么会产生稀疏性呢？

以图 10-3 为例，我们设 Jij 只在 i,j 处有非零块，那么它对 H 的贡献为 JT ijJij，具
有示意图上所画的稀疏形式。这个 JT ijJij 矩阵也仅有四个非零块，位于 (i,i)， (i,j)， (j,i)， (j,j)。对于整体的 H，由于：

请注意 i 在所有相机位姿中取值，j 在所有路标点中取值。我们把 H 进行分块：

这里 H11 只和相机位姿有关，而 H22 只和路标点有关。当我们遍历 i,j 时，以下事 实总是成立的：

1. 不管 i,j 怎么变，H11 都是对角阵，只在 Hi,i 处有非零块；
2. 同理，H22 也是对角阵，只在 Hj,j 处有非零块；
3. 对于 H12 和 H21，它们可能是稀疏的，也可能是稠密的，视具体的观测数据而定。
   这显示了 H 的稀疏结构。之后对线性方程的求解中，也正需要利用它的稀疏结构。
   。假设一个场 景内有 2 个相机位姿 (C1,C2) 和 6 个路标 (P1,P2,P3,P4,P5,P6)。这些相机和点云所对 应的变量为 ξi,i = 1,2 以及 pj,j = 1,…,6。相机 C1 观测到路标 P1,P2,P3,P4，相机 C2 观测到了路标 P3,P4,P5,P6。我们把这个过程画成示意图 10-4 。相机和路标以圆形节点表 示。如果 i 相机能够观测到 j 点云，我们就在它们对应的节点连上一条边。
   
   可以推出该场景下的 BA 目标函数应该是：
   
   以 e11 为例，它描述了在 C1 看到了 P1 这件事，与其他的相机位姿和路标无关。令 J11 为 e11 所对应的雅可比矩阵，不难看出 e11 对相机变量 ξ2 和路标点 p2,…,p6 的偏导都为 0。我们把所有变量以 x = (ξ1,ξ2,p1,…,p2)T 的顺序摆放，则有：
   
   为了方便表示稀疏性，我们用带有颜色的方块表示矩阵在该方块内有数值，其余没有 颜色的区域表示矩阵在该处数值都为 0。那么上面的 J11 则可以表示成图 10-5 的图案。同 理，其他的雅可比矩阵也会有类似的稀疏图案。
   
   为了得到该目标函数对应的雅可比矩阵，我们可以将这些 Jij 按照一定顺序列为向量， 那么整体雅可比矩阵以及相应的 H 矩阵的稀疏情况就是图 10-6 中所展示的那样。
   
   也许你已经注意到了，图 10-4 对应的邻接矩阵（Adjacency Matrix）和上图中的 H 矩阵，除了对角元素以外的其余部分有着完全一致的结构。事实上的确如此。（所谓邻接矩阵是这样一种矩阵，它的第 i,j 个元素描述了节点 i 和 j 是否存在一条边。如果存在此边，设 这个元素为 1，否则设为 0。）上面的 H 矩阵一共有 8×8 个矩阵块，对于 H 矩阵当中处于非对角线的矩阵块来说，如果该矩阵块 非零，则其位置所对应的变量之间会在图中存在一条边，我们可以从图 10-7 中清晰地看到 这一点。所以，H 矩阵当中的非对角部分的非零矩阵块可以理解为它对应的两个变量之间 存在联系，或者可以称之为约束。于是，我们发现图优化结构与增量方程的稀疏性存在着 明显的联系。
   
   现在考虑更一般的情况，假如我们有 m 个相机位姿，n 个路标点。由于通常路标数量 远远会比相机多，于是有 n ≫ m。。由上面推理可知，实际当中的 H 矩阵会像图 10-8 所 示的那样。它的左上角块显得非常小，而右下角的对角块占据了大量地方。。除此之外，非 对角部分则分布着散乱的观测数据。由于它的形状很像箭头，又称为箭头形（Arrow-like） 矩阵 [6]。同时它又很像一把镐子，所以也叫镐形矩阵。
   
   对于具有这种稀疏结构的 H，线性方程 H∆x = g 的求解会有什么不同呢？现实当 中存在着若干种利用 H 的稀疏性加速计算的方法，而本节介绍视觉 SLAM 里一种最常用 的手段：Schur 消元 (Schur trick)。在 SLAM 研究中亦称为 Marginalization（边缘化）。
   仔细观察一下图 10-8 ，我们不难发现这个矩阵可以分成 4 个块，和式（10.53）一致。 左上角为对角块矩阵，每个对角块元素的维度与相机位姿的维度相等，且是一个对角块矩 阵。右下角也是对角块矩阵，每个对角块的维度是路标的维度。非对角块的结构与具体观 测数据相关。我们首先将这个矩阵按照图10-9中所示的方式来划分区域，读者不难发现，这 四个区域正是对应了公式 (10.50) 中的四个矩阵块。为了后续分析地方便，我们称这四个 块为 B，E，C。
   
   于是，对应的线性方程组也可以由 H∆x = g 变为如下形式：
   
   其中 B 是对角块矩阵，每个对角块的维度和相机参数的维度相同，对角块的个数是 相机变量的个数。由于路标数量会远远大于相机变量个数，所以 C 往往也远大于 B。三 维空间中每个路标点为三维，于是 C 矩阵为对角块矩阵，每个块为 3×3 维矩阵。对角块矩阵逆的难度远小于对一般矩阵的求逆难度，因为我们只需要对那些对角线矩阵块分别求 逆即可。考虑到这个特性，我们线性方程组进行高斯消元，目标是消去右上角的非对角部 分 E，得：
   
   经过消元之后，第一行方程组变成和 ∆xp 无关的项。单独把它拿出来，得到关于位姿 部分的增量方程：
   
   这个线性方程组的维度和 B 矩阵一样。我们的做法是先求解这个方程，然后把解得 的 ∆xc 代入到原方程，然后求解 ∆xp。这个过程称为 Marginalization[68]，或者 Schur 消元 (Schur Elimination)。相比于直接解线性方程的做法，它的优势在于：
4. 在消元过程中，由于 C 为对角块，所以 C−1 容易解得
5. 求解了 ∆xc 之后，路标部分的增量方程由 ∆xp = C−1(w−ET∆xc) 给出。这依然 用到了 C−1 易于求解的特性。
   图（10-10）显示了进行 Schur 消元之后的一个 S 实例，可以看到 它的稀疏性是不规则的。 前面说到，H 矩阵的非对角块处的非零元素对应着相机和路标的关联。那么，进行了 Schur 消元后 S 的稀疏性是否具有物理意义呢？答案是有的。此处我们不加以证明地说， S 矩阵的非对角线上的非零矩阵块，表示了该处对应的两个相机变量之间存在着共同观测 的路标点，有时候称为共视（Co-visibility）。反之，如果该块为零，则表示这两个相机没 有共同观测。例如图 10-10 所示的稀疏矩阵，左上角前 4×4 个矩阵块可以表示对应的相 机变量 C1,C2,C3,C4 之间有共同观测。
   
   
   于是，S 矩阵的稀疏性结构当取决于实际观测的结果，我们无法提前预知。在实践当 中，例如 ORB_SLAM[73] 中的 Local Mapping 环节，在做 BA 的时候刻意选择那些具有 共同观测的帧作为关键帧，在这种情况下 Schur 消元后得到的 S 就是稠密矩阵。。不过，由 于这个模块并不是实时执行，所以这种做法也是可以接受的。但是在另一些方法里面，例 如 DSO[58]，OKVIS[74] 等，它们采用了滑动窗口方法（Sliding Window）。这类方法对每 一帧都要求做一次 BA 来防止误差的累积，因此它们也必须采用一些技巧来保持 S 矩阵 的稀疏性。
   从概率角度来看，我们称这一步为边缘化，是因为我们实际上把求 (∆xc,∆xp) 的问 题，转化成先求 ∆xc，再求 ∆xp 的过程。这一步相当于做了条件概率展开：
   
   结果是求出了关于 xc 的边缘分布，故称边缘化。在上边讲的边缘化过程中，我们实 际把所有的路标点都给边缘化了。根据实际情况，我们也能选择一部分进行边缘化。同时， Schur 消元只是实现边缘化的其中一种方式，同样可以使用 Cholesky 分解进行边缘化。
   鲁棒核函数
   在前面的 BA 问题中，我们最小化误差项的二范数平方和，作为目标函数。这种做法 虽然很直观，但存在一个严重的问题：如果出于误匹配等原因，某个误差项给的数据是错误 的，会发生什么呢？我们把一条原本不应该加到图中的边给加进去了，然而优化算法并不 能辨别出这是个错误数据，它会把所有的数据都当作误差来处理。这时，算法会看到一条 误差很大的边，它的梯度也很大，意味着调整与它相关的变量会使目标函数下降更多。所 以，算法将试图调整这条边所连接的节点的估计值，使它们顺应这条边的无理要求。由于 这个边的误差真的很大，往往会抹平了其他正确边的影响，使优化算法专注于调整一个错 误的值。这显然不是我们希望看到的。
   出现这种问题的原因是，当误差很大时，二范数增长得太快了。于是就有了核函数的 存在。**核函数保证每条边的误差不会大的没边，掩盖掉其他的边。**具体的方式是，**把原先误差的二范数度量，替换成一个增长没有那么快的函数，同时保证自己的光滑性质（不然没 法求导啊！）。**因为它们使得整个优化结果更为鲁棒，所以又叫它们为鲁棒核函数（Robust Kernel）。

鲁棒核函数有许多种，例如最常用的 Huber 核：

我们看到，当误差 e 大于某个阈值 δ 后，函数增长由二次形式变成了一次形式，相当 于限制了梯度的最大值。同时，Huber 核函数又是光滑的，可以很方便地求导。

本节我们重点介绍了 BA 中的稀疏性问题。不过，实践当中，有许多软件库为我们提 供了细节操作，而我们需要做的主要是构造 Bundle Adjustment 问题，设置 Schur 消元， 然后调用稠密或者稀疏矩阵求解器对变量进行优化即可。。图 10-12 显 示了 Huber 核函数与二次函数的对比，可见在误差较大时 Huber 核函数增长明显低于二 次函数。

除了 Huber 核之外，还有 Cauchy 核，Tukey 核等等，读者可以看看 g2o 和 Ceres 都 提供了哪些核函数。

全部摘自 slam十四讲