基于RGB-D相机的视觉里程计设计

霍智豪，苗艳松

(1.山东大学 信息科学与工程学院，山东青岛，266237；2. 鲁东大学 教育科学学院，山东烟台，264025)

0 引言

视觉SLAM在实现过程中，目前公认的经典框架包含三个主体部分，分别是前端的视觉里程计，后端的非线性优化和回环检测，如图1所示 。

图1 经典SLAM框架

在视觉SLAM技术框架中，传感器信息是由运动相机采集到的有效帧经过预处理后的可用数据；视觉里程计利用这些数据估计相机的运动与局部地图的大致轮廓；后端优化对视觉里程计估计的相机位姿进行优化，并且接受回环检测的重复信息；本文将会对视觉里程计环节进行具体实验与优化。

1 相机模型

■ 1.1 基本相机模型

我们在此次设计中使用RGB-D 相机，在基本彩色图像素获取之外，还可以通过结构光(如Kinect相机)或飞行时间法(Tine-of-Flight，ToF)主动获取深度信息，彩色图像素的获取与深度信息的获取方式是不同的，我们先来阐述基本的针孔相机模型。

我们得到了相机模型的基本几何约束：

设像素坐标系下N′点的坐标为，在齐次坐标系下表示式(1)(2)的约束条件：

我们定义上式中的运算矩阵为K，定义为相机的内参矩阵，完整的相机模型[2]叙述为：

■ 1.2 畸变矫正

引入与中心距离有关的高次多项式函数进行纠正，得到三维空间点最终映射到像素平面的完整模型：

r表示与中心坐标的距离。

■ 1.3 深度信息提取

我们通过结构光(如Kinect相机)或飞行时间法(Tine-of-Flight，ToF)主动获取深度信息。

图3 结构光

结构光法基于光学三角测量原理，根据摄像头采集到的光信号变化来计算物体的位置与深度。飞行时间法是计算接收到红外光的相位差，进而还原出深度信息。

2 特征点提取与匹配

我们将会使用ORB[3](Oriented Fast and Rotate BRIEF)算法进行特征点的处理并提出优化算法。

■ 2.1 FAST特征点提取与改进

我们遍历灰度图像中的像素点，并将其与圆周上的16个像素点进行像素值的比较，筛选出连续差值超过规定阈值的点，作为FAST角点[4]：

我们判断特征点就是对N进行比较。

图4 FAST特征点提取

图5 改进的特征点提取

■ 2.2 FAST特征点的优化

我们通过构建图像金字塔来来赋予特征点“尺度不变性”，优化后的特征点是每帧缩放图像中的特征点集。我们通过灰度质心法使特征点具有方向：

另外，我们提出将图像划分网格的改进思想，共随机抽取若干个网格进行ORB处理，保证得到的有效特征点既可以满足匹配的需要，又可以提高运算效率，满足实时性的要求。

■ 2.3 BRIEF描述子的改进

定义操作Q比较点对之间的灰度值：

BRIEF描述子就是Q值的二进制排列[5]。

我们提出改进的描述子选择方法，以关键点和特征点的质心连线为X轴建立二维取点坐标系。

■ 2.4 特征点的匹配

我们在两帧连续图像的点集Qt，Qt+1中计算每两个特征点的汉明距离：

汉明距离小于指定阈值的特征点是匹配的特征点，这种方法是暴力匹配[6]。

我们提出随机一致性采样(RANSAC,Random Sample Consensus)优化算法，对匹配之后的点对进行再次筛选。

定义齐次坐标下的计算M和S：

我们计算误差来保留正确匹配和剔除错误匹配。

图6 RANSAC改进的匹配

3 运动的估计

我们对ICP算法提出改进，在全局上描述两帧之间相机的运动：在使用RANSAC估计[7]的变换矩阵T进行变换之后，计算实际点集与估计点集误差，由其最小值计算出精确的相机运动参数。

图7 优化匹配

我们定义该误差平均数e：

每一次迭代中，使用变换矩阵nT的估计值去新源数据集np中的每一个点：

重新计算新的源数据集 1np+与目标数据集Q之间的误差ne，当满足设定的阈值时，结束迭代，并将此时的nT作为相机运动的最优解。

图8 相机运动的ICP优化

4 系统测试

■ 4.1 相机内参的标定

我们使用张氏标定法[8]，对所使用的的相机进行参数的标定。

图9，图10分别是相机坐标系和世界坐标系的视图。

图9 相机坐标系

图10 世界坐标系

最终标定得到的相机内参：

对标定得到的相机内参矩阵K进行误差分析可知，每个角点的像素误差均不超过±0.5，标定所得的内参正确。

■ 4.2 数据集测试

4.2.1 测试walking.xyz数据集

该场景中，相机的运动主要是平移和较小的旋转，干扰项是视频中两位走动的研究者（见图11）。

图11 特征点提取

在相机的连续拍摄画面中，特征点的匹配完成度较好，大部分帧场景下不会出现特征点丢失的现象。但是因为研究者的走动，过程中出现了追踪失败的现象（见图2）。

图2 针孔相机模型

使用数据集包含的evaluate_ate工具对本算法的位姿估计值进行误差分析，如图13~14所示。

图13 ATE

ATE误差的值较小， RPE误差却可以保持在1以内，说明本算法可能会存在地图的误差，但是相机的运动估计比较准确。

4.2.2 测试sitting_static数据集

在该测试场景中，两位研究者不运动，相机也没有较大幅度的运动，特征点提取压力相对较小。

由图15~16中的实验现象，我们发现，在相机运动幅度很小并且视频中画面相对变化不大的情况下，本算法的卷对误差与相对误差都较小，说明本算法在宽容条件中的准确性较高。

图12 追踪失败

图14 RPE

图15 ATE

图16 RPE

4.2.3 测试walking_halfsphere数据集

该场景中，相机的运动主要是大幅度的旋转和较小的平移，同样两位研究者的移动会给特征点的提取带来压力。

测试的结果为；RPE的值偏大，可知本算法对旋转情况下的估计性能略差与平移情况下的性能，我们需要对运动的估计做出优化。

4.2.4 测试walking_static数据集

在该场景中，相机的位姿基本固定，画面中两位研究者不断运动，给特征点的匹配带来压力。

该项测试的结果量误差都是较小的值，可知在相机运动幅度较小的情况下，本算法的误差较小。

比较四个数据集测试得到的均方根误差、均值误差、中值误差和标准差，如表1所示。

表1 测试误差比较

从表格数据可以看出，当相机与拍摄的图像变化不大时，该算法误差较小，但是当相机运动较大特别是在旋转场景下时，该算法的误差较大。

■ 4.3 真实场景测试

如图17所示，在光影变化显显著的场景下，以灰度值为特征的ORB算法不稳定甚至失效。

图17 特征点匹配失败

但是，当拍摄者走进光影变化较小的场景中，本算法就可以正常的提取特征点，以完成位姿的估计。

总的说来，在缺少后端优化[9]的条件下，尽管存在着无法应对光影对比显著场景的缺陷，视觉里程计的运动估计功能基本实现。

图18 阴影场景中匹配成功

5 总结

本设计基于RGB-D相机，完成了SLAM框架的前端视觉里程计的设计，研究并优化了传统的SLAM算法，分别实现了ORB特征点提取的“网格化处理”、特征点匹配的RANSAC改进以及相机位姿的ICP优化等，最终通过PCL输出局部点云地图；室外环境下表现较弱，光线阴影变化的地方特征点的提取和匹配算法会出现较大的误差。后续研究可以结合深度信息提取与语义信息对特征点提取与跟踪进行优化。