视觉里程计总介绍

转载地址：冯兵视觉里程计总介绍

在前两篇博客中，我们实现了一个简单的视觉里程计，在最后我们也分析了其存在的问题，那接下来的文章就是对上述简单视觉里程计一步步优化，提高算法的精度，效率以及鲁棒性。
在此之前，我们先对视觉里程计整个发展做一个回顾。具体可以参考大牛Davide Scaramuzza的两篇文章Visual Odometry: Part I[1],Visual Odometry: Part II[2].
英语也不是很好，就不对这两篇文章进行翻译了，主要对这两篇文章进行一下回顾。

• 相比普通的轮子里程计，不会因为轮子打滑，或者斜侧等原因导致结果误差较大
• 相比轮子里程计，更加精确(相对位置误差0.1%-2%)
• 可以作为轮子里程计，GPS，IMU，激光里程计等补充
• 在GPS无法涉及的环境，比如室内，水下，太空等环境VO显得比较重要。工作原理简介

具体架构

其计算相机轨迹是通过递增的方式，具体如图：

VO 与 Structure from Motion (SfM)

• SfM是从一组非畸变图像中恢复场景三维结构和相机姿态。其对图像采用了一个全局的离线优化，计算复杂度就会随着图片的增加。
• VO是SfM中的一种特殊情况，其基本思想与SfM一致，而其采用了局部优化，实时给出相机姿态信息。

VO 与 Visual SLAM

• 基于视觉的SLAM的目标是为了获得一个全局稳定的机器人路径，具体思路可以参考ORB_SLAM
  ，如下图：
  
• VO的目标是为了获得局部稳定的轨迹，而SLAM的目标是为了获得全局稳定的轨迹及地图信息

• VO可以作为SLAM中的一部分，也就是在闭环检测之前。

  问题求解

  具体整个问题的求解，前面两个博文中已经简单介绍了一个具体思路，对于问题的核心姿态估计是通过估算两幅图像的本质矩阵，对本质矩阵进行SVD分解。那有没有其他思路了呢，具体根据特征是2D还是3D，可以分成如下三种方式：

• 2D-to-2D

• 3D-to-3D
• 3D-to-2D

2D-to-2D

这种方式就是我们前面博文中介绍的方式。
具体问题就是：我们已知两幅图像中的匹配，计算两幅图像之间的TkTk:


两幅图像之间满足对极几何的性质：

根据5点法或者8点法可以求解本质矩阵，后通过SVD分解计算相机旋转关系。
对于2D-to-2D的方式存在尺度的问题，也就是不能通过两幅图像计算出平移的绝对尺度。其中一种方式通过计算两个时刻，两幅图像对应3D点之间的距离，具体如下：

i,j代表图像对，k,k−1代表时刻，X代表特征对应的3D点。
为了鲁棒性考虑，一般会计算这个时刻所有对应特征计算的r,计算所有r的均值或者使用所有r的中值。

3D-to-3D

这种方式一般用于双目，具体示意图如下：

具体优化通过：

X˜X~表示通过双目进行3角定位获得的3D点。

3D-to-2D

对于这个问题就是PnP问题，OpenCV中就有方法solvePnP以及solvePnPRansac，具体优化通过：

p^ik−1p^k−1i为上一帧特征pik−1pk−1i对应3D点投影到当前帧的特征。
对于目前单目情况，这个3D点需要根据相连两帧来确定，然后将得到的3D点投影匹配到第三幅图像中。

我们确定3D点的时候，有如下几个因素影响这个3D点的准确性。
1、图像噪声
2、相机标定误差
3、特征匹配的不确定性
因此我们将到所有交叉光线的最小距离的点作为确定的3D点，具体如图：

我们会发现，两帧的距离很近的时候会影响3D点的准确度，具体如图：

这样为了避免出现跳动的情况，我们对两帧的距离设定阈值，如下图：

关键帧的选择比较重要，后期对VO进行优化的时候会添加关键帧

我们之前计算相机变换只是考虑了相连两帧，而我们可以考虑计算非相连的帧计算的结果作为约束条件。具体图如下：

通过如下式子进行优化：

为了考虑下面，这边只计算最近的K个关键帧，具体求解可以通过Levenberg-Marquadt算法进行优化。
在对姿态进行优化后，另外对点进行优化。

具体式子：

pikpki表示的是3D点XiXi在图像kk中的对应点，g(Xi,Ck)g(Xi,Ck)表示3D点XiXi重投影到当前相机姿态CkCk上。这个的优化也可以通过通过Levenberg-Marquadt算法。不过这边为了不陷入局部最小值，应该给出的初值计算结果应接近最小值。

闭环检测对于图优化来说，是非常有用的约束条件，这个在Visual SLAM中更为常见。

通过对VO整个过程的分析，接下来就根据上述分析过程一步步实现高精度，高效率，高鲁邦的VO算法。

[1]: Scaramuzza, D., Fraundorfer, F., Visual Odometry: Part I - The First 30 Years and Fundamentals, IEEE Robotics and Automation Magazine, Volume 18, issue 4, 2011.
[2]: Fraundorfer, F., Scaramuzza, D., Visual Odometry: Part II - Matching, Robustness, and Applications, IEEE Robotics and Automation Magazine, Volume 19, issue 2, 2012.