参考文献：

[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组图像，

左图 $I_{l,0:n} = \left \{ I_{l,0}, ... \; ,I_{l,n} \right \}$

右图（双目) $I_{r,0:n} = \left \{ I_{r,0}, ... \; ,I_{r,n} \right \}$

相邻两帧的运动定义为，

$T_{0:n} = \left \{ T_{1}, ... \; , T_{n} \right \}$

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

$T_{k} = \begin{bmatrix} R_{k,k-1} & t_{k,k-1}\\ 0 & 1 \end{bmatrix}$

定义 $C_{0:n} = \left \{ C_{0},...\; C_{n} \right \}$ 为相机的pose。即与初始k=0帧的相机pose的刚体运动变换。

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

如上图所示，VO算法的目的是计算 $T_{k}$ ，进而获得相机的完整轨迹 $C_{0:n}$ 。

关于积累误差，如下图所示， $C_{k}$ 的误差包含了 $C_{k-1}$ 的误差(黑色实心椭圆)和 $T_{k}$ 的误差（灰色虚线椭圆）

特征检测与匹配

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

该算法有两大类

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

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

传送门：特征与匹配专栏

运动估计(Motion Estimation)

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

其中， $T_{k} = \begin{bmatrix} R_{k,k-1} & t_{k,k-1}\\ 0 & 1 \end{bmatrix}$

假设 $f_{(0:n)}$ 表示了特征的相关性。在第k帧上，根据 $f_{k-1}$ ， $f_{k}$ 的不同，运动估计可分为三类，

2D-to-2D

如上图， $f_{k-1}$ ， $f_{k}$ 都是2维图像上的信息。

3D-to-2D

$f_{k-1}$ 为3D， $f_{k}$ 为2D。

3D-to-3D

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

局部优化

先看主要误差来源，

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

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

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

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

RANSAC(Random Sample Consensus)

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

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

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

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

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

关于迭代次数N，

$N=\frac{log(1-p)}{log(1-(1-\varepsilon )^{s})}$

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

相机pose误差一直增长

基于Pose-Graph的优化

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

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

代价函数定义为，

$\sum_{e_{ij}}|| C_{i}-T_{e_{ij}}C_{j}||^{2}$

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

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

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

$\arg \min _{X^{i},C_{k}} \sum _{i,k}\left \| p^{i}_{k}-g(X^{i}, C_{k}) \right \|^{2}$

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

计算机视觉大型攻略 —— 视觉里程计(1) 综述相关推荐

计算机视觉与深度学习 | 视觉里程计库Libviso2详解
================================================ 博主github:https://github.com/MichaelBeechan 博主CSDN:h ...
计算机视觉大型攻略 —— SLAM(2) Graph-based SLAM（基于图优化的算法)
前面介绍了基于EKF的SLAM算法.EKF算法由于状态向量,协方差矩阵的大小随着特征点(路标)的增长而迅速增长,导致其不太适合大场景的应用.本文描述基于图优化的SLAM算法.目前由于SLAM图的稀疏性 ...
计算机视觉与深度学习 | 视觉里程计综述（框架+算法）
============================================== 博主github:https://github.com/MichaelBeechan 博主CSDN:htt ...
计算机视觉大型攻略 —— 特征与匹配（3）特征描述符
接上一篇文章.这篇写特征描述符.特征匹配算法在确定角点后,还需要使用描述符来描述这些角点. 本文参考书籍:Computer Vision: Algorithms and Applications, R ...
计算机视觉大型攻略 —— CUDA(2)执行模型
Professional CUDA C Programming[1]是一本不错的入门书籍,虽说命名为"Professional",但实际上非常适合入门阅读.他几乎涵盖了所有理论部分 ...
计算机视觉与深度学习 | 开源SLAM、视觉里程计综述（SLAM、Visual Odometry）
视觉里程计综述引言 Visual Odometry or VSLAM OF-VO:Robust and Efficient Stereo Visual Odometry Using Points a ...
从零开始学视觉里程计——一个初学者教程
从零开始学视觉里程计--一个初学者教程目录从零开始学视觉里程计--一个初学者教程什么是里程计什么是视觉里程计为什么使用立体相机,或者为什么使用单目相机? 理论足够了,现在讨论算法问题描述 ...
opencv3计算机视觉python语言实现pdf_对比《OpenCV计算机视觉编程攻略第3版》《OpenCV 3计算机视觉Python语言实现第2版》PDF代码......
OpenCV 3是一种先进的计算机视觉库,可以用于各种图像和视频处理操作,通过OpenCV 3 能很容易地实现一些有前景且功能先进的应用(比如:人脸识别或目标跟踪等).从图像处理的基本操作出发,计算机 ...
360VO：基于单个全景相机的视觉里程计
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达作者丨paopaoslam 来源丨泡泡机器人SLAM 标题:360VO: Visual Odomet ...

计算机视觉大型攻略 —— 视觉里程计(1) 综述