（十）ORBSLAM闭环
ORBSLAM闭环简介
SLAM中闭环的目的，是将当前的相机位姿和场景与回环帧处的位姿和场景进行融合，从而消除累积误差，校正运动轨迹。那么达到这个目的的前提，是必须要检测到正确的回环。当然，这个要求在前面我们讲解回环检测的时候已经详细介绍过了，如果不能通过层层筛选，那么我们会选择不要闭环。但是，在检测到正确的回环以后，我们还需要保证闭环的融合和校正是高质量的，这样才能消除累积误差。ORBSLAM2也尽可能地在满足这个要求。
接下来，我们就按下面两个方面来介绍闭环模块：
1. Bundle Adjustment；
2. ORBSLAM2闭环方法提纲；
Bundle Adjustment
相信很多人都知道，在SLAM最开始的时候，我们都是通过滤波的方式来做姿态优化的，像卡尔曼滤波、拓展卡尔曼滤波、信息滤波和粒子滤波等等。但是基于滤波的方法都是假设变量满足一阶马尔可夫假设的，也就是当前时刻机器人的状态只和前一时刻的状态有关，再往前的都不考虑。因此，这会导致一个问题，如果每一次的估计都存在一点点误差，随着时间的积累，这个误差就会越来越大，导致最终算法不可用，这对于我们来说通常是难以接受。
于是人们便想着改变这种模式，将先前的状态全部考虑进来，从而减少甚至消除累积误差，这也就产生了我们现在最常见的非线性优化的方法。但是这种方法存在一个非常大的问题，在我们利用迭代求解的时候，每次都需要计算一个海赛阵/其他替代矩阵的逆。对于SLAM问题来说，通常问题的规模都相对比较大，比如一个相机pose可以同时观测到上百个地图点，于是随着相机的运动，地图点的累积速度将非常快，而这个海赛阵/其他替代矩阵就会变得非常庞大，求解其逆的过程也变得更加困难。
直到后来，人们发现了SLAM问题的稀疏性，即每个小的雅可比，比如 $J_{ij}$，只在相机 $ID = i$ 与地图点 $ID = j$ 有连接的边的地方有非0值，其他地方均为0。通过 $J^{T}J$ 构成的矩阵满足这样的条件：
$J^{T}J = \begin{bmatrix}
B & E \\
E^{T} & C
\end{bmatrix}$
其中，$B$ 和 $C$ 是两个对角块矩阵，在矩阵求逆的时候我们只需对各个块直接求逆即可，非常方便。而 $E$ 的稠密程度取决于观测内容。因此，才有了我们现在用的非线性优化的工具。
讲到这里，基本就可以开始介绍我们的内容了。我们比较熟悉的BA是用在struct from motion里面做三维重建，struct from motion是离线的，并且数据来源是来自不同相机不同场景不同时间的。而SLAM问题的数据，基本都是一个时间序列里同一个场景同一个相机采集的，所以SLAM面对的问题相对于struct from motion的难度稍微小一些。当然，这只是单纯的从重建的角度来说，涉及其他一些方面，可能SLAM的难度就会比较大，这个我们不做详谈。
BA和我们先前介绍的运动估计方法是一致的：https://www.cnblogs.com/yepeichu/p/10746952.html
不过我们在介绍运动估计的时候并没有讨论稀疏性的问题，所以在这一讲我们再做一下补充。
假设我们的待估计变量为 $x = [\xi_{1}, \dots, \xi_{m}, p_{1}, \dots, p_{n}]^{T}$，其中 $\xi_{1}, \dots, \xi_{m}$ 表示轨迹中的 $m$ 个运动状态矩阵的李代数，而 $p_{1}, \dots, p_{n}$ 表示所有观测点在世界坐标系中的三维空间坐标。
在BA中，我们每次通过增加 $\Delta x$，使得误差下降一点点，多次迭代以后，误差值达到我们的阈值要求，这个问题就算收敛了。所以关键的问题就是求解这个 $\Delta x$。
我们再描述一下我们的误差函数：
\begin{equation}
\frac{1}{2}\|f(x)\|_{2}^{2} = \frac{1}{2}\sum\limits_{i=1}^{m}\sum\limits_{j=1}^{n}\|e_{ij}\|_{2}^{2} = \frac{1}{2}\sum\limits_{i=1}^{m}\sum\limits_{j=1}^{n}\|u_{j} - \frac{1}{s_{j}}Kexp(\xi_{i}^{\land})P_{j}\|_{2}^{2}
\end{equation}
这就是一个非常简单的重投影误差，跟我们前面介绍的内容一致。假设误差 $e_{ij}$ 对位姿 $\xi_{i}$ 的雅可比为 $F_{ij}$，对地图点 $p_{j}$ 的雅可比为 $E_{ij}$，则误差函数的增量形式可以描述成下列的形式：
\begin{equation}
\frac{1}{2}\|f(x+\Delta x)\|_{2}^{2} \thickapprox \frac{1}{2}\sum\limits_{i=1}^{m}\sum\limits_{j=1}^{n}\|e_{ij} + F_{ij}\Delta\xi_{i} + E_{ij}\Delta p_{j}\|_{2}^{2}
\end{equation}
我们通过将增量函数进行一阶泰勒展开近似，于是得到公式$(2)$的样子。为了更加简洁的描述问题，我们将三维点和位姿分成两个部分,即：
\begin{equation}
x_{c} = [ \xi_{1}, \xi_{2}, \dots, \xi_{m}]^{T} \in R^{6m}
\end{equation}
\begin{equation}
x_{p} = [ p_{1}, p_{2}, \dots, p_{n}]^{T} \in R^{3n}
\end{equation}
那么原始变量可以描述成 $x = [x_{c}, x_{p}]$。同理，对应的雅可比按照位置合成一个更大的雅可比矩阵，将其记为 $J = [F, E]$，于是可以简化问题为：
\begin{equation}
\frac{1}{2}\|f(x+\Delta x)\|_{2}^{2} \thickapprox \frac{1}{2}\|e + J\Delta x^{T}\|_{2}^{2} = \frac{1}{2}(e^{T}e + 2e^{T}J\Delta x + \Delta x^{T}J^{T}J\Delta x)
\end{equation}
公式 $(5)$ 对增量求导，并令导数为0，则有：
\begin{equation}
J^{T}J\Delta x = -J^{T}e \Rightarrow H\Delta x = g
\end{equation}
值得注意的是，这个 $H = J^{T}J$ 矩阵，就是我们介绍的，具有稀疏性的矩阵。求解公式 $(6)$ 的增量问题，可以利用 $H$ 矩阵的稀疏性，先求个一部分增量，然后利用求出来的这部分增量去求解其它的部分。这种方法叫Schur消元法，在SLAM里面也叫边缘化。先将我们的问题列出来：
\begin{equation}
\begin{bmatrix} B&E\\E^{T}&C \end{bmatrix} \begin{bmatrix}\Delta x_{c}\\ \Delta x_{p} \end{bmatrix} = \begin{bmatrix}v\\w\end{bmatrix}
\end{equation}
为了先求出一个变量，我们通过高斯消元（其实就是行列式变换），将系数矩阵转换成一个下三角矩阵：
\begin{equation}
\begin{bmatrix} I&-EC^{-1}\\0&I \end{bmatrix} \begin{bmatrix} B&E\\E^{T}&c \end{bmatrix} \begin{bmatrix} \Delta x_{c}\\ \Delta x_{p} \end{bmatrix} = \begin{bmatrix} I&-EC^{-1}\\0&I \end{bmatrix} \begin{bmatrix} v\\w \end{bmatrix}
\end{equation}
简单地整理以后，我们就得到：
\begin{equation}
\begin{bmatrix} B-EC^{-1}E^{T}&0\\E^{T}&C \end{bmatrix} \begin{bmatrix}\Delta x_{c}\\ \Delta x_{p} \end{bmatrix} = \begin{bmatrix}v-EC^{-1}w\\w\end{bmatrix}
\end{equation}
于是我们便可以得到两个方程，分别是：
\begin{equation}
[B-EC^{-1}E^{T}]\Delta x_{c} = v - EC^{-1}w
\end{equation}
\begin{equation}
E^{T}\Delta x_{c} + C\Delta x_{p} = w
\end{equation}
先求解出 $\Delta x_{c}$，再求出 $\Delta x_{p}$。值得注意的是，求$\Delta x_{c}$ 时，线性方程的维度与B矩阵一样。尽管两个式子都需要求解C矩阵的逆，但是我们在前面介绍过，B矩阵和C矩阵都是对角块矩阵，求逆只需要对各个对角块分别求逆就可以了，计算效率非常高。
于是，我们的BA问题，通过矩阵的稀疏性，就可以使我们的SLAM问题能够在可预期的时间里计算出来。而不是对一个dense的大规模矩阵直接求逆，这个直接求逆的计算量是问题规模的三次方，对于大规模的SLAM问题，显然是无法实现的。
在求解BA问题时，我们对所有的误差项都是一视同仁的。这通常会导致一个问题，如果所有匹配都是正确的，固然是好。可是如果有错误匹配，这会出现一个非常大的误差项，而优化算法为了降低误差，会选择迁就这个误差项，最终导致求解的结果与我们的预期不符。所以，我们会用到鲁棒核函数来控制误差项，防止错误匹配的影响。
比如，最常见的Huber核（参考视觉SLAM十四讲）：
\begin{equation}
H(e) = \left\{
\begin{array}{lrl}
\frac{1}{2}e^{2} && {|e|\leq \delta} \\
\delta(|e| - \frac{1}{2}\delta) && {else}
\end{array}\right.
\end{equation}
当误差项大于阈值时，函数增长由二次形式变成了一次形式，限制了错误匹配的影响范围。
OK，到这里，Bundle Adjustment的求解方法，我们基本就介绍完了。BA真是个好东西，建议大家多参考一些相关的资料。
值得一提的是，尽管我们说Bundle Adjustment可以同时调整相机内参、地图点和相机姿态。但是在ORBSLAM2中，我们会把地图点都固定住，并固定第一帧的相机姿态，利用地图点作为约束来优化其它相机姿态，所以这个问题其实更像是姿态图优化——顾名思义，就是一个只优化相机姿态的问题。
另外，笔者发现了一个非常好的博客：https://www.cnblogs.com/xiaochen-qiu/p/9768949.html，这篇博客是泡泡机器人的邱博士写的，供大家参考。
ORBSLAM2闭环方法提纲
按照惯例，我们还是同样，再来谈谈ORBSLAM2中，他做闭环的思路是什么样的，以供大家参考。
1. 首先需要关闭正在执行的全局BA；
2. 更新与当前帧的共视图关键帧，共同观测不小于15个点；
3. 为共视图关键帧各保留两个姿态，一个是原始姿态，一个是经回环校正以后的姿态；
4. 利用校正后的姿态将共视图关键帧所有地图点的位置全部更新，即利用旧姿态投影回像平面，再用新姿态投影到三维空间；
5. 更新当前帧共视图中的每个关键帧各自的共视图；
6. 融合地图点，将当前帧的所有与回环帧相关联的地图点用回环帧的地图点进行替换，若未匹配的，则直接构建新地图点，用回环帧对应位置的地图点初始化；
7. 利用更新的相机姿态将回环帧的共视图关键帧构成的局部地图点全部重投影到当前帧中，为当前帧查找最优匹配，实现更好的融合；
8. 更新共视图关键帧与回环帧的连接，并剔除旧的连接关系；
9. 优化Essential Graph，优化具有较强连接关系的姿态，并通过增加不动姿态作为约束，来增强优化的效果；
10. 执行全局Bundle Adjustment，即把所有的地图点和所有关键帧全部包括进来进行优化。值得注意的是，这里固定了第一个关键帧的姿态，固定了所有地图点，只优化除第一帧外的所有关键帧的姿态，所以更像一个姿态图优化。
于是，到这里，我们就把ORBSLAM2中的闭环模块讲完了。
总结
总结一下，本文主要介绍的是ORBSLAM2中的闭环模块，内容涉及Bundle Adjustment。
由于在前面几讲中，或多或少介绍了Bundle Adjustment的思想，但是没有涉及具体怎么利用稀疏性来求解问题，于是在这一讲中，笔者便详细地分析了这个过程。
此外，笔者罗列了闭环模块的方法大纲，供大家在实际开发项目时参考。
OK，ORBSLAM2这个经典框架我们基本就分析完了。接下来，笔者可能会谈谈它的实际应用等问题，不过，那是后话了。
参考文献：
[1] 视觉SLAM十四讲
PS：
如果您觉得我的博客对您有所帮助，欢迎关注我的博客。此外，欢迎转载我的文章，但请注明出处链接。
对本文有任何问题可以在留言区进行评论，也可以在泡泡机器人论坛：http://paopaorobot.org/bbs/index.php?c=cate&fid=1中的SLAM技术交流模块发帖提问。
我的github链接是：https://github.com/yepeichu123/orbslam2_learn。
