OV2SLAM : A Fully Online and Versatile Visual SLAM for Real-Time Applications论文笔记

OV2SLAM : A Fully Online and Versatile Visual SLAM for Real-Time Applications

论文笔记

系统框架：四线程：前端、建图、状态优化和回环检测

其中，回环检测部分是基于the online Bag-of-Words (BoW) algorithm iBoW-LCD
相关论文 [ ibow-lcd: An appearance-based loop-closure detection approach using incremental bags of binary words]

---

OV-2 系统结构框图：

图中，红色框内为关键 函数/线程；蓝色框内为关键帧相关的函数；橙色线为可选项。

视觉前端 Visual Front-End

以相机的帧率对相机位姿进行估计（ORB-SLAM是抽取关键帧的方式，所以处理速率低于帧率）

前端任务：

• 图像预处理（image pre-processing ）
• 关键点追踪（keypoint tracking）
• 离群值滤波（outlier filtering）
• 位姿估计（pose estimation）
• 触发条件，创建关键帧（keyframe creation triggering）

前端输入通道完全为单目通道，将其所有操作限制在双目摄像机的左侧相机提供的帧中

图像预处理

通过CLAHE进行对比度增强，这既增加了动态范围，又限制了曝光适应引起的强度变化。

关键点追踪

关键点跟踪是通过引导的从粗到精的光流法来执行的。使用具有9×9像素窗口和金字塔比例因子为2的反向组合Lucas-Kanade(LK)算法[12]的金字塔实现来单独跟踪关键点。

跟踪过程取决于关键点的性质。对于2D关键点-即那些没有关于其真实3D位置的先验信息的关键点-初始位置被设置为它们在前一帧中的位置，我们使用四层金字塔。对于3D关键点(即已经三角化的3D关键点)，其初始位置是根据投影模型π、其3D位置λwk和使用恒速运动模型对当前姿势Twci的预测来计算的。我们将最小化过程限制在图像金字塔的前两层(即分辨率最高的层)。

然后在四层金字塔上搜索我们在此步骤中未能跟踪的关键点，以及2D关键点。这两个阶段的过程减少了跟踪运行时间，同时对造成预测误差或不准确的3D地图点具有很强的鲁棒性。

离群值滤波

我们采用基于核线约束的RANSAC滤波

为了提高滤波器的效率 仅使用3D关键点估计基础矩阵（Essential Matrix）然后用它来过滤不一致的2D关键点

位姿估计

通过使用稳健的Huber成本函数最小化3D关键点重投影误差来执行姿态估计

非线性优化：Levenberg-Marquardt 列文伯格-马夸尔特法（数值分析中会学到，和牛顿高斯法都作为用来解最小二乘问题的经典方法）

对于初始值的选择，丛恒速运动模型预测的位姿开始，由于运动模型不充分，可能会使预测出现错误。若出现错误，导致成功跟踪的关键点不到一半，将拒绝预测的位姿，在执行上述公式之前会使用P3P RANSAC进行新的位姿预测。

创建关键帧

主要是，如果跟踪的3D关键点的数量w.r.t.。最后一个关键帧低于阈值(跟踪的关键点少于85%)，或者如果检测到明显的视差(平均15个像素的未旋转关键点运动)，则会创建新的关键帧。 （这句话没理解）

新关键点的检测采用网格策略，使用35×35像素的单元格。我们处理空单元格的方法是：选择单元格中具有最高Shi-Tomasi分数[16]的点，然后计算其位置的亚像素精化。然后为所有(先前跟踪或新检测到的)关键点计算BRIEF描述子[17]。此时，keyframe creation triggering线程进一步处理创建的关键帧，并在下一帧继续其操作。

初始化

在单目情况下，本文在最开始创建的两个关键帧中计算基础矩阵Essential Matrix。然后从基础矩阵E中提取一个相对位姿，选择一个任意的尺度并设置当前关键帧的姿态。Mapping线程使用提取出的姿态来初始化三维地图。

建图线程 Mapping

主要处理两个任务，通过三角化从新的关键帧中创建新的3D地图点，为了最小化漂移追踪当前局部地图。这两个任务的优先级不同，优先进行三角化，它对于在前端中保持精准的位姿估计是很关键的。

首先进行三角化，然后执行局部地图追踪，若有新的关键帧可用，则中止操作。

双目匹配

2D和3D关键点都在正确的视图（right view 好像是右侧视图）中被追踪，以便在未来的BA中同时进行三角化和创建额外的双目约束。

对于3D关键点，我们只需使用估计的关键帧的位姿，将它们投射到右侧视图中。

对于2D关键点，如果周围单元格中至少可以找到3个3D关键点，则我们使用相邻3D关键点的深度来初始化估计，否则使用左侧图像像素位置。

然后，通过移除 未失真的坐标与其对应的极线相差超过2个像素的坐标匹配，来过滤找到的立体匹配。

暂时/当时三角化？Temporal Triangulation

通常用于单目初始化3D地图，本文发现在双目情况下可以用于在当前关键帧之前，已经正确追踪到，但找不到双目匹配的关键点。好处是双重的，

所有成功三角化的地图点立刻应用于前端进行定位，然后这些3D位置通过BA进行重新优化。

局部地图追踪

类似于ORB-SLAM2局部地图，局部地图包括当前关键帧或共视图关键帧观察到的3D地图点。

局部地图追踪的目标是找出属于局部地图的3D地图点是否可以与当前帧的关键点相匹配。 这种重追踪的操作可以看做是基本的回环检测。

任意这样的3D地图点，在当前关键帧的投影与一个关键点的距离少于两个像素点时被定义为候选匹配。计算3D地图点和其他几个候选点的距离，接受最近距离的候选。

状态优化线程 STATEOPTIMIZATIONTHREAD

使用局部BA来精炼已选择的关键帧位姿和3D地图点位置，此外过滤冗余的关键帧来限制外来局部BA的运行时间。

局部BA优化

局部 ba 通过对局部地图进行多视点优化来限制来自视觉测量噪声的漂移。

和ORB-SLAM2相似,利用共视图，并在BA中包括与最近的关键帧共享至少25个共同观察的每个关键帧。每个被这些关键帧观测到的3D地图点也会被包含在BA中。

3D地图点被参数化为逆深度的固定点

3D map points are parameterized as anchored points with an inverse depth [18], [19], that is a 3D map pointλwk is defined by its undistorted pixel positionxαkin its first observing keyframeTwαand its inverse depthγkrelative to this keyframe：

BA的成本函数由2D重投影误差组成，在双目的情况下，额外的成本/代价对应于右侧相机的观测为：

其中Ki为对BA有贡献的全部关键帧集合，Li是由Kj观察到的ζ中的映射点的集合，并且如果在右摄像机中也观察到映射点，则β_j被设置为1，否则被设置为0

非线性最小二乘问题在manifold 上使用用Levenberg-MarQuardt算法求解。然后，如求解等式(1)之后那样，去除由鲁棒Huber函数标记的离群值，并且更新优化状态。

关键帧过滤

删去关键帧的标准为：该关键帧观测到的95%3D点已经被至少其他4个关键帧观测到

在线词袋回环检测器

回环检测主要负责检测回环和重定位，即校正当前姿势估计和当前帧与已检测到LC的已传递关键帧之间的估计轨迹。

在线词袋（ORB-SLAM2使用的是离线Bow即加载一个训练好的词典文件）

大多数SLAM算法是基于线下训练的BoW，因此会根据训练数据的不同产生偏差。

另一种方式是建立在线词汇树。在线词汇树引用相关论文如下：

[Fast and incremental method for loop-closure detection using bags of visual words]

[Automatic visual bag-of-words for online robot navigation and mapping]

主要思想是创建一个适合当前环境的词汇树。OV2SLAM中选择的是一种 iBoW-LCD的变更版本。我们坚持原来的实现来查找回环候选，但是执行我们自己的处理来评估该候选的正确性。（具体看代码）

关键帧预处理

在接受关键帧之后，进行一次特征提取。由于没有提取到特定数目的可以用于定位的关键点，在更新词汇树和搜索回环候选之前提取额外的特征。使用FAST检测器和BRIEF描述子提取特征，保留最好的300个特征。然后使用SLAM特征和新提取的特征更新词汇树，并使用生成的BoW signature与之前的关键帧进行相似性得分计算。

回环候选处理

发现一个好的LC候选，首先确定不是一个假阳性。对当前关键帧Ki和候选关键帧Klc，首先应用一个k最近邻暴力匹配算法在这两个关键帧的描述子中。而不明确的匹配首先由经典的ratio test进行过滤。然后使用RANSAC方法进行计算基础矩阵Essential Matrix 以便只保持满足极线几何（对极几何）的匹配。

使用这种内在维持方法，我们使用P3P-RANSAC方法用回环候选关键帧Klc观测到的剩余3D地图点对当前关键帧Ki进行一个假设位姿计算。若计算出一个可靠位姿，由大量内参器（inliers）确定，我们得到候选关键帧Klc的局部地图，然后搜索在当前关键帧Ki的额外的匹配，将3D地图点的投影使用P3P的方法计算位姿。

然后，使用等式(1)基于到目前为止发现的所有匹配来精炼假设位姿，并且基于由Huber鲁棒成本函数检测到的离群值来执行最后的过滤步骤。

位姿图优化

最终如果保留30个内点，我们验证回环检测，并执行位姿图优化(PGO)来矫正完整轨迹。

PGO是从Klc到Ki的轨迹部分执行的，并且寻求最小化给定其初始相对位姿(即，LC检测之前的关键帧)和新估计的Ki姿势的连续关键帧之间的相对位姿误差。

对这些关键帧的位姿执行Levenberg-MarQuardt优化，并且我们使用Campbell-Baker-Haussdorf公式[23]的二阶近似来计算它们各自的Jacobian se(3)。

由于自当前LC检测开始以来可能已经创建了新的关键帧，因此我们将姿势图校正传播到轨迹的最新部分，使用其先前的相对姿势和校正后的Ki姿势来更新关键帧。然后合并在KiandKlcare之间匹配的3D地图点，并且通过向前-向后投影来更新由任何校正的关键帧首先观察到的所有3D地图点，以对齐3D地图w.r.t。至LC更正：

Loose BA优化

PGO结束后，应用"Loose BA"，含义是仅对受回环检测矫正影响的地图部分进行BA优化（即校正的关键帧观察到的所有3D地图点以及观察这些3D点的所有关键帧)。

它以与PGO相同的方式完成，使用关键帧姿势之前的相对姿势传播校正，并更新它们通过公式(5)观察到的3D地图点。