视觉里程计的详细介绍和算法过程

视觉里程计（Visual Odometry）

在机器人学与计算机视觉领域，视觉里程计是一个通过分析相关图像序列，来确定机器人位置和朝向的过程。

在导航系统中，里程计（odometry）是一种利用致动器的移动数据来估算机器人位置随时间改变量的方法。例如，测量轮子转动的旋转编码器设备。里程计总是会遇到精度问题，例如轮子的打滑就会导致产生机器人移动的距离与轮子的旋转圈数不一致的问题。当机器人在不光滑的表面运动时，误差是由多种因素混合产生的。由于误差随时间的累积，导致了里程计的读数随着时间的增加，而变得越来越不可靠。

视觉里程计是一种利用连续的图像序列来估计机器人移动距离的方法。视觉里程计增强了机器人在任何表面以任何方式移动时的导航精度。

视觉里程计算法：

大多数现有的视觉里程计算法都是基于以下几个步骤：

1、图像获取：单目照相机、双目照相机或者全向照相机；

2、图像校正：使用一些图像处理技术来去除透镜畸变；

3、特征检测：确定感兴趣的描述符，在帧与帧之间匹配特征并构建光流场；

（1）、使用相关性来度量两幅图像间的一致性，并不进行长时间的特征跟踪；

（2）、特征提取、匹配（Lucas–Kanade method）；

（3）、构建光流场；

4、检查光流场向量是否存在潜在的跟踪误差，移除外点；

5、由光流场估计照相机的运动；

（1）、可选方法1：使用卡尔曼滤波进行状态估计；

（2）、可选方法2：查找特征的几何与3D属性，以最小化基于相邻两帧之间的重投影误差的罚函数值。这可以通过数学上的最小化方法或随机采样方法来完成；

6、周期性的重定位跟踪点；

视觉里程计算法（基本知识）：

大多数现有的视觉里程计算法都是基于以下几个步骤：

1、图像获取：单目照相机、双目照相机或者全向照相机；

2、图像校正：使用一些图像处理技术来去除透镜畸变；

3、特征检测：确定感兴趣的描述符，在帧与帧之间匹配特征并构建光流场；

（1）、使用相关性来度量两幅图像间的一致性，并不进行长时间的特征跟踪；

（2）、特征提取、匹配（Lucas–Kanade method）；

（3）、构建光流场；

4、检查光流场向量是否存在潜在的跟踪误差，移除外点；

5、由光流场估计照相机的运动；

（1）、可选方法1：使用卡尔曼滤波进行状态估计；

（2）、可选方法2：查找特征的几何与3D属性，以最小化基于相邻两帧之间的重投影误差的罚函数值。这可以通过数学上的最小化方法或随机采样方法来完成；

6、周期性的重定位跟踪点；

我选择的视觉里程计算法是：“ sift特征匹配点——基本矩阵——R和T”。

第一步：由特征点计算基本矩阵F。

一般而言，sift点是存在误匹配的情况，因此，采用ransac鲁棒方法计算基本矩阵F。这个过程已经实现，但是还有一个小问题：同样的一组sift点，进行两次基本矩阵计算，得到的基本矩阵差异很大，因此，我在ransac方法的基础上，根据得到的inliers点，采用常规的8点基本矩阵计算方法，这样得到的基本矩阵能保持不变

第二步：由基本矩阵计算R和T

方法1：奇异值分解

E = KK'*F*KK; %%这是真实的本质矩阵E

[U,S,V] = svd(E); %奇异值分解。
T_nonscale = U(:,3); %% 不含有刻度因子的平移向量
D= [0 1 0 ;-1 0 0; 0 0 1];
Ra = U*D*V';或者Rb = U*D'*V';

方法2：非线性优化解迭代求解。我尝试了三种不同的目标函数形式：

% RT_from_E_ydf.m; 物理意义不明显！精度和速度都不如后面的好。
% RT_point_constraints_ydf.m 速度折中，精度较高
% RT_point_constraints_ydf02.m 速度最快，精度不够高

发现的问题：特征点的误匹配问题。

误匹配对位姿解算结果影响分析：我们将手动选取的匹配点加上一个噪声后（1-2个像素的噪声），位姿误差很大，也就是说，要得到精确的位姿解算结果，噪声不能大于1个像素，即需要考虑亚像素级别的特征点匹配！！实验表明，用sift特征是一种比较合理的方法，但是，sift特征容易出现误匹配点的情况，将其进行剔除是很有意义的一件事情。

目前我采用的是手动选择匹配好的区域，从而选择比较好的匹配点，见hand_choose_sift_ydf.m。结果如下：

Image0053 & Image0056 (sift特征点误匹配率较高)	备注
剔除前（°）	172.1492	178.2707	-133.4679	×
剔除后（°）	-0.0774	10.2036	0.0192	√
真实值（°）	0	10	0
Image0053 & Image0054 (sift特征点误匹配率较低)
剔除前（°）	0.0780	-10.9056	-0.0013	√
剔除后（°）	-0.0133	-10.7892	0.2188	√
真实值（°）	0	-10	0

下一步研究sift特征点的自动选择算法。

一般而言，针对匹配点的鲁棒算法有M-estimators等，我这里选择的是最小中值法（least-median-squares）。程序见“RT_from_siftpoint_ydf03.m”，该方法运算量非常大，需要进行163（以保证能得到一组正确的样本，假设sift误匹配率为40%的情况下）组优化求解计算。Very time consuming

疑问，为什么横向选取特征点的时候，结算结果会出现很大的误差呢？

【横向选取Sift特征点和纵向选取sift特征点，会有不同。能不能进行分析，得到如下结论：如果关心的是航向角，则sift特征点集合呈竖状比较好，即sift点集合和旋转轴平行。

1×8选取特征点

表不同方向手动选取特征点

Image0053 & Image0054	备注
1×8	-96.0655	14.8033	-11.8376	×
-168.9075	35.7764	-17.7804	×
0.4452	-19.4641	-1.7153	×
8×1	0.0554	-10.8175	-0.0209	√
0.0904	-10.8215	-0.0115	√
0.5212	-10.8057	0.2690	√
真实值（°）	0	10	0