计算机视觉大型攻略——视觉里程计(VisualOdometry)

参考文献：

[1] Visual Odometry Part I: The First 30 Years and Fundamentals,  Friedrich Fraundorfer and Davide Scaramuzza

[2] Visual Odometry Part II: Matching, Robustness, Optimization, and Applications, Friedrich Fraundorfer and Davide Scaramuzza

[3] Random Sample Consensus: A Paradigm for Model Fitting with Applications to Image Analysis and Automated Cartography

视觉里程计(Visual Odometry)

视觉里程计(以下简称VO)使用相机拍摄的连续图像，估计相机的姿态(pose)变化。

用视觉算法解决这类问题，通常对应用场景和环境有一些要求，

• 环境中光线充足
• 场景中静态物体占多数
• 纹理特征明显
• 连续帧足够多的重复特征

VO可以作为轮速仪，GPS导航仪，惯导，激光里程计的有效补充。

[1]中给出了算法流程，  

首先输入图像序列，然后检测特征点及匹配，估计运动，最后优化结果。

问题定义

假设刚性连接的相机在时间序列下获得n组图像，

  左图                       

  右图（双目)           

相邻两帧的运动定义为，

k-1帧，k帧之间的刚体变换定义为，用旋转平移矩阵表示，

定义  为相机的pose。 即与初始k=0帧的相机pose的刚体运动变换。

其中， ，  是k=0帧的pose。注意到计算Cn的时候是把Cn-1级联乘进去的，这样Cn-1的误差会被积累到Cn。后续Local Optimization采用了许多算法（如bundle adjustment)来减小此误差。 

如上图所示，VO算法的目的是计算，进而获得相机的完整轨迹  。

关于积累误差，如下图所示，的误差包含了的误差(黑色实心椭圆)和的误差（灰色虚线椭圆）

特征检测与匹配

输入图像序列后，算法首先要做的是特征点检测与匹配。

该算法有两大类

• Appearance-base的方法。使用两幅图像所有的像素的信息。
• Feature based的方法。只考虑特征点的匹配。

显然，第一种方法在精度和计算效率上都不如第二种方法。VO算法通常采用基于特征的方法，比如SIFT, SURT, ORB等。关于特征点与匹配的更多内容可移步我的专栏。

传送门：特征与匹配专栏

运动估计(Motion Estimation)

运动估计算法是VO算法的主要部分。算法在这一步计算两帧之间的刚体运动变换。

  其中，                    

假设表示了特征的相关性。在第k帧上，根据，的不同， 运动估计可分为三类，

• 2D-to-2D

如上图，，都是2维图像上的信息。

• 3D-to-2D

 为3D，为2D。

• 3D-to-3D

参考文献[2]中对以上三种算法做了详细说明。

局部优化

先看主要误差来源，

• 图像本身的噪声，遮挡，光线变化等。
• 相机模型和校准误差。
• 特征匹配算法的误差。

首先，这些误差导致了大量的外点（错误匹配）。此外，由于pose是逐帧相乘的结果，，下一帧积累了上一帧的误差，因此整个轨迹的累积误差较大。

因此局部优化的主要目标就是

1. 去外点（错误的匹配点）
2. 消除积累误差

RANSAC(Random Sample Consensus)

VO的运动估计算法引入RANSAC[3]去除上面所说的外点。

上图以直线拟合为例，解释了RANSAC算法。

• 随机挑选两个点
• 根据这两个点拟合一条直线
• 计算所有点距离此直线的距离
• 选择距离小于一定距离的点作为内点
• 重复k次
• 选择内点最多的模型，重新拟合。

类似的，RANSAC在运动估计中的应用

• 随机挑选一组相关点对。
• 计算刚体运动变换和内点个数，保留内点。
• 重复N次
• 采用含最多内点的组重新估计运动。

关于迭代次数N，

• s是模型所需要的点的个数
• 是外点的比例
• p是成功的概率

相机pose误差一直增长

基于Pose-Graph的优化

通过定义图G(V,E)模型，优化代价函数，可以改善积累误差。

图G(V, E)的节点为每个时刻的相机pose()，把边定义为pose之间的刚体变换()。

代价函数定义为，

刚体变换T中的旋转量R导致了代价函数的非线性，需要采用非线性优化算法优化(如Levenberg-Marquadt)。

基于窗口的Bundle Adjustment(BA)优化

与Pose-Graph类似，Bundle Adjuatment同样通过优化代价函数来优化pose。它适用于在多幅图像上对同一特征跟踪的情况。

假设第k帧上有点P。找到P在第i帧上匹配点P'，P'的坐标为。 是第k帧上点P的3-D坐标。通过函数g和相机Pose C，可以把映射到第i帧的图像上(计算的出第i帧上的图像坐标)。算法最优化两者的差，以达到减少整体误差的目的。同样是非线性优化问题，可采用Levenberg-Marquadt算法优化。