视觉SLAM笔记（52） BA 与图优化

---

1. Bundle Adjustment

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

在以图优化框架的视觉 SLAM 算法里， BA 起到了核心作用
它类似于求解只有观测方程的 SLAM 问题

在最近几年视觉 SLAM 理论的研究中， BA 算法不仅具有很高的精度，也开始具备良好的实时性，能够应用于在线计算的 SLAM 场景中
而在 21 世纪早期，虽然计算机视觉领域的研究者已经开始利用 BA 进行重构
但 SLAM 的研究者通常认为包含大量特征点和相机位姿的 BA 计算量过大，不适合实时计算
直到近十年来，人们逐渐认识到 SLAM 问题中 BA 的稀疏特性，才使它能够在实时的场景中使用
因此，掌握好 Bundle Adjustment，深入研究其中的理论和实践细节，是做好视觉 SLAM 的关键

---

2. 投影模型和 BA 代价函数

在 视觉SLAM笔记（20） 单目相机模型 里，介绍了投影模型和畸变
从一个世界坐标系中的点 p 出发，把相机的内外参数和畸变都考虑进来，最后投影成像素坐标，
一共需要如下几个步骤:

这个过程也就是观测方程，之前把它抽象地记成：

现在，给出了它的详细参数化过程
具体地说，这里的 x 指代此时相机的位姿，即外参 R; t，它对应的李代数为 ξ
路标 y 即这里的三维点 p，而观测数据则是像素坐标

以最小二乘的角度来考虑，那么可以列写关于此次观测的误差：

然后，把其他时刻的观测量也考虑进来，可以给误差添加一个下标
设 z_ij 为在位姿 ξ_i 处观察路标 pj 产生的数据，那么整体的代价函数（Cost Function）为：

对这个最小二乘进行求解，相当于对位姿和路标同时作了调整，也就是所谓的 BA

---

3. BA 的求解

观测模型 h(ξ, p)，很容易判断该函数不是线性函数
所以希望使用一些非线性优化手段来优化它

根据非线性优化的思想，应该从某个的初始值开始，不断地寻找下降方向 ∆x 来找到目标函数的最优解
即不断地求解增量方程中的增量 ∆x
尽管误差项都是针对单个位姿和路标点的
但在整体 BA 目标函数上，必须把自变量定义成所有待优化的变量：

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

其中 F_ij 表示整个儿代价函数在当前状态下对相机姿态的偏导数
而 E_ij 表示该函数对路标点位置的偏导
现在，把相机位姿变量放在一起：

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

那么，式1可以简化表达为如下

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

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

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

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

---

4. 稀疏性和边缘化

SLAM 的一个重要进展是认识到了矩阵 H 的稀疏结构
并发现该结构可以自然、显式地用图优化来表示

H 矩阵的稀疏性是由雅可比 J(x) 引起的
考虑这些代价函数当中的其中一个 e_ij
这个误差项只描述了在 ξ_i看到 p_j 这件事，只涉及到第 i 个相机位姿和第 j 个路标点，对其余部分的变量的导数都为 0
所以该误差项对应的雅可比矩阵有下面的形式：

其中 0_2×6 表示维度为 2 × 6 的 0 矩阵
该误差项对相机姿态的偏导 $\partial$e_ij/ $\partial$ξ_i 的维度为 2 × 6，对路标点的偏导 $\partial$e_ij/ $\partial$p_j 维度是 2 × 3
这个误差项的雅可比矩阵，除了这两处为非零块之外，其余地方都为零
这体现了该误差项与其他路标和轨迹无关的特性

那么，它对增量方程有何影响呢？ H 矩阵为什么会产生稀疏性呢？

设 J_ij 只在 i，j 处有非零块，那么它对 H 的贡献为 J_ij^TJ_ij，具有示意图上所画的稀疏形式
这个 J_ij^TJ_ij 矩阵也仅有四个非零块，位于 (i, i)， (i, j)， (j, i)，(j, j)
对于整体的 H，由于：

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

这里 H₁₁ 只和相机位姿有关，而 H₂₂ 只和路标点有关
当遍历 i，j 时，以下事实总是成立的：

1. 不管 i，j 怎么变， H₁₁ 都是对角阵，只在 H_i,i 处有非零块
2. 同理， H₂₂ 也是对角阵，只在 H_j,j 处有非零块
3. 对于 H₁₂ 和 H₂₁，它们可能是稀疏的，也可能是稠密的，视具体的观测数据而定

这显示了 H 的稀疏结构
之后对线性方程的求解中，也正需要利用它的稀疏结构

假设一个场景内有 2 个相机位姿 (C₁, C₂) 和 6 个路标 (P₁, P₂, P₃, P₄, P₅, P₆)
这些相机和点云所对应的变量为 ξ_i,i = 1, 2 以及 p_i,i = 1,……,6
相机 C1 观测到路标 P₁, P₂, P₃, P₄
相机 C2观测到了路标 P₃, P₄, P₅, P₆

把这个过程画成示意图，相机和路标以圆形节点表示

如果 i 相机能够观测到 j 点云，就在它们对应的节点连上一条边

可以推出该场景下的 BA 目标函数应该是：

这里的 e_ij 使用之前定义过的代价函数
以 e₁₁ 为例，它描述了在C₁ 看到了 P₁ 这件事，与其他的相机位姿和路标无关
令 J₁₁ 为 e₁₁ 所对应的雅可比矩阵
不难看出 e₁₁ 对相机变量 ξ₂ 和路标点 p₂,……, p₆ 的偏导都为 0
把所有变量以 x = (ξ₁, ξ₂, p₁, ……, p₂,)^T 的顺序摆放，则有：

为了方便表示稀疏性，用带有颜色的方块表示矩阵在该方块内有数值，其余没有颜色的区域表示矩阵在该处数值都为 0
那么上面的 J₁₁ 则可以表示成下图的图案

同理，其他的雅可比矩阵也会有类似的稀疏图案

为了得到该目标函数对应的雅可比矩阵，可以将这些 J_ij 按照一定顺序列为向量
那么整体雅可比矩阵以及相应的 H 矩阵的稀疏情况就是下图中所展示

也许已经注意到了，对应的邻接矩阵（Adjacency Matrix） 和上图中的 H矩阵，除了对角元素以外的其余部分有着完全一致的结构
上面的 H 矩阵一共有 8 × 8 个矩阵块，对于 H 矩阵当中处于非对角线的矩阵块来说
如果该矩阵块非零，则其位置所对应的变量之间会在图中存在一条边
可以从下图中清晰地看到这一点

所以， H 矩阵当中的非对角部分的非零矩阵块可以理解为它对应的两个变量之间存在联系
或者可以称之为约束
于是，发现图优化结构与增量方程的稀疏性存在着明显的联系

现在考虑更一般的情况，假如有 m 个相机位姿， n 个路标点
由于通常路标数量远远会比相机多，于是有 n ≫ m
由上面推理可知，实际当中的 H 矩阵会像下图所示

它的左上角块显得非常小，而右下角的对角块占据了大量地方
除此之外，非对角部分则分布着散乱的观测数据
由于它的形状很像箭头，又称为箭头形（Arrow-like）矩阵
同时它又很像一把镐子，所以也叫镐形矩阵

对于具有这种稀疏结构的 H，线性方程 $H∆x = g$ 的求解会有什么不同呢？
现实当中存在着若干种利用 H 的稀疏性加速计算的方法
而这里介绍视觉 SLAM 里一种最常用的手段： Schur 消元 (Schur trick)

在 SLAM 研究中亦称为 Marginalization（边缘化）
仔细观察一下上图 ，不难发现这个矩阵可以分成 4 个块，和之前的式子一致

左上角为对角块矩阵，每个对角块元素的维度与相机位姿的维度相等，且是一个对角块矩阵
右下角也是对角块矩阵，每个对角块的维度是路标的维度
非对角块的结构与具体观测数据相关

首先将这个矩阵按照下图中所示的方式来划分区域
不难发现，这四个区域正是对应了之前公式以 G-N 的 H 矩阵中的四个矩阵块

为了后续分析地方便，我们称这四个块为 B， E， C

于是，对应的线性方程组也可以由 $H∆x = g$ 变为如下形式：

其中 B 是对角块矩阵，每个对角块的维度和相机参数的维度相同，对角块的个数是相机变量的个数
由于路标数量会远远大于相机变量个数，所以 C 往往也远大于 B
三维空间中每个路标点为三维，于是 C 矩阵为对角块矩阵，每个块为 3 × 3 维矩阵
对角块矩阵逆的难度远小于对一般矩阵的求逆难度
因为只需要对那些对角线矩阵块分别求逆即可

考虑到这个特性，线性方程组进行高斯消元，目标是消去右上角的非对角部分 E，得：

整理，得：

经过消元之后，第一行方程组变成和 ∆x_p 无关的项
单独把它拿出来，得到关于位姿部分的增量方程：

这个线性方程组的维度和 B 矩阵一样
做法是先求解这个方程，然后把解得的 ∆x_c 代入到原方程，然后求解 ∆x_p
这个过程称为 Marginalization，或者 Schur消元 (Schur Elimination)

相比于直接解线性方程的做法，它的优势在于：

1. 在消元过程中，由于 C 为对角块，所以 C⁻¹ 容易解得
2. 求解了 ∆x_c 之后，路标部分的增量方程由 ∆x_p = C⁻¹(w − E^T ∆x_c) 给出
   这依然用到了 C⁻¹ 易于求解的特性
   于是，边缘化的主要的计算量在于求解上式
   它仅是一个普通的线性方程，没有特殊的结构可以利用
   将此方程的系数记为 S，它的稀疏性如何呢？
   下图显示了进行 Schur 消元之后的一个 S 实例，可以看到它的稀疏性是不规则的
   
   前面说到， H 矩阵的非对角块处的非零元素对应着相机和路标的关联
   那么，进行了Schur 消元后 S 的稀疏性是否具有物理意义呢？
   有的，S 矩阵的非对角线上的非零矩阵块，表示了该处对应的两个相机变量之间存在着共同观测
   的路标点，有时候称为共视（Co-visibility）
   反之，如果该块为零，则表示这两个相机没有共同观测
   例如，左上角前 4 × 4 个矩阵块可以表示对应的相机变量 C₁, C₂, C₃, C₄, 之间有共同观测
   

于是， S 矩阵的稀疏性结构当取决于实际观测的结果，无法提前预知
在实践当中，例如 ORB_SLAM 中的 Local Mapping 环节
在做 BA 的时候刻意选择那些具有共同观测的帧作为关键帧
在这种情况下 Schur 消元后得到的 S 就是稠密矩阵
不过，由于这个模块并不是实时执行，所以这种做法也是可以接受的

但是在另一些方法里面，例如 DSO， OKVIS 等，它们采用了滑动窗口方法（Sliding Window）
这类方法对每一帧都要求做一次 BA 来防止误差的累积
因此它们也必须采用一些技巧来保持 S 矩阵的稀疏性

从概率角度来看，称这一步为边缘化
是因为实际上把求 (∆x_c, ∆x_p) 的问题，转化成先求 ∆x_c，再求 ∆x_p 的过程
这一步相当于做了条件概率展开：

结果是求出了关于 x_c 的边缘分布，故称边缘化
在上边讲的边缘化过程中，实际把所有的路标点都给边缘化了
根据实际情况，也能选择一部分进行边缘化
同时，Schur 消元只是实现边缘化的其中一种方式，同样可以使用 Cholesky 分解进行边缘化

在进行了 Schur 消元后，还需要求解线性方程组位姿部分的增量方程

很遗憾地说，这部分就属于传统的矩阵数值求解的部分了，通常是用分解来计算的
不管采用哪种求解办法，都建议利用 H 的稀疏性进行 Schur消元
不光是因为这样可以提高速度，也同时是因为消元后的 S 矩阵的条件数往往比之前的 H 矩阵的条件数要小
Schur 消元也并不是意味将所有路标消元，将相机变量消元也是SLAM 当中采用的手段

---

5. 鲁棒核函数

在前面的 BA 问题中，最小化误差项的二范数平方和，作为目标函数
这种做法虽然很直观，但存在一个严重的问题：
如果出于误匹配等原因，某个误差项给的数据是错误的，会发生什么呢？

把一条原本不应该加到图中的边给加进去了，然而优化算法并不能辨别出这是个错误数据，它会把所有的数据都当作误差来处理
这时，算法会看到一条误差很大的边，它的梯度也很大
意味着调整与它相关的变量会使目标函数下降更多

所以，算法将试图调整这条边所连接的节点的估计值，使它们顺应这条边的无理要求
由于这个边的误差真的很大，往往会抹平了其他正确边的影响，使优化算法专注于调整一个错误的值
这显然不是希望看到的
出现这种问题的原因是，当误差很大时，二范数增长得太快了
于是就有了核函数的存在
核函数保证每条边的误差不会大的没边，掩盖掉其他的边
具体的方式是，把原先误差的二范数度量，替换成一个增长没有那么快的函数
同时保证光滑性质，不然没法求导
因为它们使得整个优化结果更为鲁棒，所以又叫它们为鲁棒核函数（Robust Kernel）

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

当误差 e 大于某个阈值 δ 后，函数增长由二次形式变成了一次形式，相当于限制了梯度的最大值
同时， Huber 核函数又是光滑的，可以很方便地求导
下图示了 Huber 核函数与二次函数的对比
可见在误差较大时 Huber 核函数增长明显低于二次函数

---

参考：

《视觉SLAM十四讲》

---

相关推荐：

视觉SLAM笔记（51） 非线性系统和 EKF
视觉SLAM笔记（50） 线性系统和 KF
视觉SLAM笔记（49） 后端状态估计
视觉SLAM笔记（48） 局部地图
视觉SLAM笔记（47） 优化 PnP 的结果

---

谢谢！