一、前言
视觉里程计与传统的里程计不同，不使用码盘等设备，只利用摄像头拍摄的连续图像帧就可以计算里程，非常方便，因而用途广泛。本文介绍SLAM中的视觉里程计的设计与实现。
有了前面的基础，我们就可以着手做一个简单的视觉里程计（Visual Odometry，VO）。VO作为SLAM系统的前端，负责接收图像序列，通过特征匹配等方法估计相机在相邻帧间的运动，从而获得累积的里程信息。
二、VO系统框架
首先，让我们来看看VO应该由哪些模块组成。下图为VO系统框架图。
- Frame代表每一帧图像，存储着拍摄该帧图像时的相机位姿、图像的彩色图和深度图、以及是否为关键帧。
- Camera代表相机模型，与实际拍摄时的相机对应，只包含内参数，不包含外参数，因此在整个VO过程中只需要一个Camera对象。
- Map代表一个局部地图，既包含关键帧又包含路标点。关键帧和路标点会根据适当的规则添加进地图中。但需要注意，这里的地图是局部地图而不是全局地图，只包含了当前位置附近的路标点，距离更远的路标点会被删除。
- MapPoint代表路标点，又称为地图点。这些点都是通过特征提取筛选出来的点，因而包含了特征描述符。由于同一个特征点会被多个帧观测到，因此也包含了这些能观测到该路标点的帧的指针。同时，为了控制地图规模，记录了路标点匹配成功的次数和被观测到的次数，两者比值一旦过小就丢弃该路标点（因为该点出现次数很多但对VO没有帮助）。
有了这几个模块，一个简单的VO就可以搭建起来了。
三、VO算法流程图
接下来我们详细介绍VO算法的流程。以最简单的RGBD-VO为例，避免复杂的初始化过程。
首先，程序启动后等待第一帧抵达，执行初始化操作。这里的初始化只需要将第一帧设置为关键帧，同时把该帧中观测到的所有路标点添加进地图。
后续帧抵达后，提取关键点，计算描述子，与地图中的路标点匹配。与地图中的路标点匹配是为了提高匹配的成功率，如果只与上一帧或上一关键帧匹配的话很容易导致匹配结果太少。
匹配成功后，执行PnP位姿估计。这里使用的是RANSAC PnP加上非线性优化的方式估计相机位姿，使用g2o构造图优化问题。
接下来判断PnP位姿估计是否成功。如果内点个数过少或者估计出的运动距离过大，则认为位姿估计失败。如果成功则优化地图点并决定是否添加当前帧为关键帧，如果失败则结束VO。其中，优化地图点是一个删除距离较远或无效点的过程，通过判断路标点匹配成功的次数与被观测到的次数之间的比值是否低于某阈值，从而决定是否删除这个点。总之，优化地图点的目的是使地图规模不至于太大且只包含与当前帧最接近的局部环境。
整个流程如下图所示。
四、运行效果和存在的问题
使用TUM数据集rgbd_dataset_freiburg1_desk测试效果如下。
左图为当前视频帧及局部地图中路标点的投影，右图为世界坐标系和当前帧的相机坐标系位置。从后三张图可以发现，特征点位置出现了一些明显的偏差，这可能是由于相机位姿估计不准确造成的。说明这个简单的VO在运动过快的情况下产生了漂移。
在完整的SLAM中，既有前端的VO，又有后端优化和回环检测，从而可以在一定程度上降低VO的漂移问题。我们将在后续文章中一一介绍。
本文的VO例程完整代码地址：https://github.com/jingedawang/VO
该代码依赖于OpenCV 3.2.0，opencv-viz，Sophus，g2o，Eigen等库，请特别检查viz模块是否正确安装。另外，如果Sophus库找不到，请手动编写FindSophus.cmake。
五、参考资料
《视觉SLAM十四讲》第9讲 实践：设计前端 高翔
