DVO-SLAM详解

一、整体框架

本系列的文章主要解释DVO-SLAM的Tracking和闭环检测部分,首先从benchmark_slam.cpp这个文件入手，整个算法框架如下：

配置好相关参数后，程序循环读取数据集中的图片，开始运行算法，如下图所示：

整个算法流程如下：

（1） 配置相关参数，读取所有的rgb图及其对应深度图的保存路径

（2） 创建KeyframeTracker对象

（3） 循环读取数据

    （3.1) 加载图片

    （3.2) 将图片及其时间戳传给KeyframeTracker对象，返回该图片对应的相机位姿

    （3.3） 保存相机位姿

（4） 全局进行一次闭环检测，进一步完善位姿图，再对位姿图进行优化。

二、KeyframeTracker对象的创建

    主函数中创建了KeyframeTracker对象，程序进入keyframe_tracker.cpp文件，这个文件中包含了KeyframeTracker对象的所有接口，KeyframeTracker对象主要包括两部分：KeyframeGraph对象和LocalTracker对象，KeyframeGraph对象主要负责后端位姿图的搭建、闭环检测、位姿图优化，LocalTracker对象主要负责前端位姿估计。

三、KeyframeTracker的运行

       KeyframeTracker对象每读取一张图片，就对图片进行如下操作：

（1） 将当前图片与前一帧以及关键帧之间进行帧间匹配，得到帧间运动变换量r_odometry、r_keyframe

（2） 根据r_odometry、r_keyframe判断当前帧是否为关键帧，若是则更新关键帧，进行局部闭环检测，搭建位姿图，否则直接将结果添加到位姿图中。

（3） 根据r_keyframe得到当前相机的位姿

跟踪

    DVO-SLAM的跟踪通过将当前帧与关键帧进行帧间匹配进而确定当前帧对应的相机位姿来实现，本文将从帧间匹配和关键帧选取这两部分解释DVO-SLAM跟踪的具体操作流程。

一、帧间匹配

      帧间匹配即计算两帧图像间的相机运动变换矩阵，DVO-SLAM中用并行计算的方式同时计算当前帧与前一帧及关键帧间的运动变换矩阵r_odometry、r_keyframe，相关代码在local_tracker.cpp中。

（1） 选取参考帧中灰度梯度及深度梯度值较明显的点作为参考点，这里的参考帧指当前帧的前一帧和当前帧对应的关键帧。

（2）理想情况下，存在一个帧间运动变换量，使得参考点的灰度值、深度值与其投射到当前帧中的点的灰度值、深度值差为零。实际应用中，通过最小化残差函数来求帧间运动，DVO-SLAM中使用高斯牛顿法迭代求解这个问题。

（3） 将上一步求到的帧间运动变换矩阵、信息矩阵、似然估计值保存到结果当中，为之后的关键帧选取、闭环检测做准备。

   高斯牛顿法求解帧间运动变换的具体流程如下图所示：

（1） 累积迭代增量，作为本次迭代的初始值。

（2） 计算各点的残差。

（3） DVO-SLAM中假设残差服从t分布，因此这里根据t分布计算个残差的权重。

（4） 计算t分布的方差参数。

（5） 计算残差的似然估计。

（6） 求本次迭代增量。

二 、关键帧选取

     之所以将每帧图片与其关键帧进行匹配进而确定相机位姿，是因为这样一定程度上可以避免相邻帧匹配带来的误差累积。DVO-SLAM中的关键帧选取原则如下：

（1） r_Keyframe的后验概率估计反应了r_Keyframe的可信度，当可信度低于某一阈值时则重选关键帧。

（2）DVO-SLAM使用直接法进行帧间匹配，该方法基于灰度一致假设，所以r_Keyframe和r_odometry的位移不能过大，当位移超过某一阈值则重选关键帧。

（3） ValidConstraints / ValidPixels反应了落在当前帧中的参考点相对于参考点总数的比例，比例越高说明相机的位姿变换越小，越低说明位姿变换越大，当比值低于某一阈值则重选关键帧。

闭环检测

    DVO-SLAM的后端用图优化的方式对位姿图进行优化，首先需要搭建位姿图，在关键帧更新之前，将每帧对应的相机位姿作为节点，相邻帧间以及每帧与其关键帧间的运动变换矩阵作为边，运动变换矩阵的信息矩阵作为边的信息。当检测到新的关键帧时，则检测与先前关键帧间是否存在闭环，若存在则作为位姿图的边加入位姿图。

一、局部闭环检测

    DVO-SLAM中局部闭环检测的代码在keyframe_graph.cpp文件的insertNewKeyframe（）函数中，具体流程如下图所示：

（1） 将上一个关键帧及以其为关键帧的帧搭建的局部位姿图优化后添加到全局位姿图中。

（2） 在已有关键帧中搜索离新选取的关键帧距离一定范围内的帧，作为候选帧。

（3） 在候选帧中选取满足一定条件的帧，与新的关键帧构建边加入全局位姿图中。

上文第三步中的条件，包括如下几条：

二、全局闭环检测

    局部闭环检测只检测新选取的关键帧之前的关键帧，在读取完全部图片后，针对每个关键帧，在全部关键帧中寻找闭环。