《增强现实：原理、算法与应用》读书笔记（9）同时定位与地图构建&视觉SLAM

《增强现实：原理、算法与应用》读书笔记（9）视觉SLAM

早在人类开始航海的时期，人类借助指南针与六分仪在大洋上导航。中国古代，人们借助指南车的轮式里程计的差速获取当前朝向。这种仅根据某一时刻传感器的局部信息进行推断运动从而恢复位置的方法称为航位推测法。

然而，航位推测法不能解决定位中误差不断累积的问题，如果没有地图或绝对位置信息，航线会逐渐偏离目的地。这时就需要根据已经记录过的地貌来矫正定位误差。

SLAM算法可以在未知环境中定位自身方位并同时构建三维地图。SLAM算法根据传感器不同分为很多种，常用的包括摄像头、深度传感器、激光雷达、差分GPS、惯导等。在增强现实应用中，由于虚拟物体的叠加目标通常为图像/视频，因此基于图像/视频等视觉信息的SLAM方案，对于确保虚实融合结果在几何上保持一致有着天然优势。

目前SLAM系统较常采用的并行跟踪和地图构建框架，最早为Klein等（2007）所提出，即将实时定位和地图构建分别分配给前台线程和后台线程上执行。

前台线程实时处理输入的传感器信息，恢复每个时刻的设备位姿；后台线程主要对关键帧和三维点进行局部或全局优化，以减少误差累积；此外，后台线程还会通过检测是否存在回路并将其闭合来减少误差累积。

SLAM地图与常用的地图不同，多采用体素或点云等方式表达，而且往往还带有特征点、关键帧等信息，以便于对设备进行准确定位。SLAM的计算过程往往是假设先有一个初始位置（即一个局部坐标系），然后从这个局部坐标系开始不断扩展地图并更新自身位置，一点点完成全部地图的构建。

视觉SLAM

本节将重点介绍基于特征匹配的视觉SLAM。基于特征匹配的视觉SLAM的常用系统框架包含这样几个模块：（1）初始化；（2）前台特征跟踪与位姿求解；（3）后台优化；（4）重定位；（5）回路检测与闭合。下面将就针对这几个模块分别进行介绍。

初始化

某种程度上，单目SLAM系统的初始化与SfM系统在相机内参已知情况下非常类似，而双目或多目SKAM的初始化步骤更简单。

对于单目SLAM来说，如果想要精确地三角化出2D匹配点的三维坐标，就需要知道这两帧对应的相机的相对位姿。在初始状态下，单目SLAM通常会先根据两帧之间2D-2D的特征匹配来估计出相机的初始位姿，并三角化出匹配上的特征点的三维位置，之后再用3D-2D的方法对这些特征进行跟踪匹配，并求解相机运动。

对双目或多目SLAM而言，由于预先标定了多个同步相机之间的相对位姿变换，三维信息可以通过多相机视图进行特征匹配并三角化来获得，在这之后的相机运动直接通过3D-2D的方法求解，初始化步骤会简单很多。

通常，我们可以借助极线几何的相关理论来进行运动的估计。假设相邻图像上的匹配特征的齐次坐标分别表示为${\hat{x}}_1,{\hat{x}}_2$，用内参归一化后的坐标为${\tilde{x}}_1,{\tilde{x}}_2$，那么匹配点满足如下极线约束：
$${\hat{x}}_2^{\top }F{\hat{x}}_1^{\top }={\tilde{x}}_2^{\top }E{\tilde{x}}_1=0$$

式中，本质矩阵$E=[t{]}_{\times }R$，基础矩阵$F=K^{-\top }[t{]}_{\times }R{K}^{-1}$，$K$为相机内参矩阵，$R$为两帧之间的旋转矩阵，$t$为平移向量。

可以看出，通过匹配特征的空间位置关系，建立了相机运动中旋转$R$和平移$t$的关联。有了特征匹配的关系之后，根据匹配的像素点位置求出$E$或者$F$，再根据$E$或者$F$分解求出$R$和$t$。

除了基础矩阵和本质矩阵，在一些特殊的情况下，如果场景中的特征点都位于同一个平面上，那么可以通过单应性进行运动估计，分解出$R$和$t$。单应性矩阵描述了两个平面之间的映射关系，假设图像之间的单应性变换为$H$，参照之前的推导有：
$${\hat{x}}_2\sim H{\hat{x}}_1$$

式中，$H=K\left(R-\frac{t{n}^{\top }}{d}\right)K^{-1}$，平面参数$\pi =(n^{\top },d{)}^{\top }$。

单应性在SLAM中有着非常重要的意义，尤其是在特征点共面或者相机发生纯旋转运动，导致基础矩阵的自由度下降，引起所谓的退化问题时。然而，现实中的数据都是存在噪声的，此时如果按照传统的方法进行基础矩阵的求解，基础矩阵多出来的自由度将会被噪声决定，进而可能会导致运动估计的错误。因此，在实际求解过程中，通常会同时估计基础矩阵和单应性矩阵，并根据一定策略（如选择重投影误差较小的一个）来选择最终的运动估计矩阵。

Mur-Artal等（2015）提出的ORB-SLAM在初始化阶段不需要用户指定初始化帧，而是自动地从输入图像序列中选取两帧，同时估计单应性矩阵和基础矩阵，并通过计算两个矩阵模型下点匹配的对称传递误差，根据统计模型计算得分，最后再依据一定的策略选择其中一个矩阵模型用于初始化。

前台跟踪

视觉初始化完成后，能够得到一个初始的地图，当相机运动捕获到新的图像时，通过特征匹配建立图像点同地图点之间的匹配关系并进行当前相机的位姿求解。同时，根据图像特征之间的共视关系，可以生成新的地图点，对地图不断扩展。前台跟踪的过程就是不断进行特征匹配和位姿求解。

【1】特征提取与匹配

特征点在图像发生旋转、缩放等变换后仍然具有比较好的局部唯一性和显著性，如ORB、SIFT、SURF等。这里我们队SLAM系统中的特征匹配的两种常见策略进行介绍：

（1）基于关键帧的匹配

对于实时性要求较高的SLAM系统，通常会采取提取关键帧的策略，地图点的三角化也只针对关键帧上的点进行。同一般的特征匹配不同，/1/当前帧不是同前一帧的特征进行匹配，而是同关键帧上的地图点进行匹配；/2/一般会考虑运动先验信息，将关键帧上的地图点根据运动信息投影到当前帧上，再在投影点附近选择特征点进行匹配，该策略能极大地提高特征匹配的速度和精度；/3/特征点提取以及匹配过程中会考虑点的空间均匀分布，防止点集中在图像上某一局部区域，造成求解结果的偏差。

此外， 如果场景中存在运动的物体或逐渐变化，那还需要检测出改变的地图点并将其从地图中删掉。一种可行的做法（Tan et al., 2013）是将与当前帧视角比较接近的关键帧上的地图点投影到当前帧来比较颜色的变化，如果颜色差异较大且排除遮挡的因素，那么可以判定发生了改变，将其从地图中删掉。

（2）连续帧跟踪

连续帧跟踪的方法是将当前帧同前一帧图像直接进行匹配，连续帧跟踪通常采用基于光度误差计算的光流法跟踪，而不是描述子距离。由于连续帧跟踪更加强调特征的连续性，因此需要更加频繁地进行点的三角化和地图中三维点的更新。而且，一旦三维点移出了视野之后重新进入视野，就需要重新匹配和估计三维。

同时，连续帧跟踪一般没有描述子，难以用来做重定位和回路闭合。此外，连续帧跟踪的误差随着场景的扩展也会逐渐累积，并且难以消除。因此，单纯的连续帧跟踪在较小场景中的短时间跟踪结果较好，但是不太适合大场景下或长时间的跟踪。

但是，相对关键帧匹配来说，连续帧跟踪一般速度会更快，而且对弱纹理、重复纹理的鲁棒性更好。如果能将关键帧匹配同连续帧跟踪策略很好的结合起来，就可以同时发挥出各自的优势，提高特征跟踪的鲁棒性。

【2】位姿估计

地图初始化后，可以根据地图上的三维点与当前帧图像的二维匹配点之间的关系，通过最小重投影误差求解出当前帧的位姿，这个过程称为PnP（perspective-n-point）。关于PnP的方法有很多，比如直接线性变换（DLT）、P3P、EPnP、UPnP以及构造最小二乘问题进行非线性优化迭代求解等。

后台优化

前台跟踪部分可以看做后台优化的准备工作，包括构建视觉特征与相机状态之间的关联、筛选关键帧、获得相机状态或地图状态的初值等。其结果一般作为后台优化的输入，通过一些机制来触发后台的更新。而后台则通常要通过集束调整对相机状态和地图状态进行更新来保证全局一致性。

一般，前台线程运行实时性要求较高的任务，例如AR应用中与用户的交互、每一时刻相机位姿的求解。运算任务较重的，如局部优化、全局优化、回路闭合和重定位，甚至三维场景的恢复一般都放到后台线程执行。

实时的SLAM跟踪中，每次都求解完整的集束调整将会严重降低系统性能，无法满足及时降低误差的要求。针对这一问题，常见的做法是高频的局部优化和低频的全局优化相结合，甚至只在回路闭合的时候调用全局优化。

（1）局部窗口优化

局部优化通常采用滑动窗口扫描式进行：最新的一帧图像被加入到滑动窗口进行优化，而窗口内最老的一帧图像被移除，窗口内始终保持一定数量的图像。对于一个时间窗口内的图像 I = { I k ∣ k = i − n + 1 , ⋯   , i } I=\left \{ I_k|k=i-n+1,\cdots,i \right \} I={Ik​∣k=i−n+1,⋯,i}依旧可以采用集束调整的方式，通过最小化重投影误差来对窗口内全部的相机位姿 C = { R k , t k ∣ k = i − n + 1 , ⋯   , i } C=\left \{ R_k,t_k|k=i-n+1,\cdots,i \right \} C={Rk​,tk​∣k=i−n+1,⋯,i}和相关的三维点集 M = { X j } M=\left \{ X_j \right \} M={Xj​}进行优化：
$$\underset{C,M}{\mathrm{argmin}}\sum _{k=i-n+1}^i\sum _{x_{kj}\in I_k}{\Vert \pi (K_k(R_k{X}_j+t_k))-x_{kj}\Vert }_{{\sum }_{kj}}^2$$

式中，${\sum }_{kj}$表示相机位姿$R_k,t_k$与三维点$X_j$之间的协方差矩阵，有${\Vert e\Vert }_{{\sum }_{kj}}^2=e^{-\top }{\sum }_{kj}^{-1}e$。

每当向局部窗口内加入一个新的帧时，就要调用一次局部窗口优化。另外，对于滑出窗口的帧，不同的系统也有不同的选择策略。有的按部就班，按照时间顺序对窗口进行滑动；有的系统会判断将要加入窗口的帧与窗口中最新的一帧能否形成足够的视差，在视差不足的时候，会选用新帧替换窗口中最新一帧，而不是将最老的一帧滑出窗口。

不同的选择适应不同的应用场景，在AR应用中，经常会遇到运动不够大，相机接近静止的情况，这时候为了保证求解的稳定性，在视差不够时使用新帧替换窗口中最新一帧的策略就比较合适。

（2）带状态先验的局部窗口优化

一般单目视觉SLAM系统有7个状态自由度是不可观的，包括全局的位姿（6DOF）和尺度（1DOF）。即使是重力已知的单目视觉惯性SLAM，也至少有4个状态自由度是不可测的，包括关于重力方向的旋转（1DOF）和全局的平移（3DOF）。

对于离线的SfM系统通常可以不考虑这些，这对结果不会产生影响。但是对于AR应用，进行集束调整时任何的欠约束情况都有可能造成求解结果的反复跳变，从而严重破坏用户体验。

一种做法是每次求解局部窗口集束调整时将从窗口滑出去的帧的状态和在窗口中的帧不可见的三维点设置为常量。这样虽然保证了集束调整的结果总是不会跳变的，但是却却引入了误差累积——滑出窗口的状态和三维点被条件化了，这种情况下，如果不借助全局优化，误差累积是无法消除的。

另一种做法就是在每次将状态和相应的三维点滑出窗口时，对其做边缘化（marginalization），然后将其结果作为状态先验加入到下一次局部窗口优化中。这样窗口优化求解的就是下面这样一个最大后验概率估计：
$$\underset{C,M}{\mathrm{argmin}}\left({\Vert r_I\Vert }_{{\sum }_I}^2+\sum _{k=i-n+1}^i\sum _{x_{kj}\in I_k}{\Vert \pi \left(K_k\left(R_k{X}_j+t_k\right)\right)-x_{kj}\Vert }_{{\sum }_{kj}}^2\right)$$

式中，$r_I$是先验部分的残差，${\sum }_I$是先验协方差矩阵。这里需要指出的是新加入窗口的状态是没有先验部分的。边缘化操作虽然会比较大地影响问题结构的稀疏性，但是在窗口长度设置较小时，其对求解速度的影响并不显著。在精度要求和全局一致性要求不是很苛刻的场景下， 这种做法是比较合理的。

（3）全局优化

全局优化即对所有历史相机状态和地图状态进行批量式优化。后台地图的优化无需实时完成，也不需要频繁调用，有的系统甚至没有全局优化这一部分。但由于回路闭合、重定位，甚至三维结构恢复等需求的存在，有时还是需要一个全局优化模块来不时地进行全局更新，以消除误差累积。

通常，加入到全局优化中的状态和三维点都是要经过严格筛选的，要尽可能不引入外点。在检测到回路的时候需要调用全局优化使得全局的相机状态和地图保持一致，以达到最优。除此之外也可以定时的触发全局优化，以防止在触发回路检测的时候由于误差太大导致回路闭合失败。

由于全局优化问题规模较大， 同时又由于回路检测等因素的存在，导致我们要处理一个变量更多、结构更一般的稀疏非线性最小二乘。此外，局部的滑动窗口优化会不断产生积累误差，需要及时获得地图的更新来进行纠正，因此也要采取一些手段保证后台地图能够在可接受的时间内完成。具体来说，就是采用之前提到过的一些策略，如利用地图中三维点变量个数远大于相机状态变量的特点，选用无结构的建模只求解位姿图优化问题；或是利用SLAM问题的局部性特点，采用增量式的集束调整方法，每次优化仅对少数受到新的观测影响的变量进行更新，避免大规模的批量集束调整。

即便如此，让全局地图无限制地增长下去也不是明智的选择，不仅会对应用的性能造成影响，也会使得内存占用过大。因此我们可以引入一些状态删除的策略来解决这个问题。与局部窗口优化类似，删除状态时也有直接删除和通过边缘化操作之后再删除这两种策略的选择。

直接删除状态可保证运行时间的上限，但相对来说精度更低。边缘化操作除了自身需要计算时间，同时也会使得问题的稀疏性变差，影响后续的优化速度，但精度更好。

重定位与回路闭合

实时跟踪定位过程中误差难以避免。在传统的视觉SLAM系统中，当前帧的位姿估计是依赖于之前帧的位姿估计，整个相机定位过程中误差会不断累积，影响后续帧的跟踪。

我们可以通过一些特殊的约束对当前相机的位姿进行修正，回路闭合就是一种常用的约束。在跟踪过程中，一旦发现当前帧包含之前帧的场景，那么相机的轨迹中就产生了一个回路，利用回路约束可以减少之前相机轨迹求解过程中的误差。

回路闭合也只能应用在跟踪还算正常的情况下，对于视觉SLAM，图像质量过差（剧烈运动导致严重的运动模糊），或者图像缺少特征（偶然出现的一面纯白的墙，或者高光的玻璃）都会导致一段时间内跟踪系统崩溃。对于这样的极端情况，需要通过重定位来重置跟踪状态。

重定位指的是由于跟踪失败或误差过大，导致需要根据之前构建的地图进行重新匹配和相机位姿求解，以恢复正确的相机位姿。不进行重定位，位姿将持续丢失。

回路闭合和重定位的第一步操作都是将当前场景与之前保存过的地图对比，寻找曾经访问过的场景。这其实是一个图像检索的过程，按照使用的特征类型可以分为局部特征检索方法和全局图像检索方法。

局部特征检索方法主要基于各种特征点（如SIFT、SURF、ORB等）的检索方法，可以一定程度上容忍视角的变化，结合词袋算法，可以在保证一定准确率的同时得到不错的速度，但是对场景变化的容忍度较差。使用整张图像信息来进行图像检索的方法对于视角变化的容忍度差，但对场景变化的容忍度好。

现有的SLAM系统中比较流行的重定位和回路闭合方法是特征点结合词袋的方法，如ORB-SLAM、VINS-Mono等。基于词袋的方法需要预加载一个词袋字典树，将图像中的每一个特征点的描述子转换为一个单词，通过对整张图像的单词统计生成一个词袋向量，词袋向量间的距离既代表了两张图像之间的差异性。在图像检索过程中，会利用倒排索引先找出与当前帧拥有相同单词的关键帧，并根据它们的词袋向量计算与当前帧的相似度。

词袋方法的主要缺点是词袋文件通常较大，以ORB-SLAM为例，其词袋字典树文件一般至少几十MB，对于移动设备来说负担较大。

不论是对于重定位还是回路闭合，匹配的准确性都极为重要，都需要使用其他方法进行验证。在验证阶段，由于重定位和回路闭合的求解目的不同，验证的方法也会有所不同。

以ORB-SLAM为例，重定位的验证过程中，会将每个候选帧与当前帧进行特征匹配，对匹配的结果使用PnP算法进行验证。如果特征点匹配数量不够或大量的匹配点在PnP求解过程中被剔除，说明这两帧的匹配不符合空间一致性，匹配错误。得到准确的图像匹配后，就可以根据匹配结果求解相机位姿，事实上OBR-SLAM直接使用PnP求解出的位姿作为当前帧的位姿。

近年来，随着深度学习在三维视觉领域的深入应用，一些端到端的相机位姿估计方法也涌现出来。深度学习和视觉定位结合的开创性工作PoseNet（Kendall et al., 2015）就使用神经网络直接从图像中得到6DOF的相机位姿。相较于传统的视觉定位方法省去了复杂的图像匹配过程，并且也不需要对相机位姿进行迭代求解，但是输入图像必须在训练场景中。

再后来的工作中（Kendall et al., 2017），他们在误差函数中使用了投影误差，进一步提高了位姿估计的精度。

也有一些深度学习的研究者将神经网络和视觉SLAM中的另一些方法结合以达到更好地效果，如MapNet（Brahmbhatt et al., 2017）就使用了传统方法求解两张图像的相对位姿，与网络计算出的相对位姿对比得到相机的相对位姿误差，将相对位姿误差添加到损失函数中，使得求解出的相机位姿更加平滑，并且MapNet还会将连续多帧的结果进行位姿图优化，使得最终估计出的相机位姿更为准确。

直接法跟踪

虽然当下基于特征点的方法仍然是视觉SLAM中比较主流的方法，但是也存在一些缺点：（1）基于特征点跟踪的SLAM都需要预先对图像进行特征点的提取和匹配，这个过程耗远比位姿估计多，通常前段跟踪一半以上时间花在了特征提取和匹配上；（2）特征点不足以表达图像上的完整信息，不可避免的导致一些信息丢失；（3）基于特征点的方法过于依赖特征，对于特征不丰富或重复纹理的情况难以鲁棒地进行跟踪和位姿估计。

直接基于光度误差优化的SLAM算法统称为直接法，直接法一方面避免了特征提取和匹配的过程，另一方面也能够更加充分地利用图像上的像素信息，一定程度上可以缓解特征不足的问题。

直接法根据采用的像素点数量不同，可以分为稀疏、半稠密和稠密三种。相对于特征点法只能构建稀疏的地图，直接法能够重建出更加稠密的地图。

讲直接法需要先了解光流法特征匹配。光流描述的是图像上的像素点随着时间的运动情况。根据计算像素点个数的不同，可以分为稀疏光流法和稠密光流法。

光流估计中普遍采用了灰度一致性假设，即认为随着时间推移，图像上同一个像素点的灰度不发生变化。在SLAM中用于特征跟踪的往往指稀疏光流。下文中若不加说明，光流法指的都是稀疏光流法。

光流法在进行特征匹配的过程中采用了同直接法类似的光度误差，但是在运动估计中采用的仍然是同间接法类似的重投影误差。

同光流法非常类似，基于直接跟踪的SLAM算法也是基于灰度不变假设进行运动估计的，只是两者使用的时机不同。光流法是依据灰度一致性假设进行特征的匹配，而直接法是直接根据灰度不变性约束进行运动求解。

在基于特征的方法中，通过特征匹配我们可以获取特征点$x_1,x_2$之间的点对关系，从而可以计算重投影误差。但是在直接法中我们并不会进行特征匹配的操作，所以无法确定当前帧图像中的像素点$x_1$同下一帧图像的哪一个像素点$x_2$是对应的，而这个相似程度就可以通过灰度差异进行衡量。因此，通过优化相机的位姿，来寻找同$x_1$醉相思的像素点$x_2$，通特征点类似，这也是一个优化问题，不同的是这里不再是最小化重投影误差，而是光度误差，目标方程为：

$$\underset{R,I}{\mathrm{argmin}}\sum _i{\Vert I_1\left(x_1\right)-I_2\left(x_2\right)\Vert }^2$$
式中，$x_1=\pi (X)$，$x_2=\pi (K(RX+t))$。关于这个方程的求解通常采用非线性优化的方法（如高斯牛顿法、LM方法）进行迭代求解。

根据使用点的个数的不同，直接法还可以进一步划分为以下几种。

1）系数直接法

仅采用少数几百个或几千个关键点来进行运动估计，并且同光流法计算类似，需要假定这些关键点周围的像素点也是灰度不变的。同特政法相比，由于不需要进行描述子计算来进行特征匹配，所以稀疏直接法的速度通常较快，但同特征点法一样只能重建稀疏的结构。稀疏直接法比较有代表性的工作有SVO和DSO。SVO提取了一些角点作为关键点，而DSO则没有采用传统的特征点，而是将图像划分成网格，再从每个网格中选取梯度变化最显著的点作为关键点，由于点的个数并不是很多，所以仍然可以看做稀疏直接法。

2）稠密直接法

使用图像上所有像素点进行运动估计，计算复杂度比较高。由于图像上梯度变化不明显的点在优化过程中能够提供的贡献是非常有限的，而且重构时也难以计算其三维位置，因此在实际使用中很少会采用这种方法。稠密直接法比较有代表性的一个工作是DTAM（Newcombe et al., 2011b），为了达到实时，DTAM使用了GPU加速。

同特征点法相比，直接法不需要进行特征的提取、描述子计算和特征匹配，能够处理特征比较弱场景，能够恢复半稠密甚至稠密的场景结构。但是直接法仍然存在一些缺点：

（1）首先直接法非常依赖于灰度不变假设，而灰度不变假设比较容易受到光照变化的影响，相比之下特征点法对于光照的鲁棒性就要好很多；

（2）如果只考虑单个像素，相似的像素点通常很多，因此为了降低这种不确定性，通常是采用以梯度变化比较明显的像素点附近的局部窗口块来进行计算，而这无疑会增加计算的复杂性：同时，窗口内像素点也并不总是满足同样的运动模型，因此实际上通常是采用少数服从多数的原则；

（3）直接法的求解过程采用的是梯度下降方法，而目标函数采用的是图像的像素点灰度值，由于图像是一个高度非凸的函数，因此求解容易陷入局部最小。