从零编写一个双目视觉SLAM系统（一）：一个滑窗的PnP里程计

    最近在尝试用光流法写一个基本的双目视觉SLAM，希望通过这个项目把SLAM的各个模块都自己实现一遍，以此加深对SLAM各个模块的理解。
    目前刚完成了一个通过求解相邻帧PnP问题的双目视觉里程计，基本有了一个粗糙的效果，后续还需要把Bundle Adjustment、边缘化、回环检测和位姿图优化加上去。回环检测暂时设想用ORB来做。
    在这里把这个初步的里程计的思路记录一下。
代码链接

1.前端

    前端维护了一个大小为N的滑动窗口，我的代码里默认N=10。滑窗中记录了左相机位姿和一系列特征点。当接收到一对新的双目图像时，首先对上一帧左图像到当前帧左图像进行光流跟踪，去除外点后计算平均视差，如果平均视差太小，丢掉当前帧，继续接受下一帧。

1.1 特征点跟踪

    这里使用KLT光流对特征点进行跟踪，相比于利用描述子对特征点进行匹配，该方法可以节省大量运算时间。这里的特征点跟踪分成两类：

1.1.1 左右图像的光流跟踪

    对左右图像的光流跟踪主要是为了获取特征点的深度。得到深度的公式为：
d e p t h = B f x d depth=\frac{Bf_x}{d} depth=dBfx​​
    其中 B B B为左右相机的基线长度， f x f_x fx​为焦距， d d d为视差。这里要稍微关注一下左右图像光流跟踪时去除外点的方法。因为光流跟踪会出现一些错误跟踪的点，如果不去掉错误跟踪的点，会给求解位姿带来很大的误差。我在上一篇博客里写了2种去除外点的方法，后来我又想了一种比较简单的方法，即通过比较匹配点在y坐标上的差值来去除外点。因为对于标定好的双目图像而言，正确匹配的特征点在y坐标上相差极小，因此我们可以设置一个阈值，把超出阈值的去掉。

1.1.2 左相机相邻帧光流跟踪

    这个好像没什么好讲的，就是用一个cv::findFundamentalMat去除外点。

1.2 初始化3D结构

    当接收到的图像数达到窗口大小N时，进行初始化3D结构，本质上就是求解PnP问题来计算各帧的位姿。这里我想了两种方案：
    a.选择窗口里的某一帧 l l l，并把 l l l帧的参考系作为世界坐标系，计算 l l l帧观测到的所有特征点的3D位置，然后求解窗口中其他帧的位姿。这里 l l l帧需要跟其他帧都有一定数目的共视点，共视点太少会导致求解的结果有比较大的误差。而且在求解时选择solvePnP还是solvePnPRansac也需要推敲，在我的实验里，用solvePnP求解离 l l l帧较远帧的时候会出现较大误差，而选择solvePnPRansac则能基本恢复整个3D结构。
    考虑这个方法时我考虑的是尽可能减少误差的累积，因为如果是相邻两两帧求解位姿会带来一定程度的误差累积。然而实验结果也不算太理想，有极个别位姿在y轴上有些浮动，由于我用的是KITTI数据集，图像是车子在马路上跑拍摄的，正前方为车子的Z轴方向，左右为X轴，上下为Y轴方向，理论上在y轴上的浮动应该极小才对。
    b.相邻两两帧求解位姿，用这个方法求出来的位姿会比上面的方法要平滑，但是我总感觉这样两两帧求解位姿对于后面的位姿会有较大的误差。具体误差情况怎么样还没量化分析，之后做Bundle Adjustment可以对比一下这2个方法的结果。

1.3 滑动窗口

solvePnP for adjacent frames