视觉里程计（VisualOdometry）原理及实现

一、视觉里程计(VisualOdometry)介绍

目前，有不止一种方式可以确定移动机器人的轨迹，这里将重点强调“视觉里程计”这种方法。在这种方法中，单个相机或者双目相机被用到，其目的是为了重构出机器人6-DOF的轨迹。

视觉里程计通过分析一系列图像序列，来确定机器人的朝向和位置。

二、单目(monocular)与双目(stereo)视觉里程计比较

双目VO的优势在于，能够精确的估计运动轨迹，且具有确切的物理单位。在单目VO中，你仅仅只知道物体在x/y方向上移动了1个单位，而双目VO则可明确知道是移动了1cm。但是，对于距离很远的物体，双目系统则退化成为单目系统，如果说我们的机器人很小，那么双目看起来似乎与单目无异，所以现在也有很多人只关注、研究单目VO。所以，究竟使用哪种视觉里程计方法，取决于我们的应用场景。

三、双目视觉里程计数学原理

Input:左右相机图像序列，时刻t和t+1时采集的图像表示为I^t和I^t+1，相机内参文件已提前标定好。

Output:针对不同帧图像对，需要估算出两帧图像之间的旋转矩阵R和平移向量T。

算法提纲：

1.捕捉图像对I^t，I^t+1；

2.对以上图像进行畸变校准；

3.计算时刻t和t+1的视差图；

4.使用FAST算法检测图像I^t中的特征，并与图像I^t+1中的特征匹配；

5.利用深度图，计算上一步中匹配特征点的3D坐标值，此时，获得两组点云数据；

6.选取点云数据中的一部分子集，使得所有的匹配都是互相兼容的；

7.估算上一步检测中的内点(inliers)。

Undistortion,Rectification

在计算深度图之前，为了补偿镜头带来的畸变，通过相机标定以及校准等过程，可以得到去畸变的图像对，图像对通过校准后，同一目标在左右图像中的水平位置是对齐的。

Stereoimages overlayed from KITTI dataset, notice the feature matches arealong parallel (horizontal) lines

DisparityMap Computation

关于深度图重建方法此处不赘述，前面开发的SGBM与BM算法皆能够较好实现深度图重建。

Adisparity map computed on frames from KITTI VO dataset

FeatureDetection

使用高效的FAST特征代替SIFT做特征检测。假设需要检测一个点是不是角点，那么我们就在这个像素附近画一个圆，判断圆周上面的16个像素点的强度，如果有连续的一组像素的强度大于原始像素点的强度，另外有一组连续的像素点强度小于原始像素点的强度，那么，可标记该原始点为角点。一种启发式的方法用来拒绝非角点，只需通过判断1,5,9,13像素强度，至少有3个点的强度大于或小于原始像素值，那么就有可能是角点。

Image from the original FAST feature detection paper

为了整幅图上都有特征表达，选取一种“桶”特征的方式对全图进行表示，例如，将整幅图像分割成100x100pixel大小的网格，在每个网格块中，获取至少20个以上特征，这样整幅图像中就能够维持一个比较均匀的特征分布。

Feature Description andMatching

使用KLT方法跟踪上一步计算得到的角点特征，通过寻找每个角点进行跟踪。

Triangulation of 3D PointCloud

同一目标点的万维坐标计算也不必多说，利用标定参数文件，重投影矩阵Q即可计算结果。

The Inlier Detection Step

该算法不同于其他算法的是，不存在异常值检测步骤，但是存在正常值检测步骤。从原始的点云数据中，希望能够选取出最大的一个子集合，里面所有的点都是一致的，那么这个问题等价于最大团问题(MaximumClique Problem )

Computation of rotation matrixR and translation vector T

目前最常用的求解R,T的思路是先用RANSAC方法求出内点的集合，即把误匹配的外点(离群点)筛除，然后对内点的集合，采用线性最小二乘法去求得R,T的初值，代入初值并利用L-M非线性优化方法求得经过优化后的RT，作为最终值。

参考博客地址：Avi Singh's blog