基于图优化的Kinect三维视觉里程计设计

张兆博， 伍新华， 刘 刚

(武汉理工大学 计算机学院，湖北 武汉430000)

0 引 言

里程计作为同步定位和地图构建(simultaneous localization and mapping,SLAM)[1,2]的关键部分，近年来逐渐成为机器人领域的热点研究问题，而SLAM更被认为是实现移动机器人自主化的核心技术。视觉里程计(visual odometer)一词源于Nister D等人[3]在2004年的一篇里程碑式的文献，旨在通过分析视觉信息，来获取运动的轨迹信息，是通过视觉传感器作为机器人“双眼”的基础。

视觉里程计根据计算运动轨迹所使用的数据传感器类型，可以分为单目里程计、双目里程计、以及RGB—D里程计。2010年下半年，微软推出了可以直接获取景深信息的RGB—D相机Kinect，再加上其大众化的价格，深受众学者的喜爱。

早期Bailey T等人[4]使用扩展卡尔曼滤波算法来求视觉里程计。文献[5～7]介绍了利用滤波求解视觉里程计的典型方法。这类算法对于运动时间较短的小场景可以取得较好的结果，但大规模场景的里程计会随着误差的累积产生较大的偏差。

Martinez Henry P等人[8]在2012年最先提出使用RGB—D相机求解视觉里程计，并提出一种TORO算法用来对相机位姿进行优化。该类算法虽起步较晚，但却在近年来取得了不俗的成果[9～11]。Nikolas Engel Hard等人[12]在2012年实现了一种手持RGB-D里程计系统并进行了改进。Engel J等人[13]在2014年提出的LSD—SLAM中首次引入“关键帧”的概念，即通过选取有代表性的帧来求解视觉里程计。Mur-Artal R等人[14]在2015首次将优化的过程运行在线程上，提高了系统的性能。

本文使用Kinect作为视觉传感器，进行视觉里程计的计算。本文在Henry算法的基础上，充分结合Kinect相机易于获取深度图像这一特点，提出一种新的基于深度图像匹配的关键帧选取算法，通过在关键帧上构建带环图模型，对视觉里程计结果进行局部和全局优化。实验结果表明，相比较Henry P等人的方法，本文方法在减小里程计相对误差上效果显著，这对以视觉里程计为基础，重建三维场景的应用中具有重要的作用[15，16]。

1 基于深度图像的关键帧选取

Kinect摄像机的帧率一般在30 Hz左右，计算每一帧的视觉里程计是不可取的。因为当帧与帧之间距离过近时，相当于没有移动；过远时，则无法进行计算。因此，只用当摄像机的运动在一定范围内时，计算才有意义，这样的帧定义为“关键帧”。本文根据Kinect摄像机易于获取深度信息这一特点，利用光流法匹配帧间位姿，利用阈值来筛选关键帧。

假设3维空间中一点X=[x,y,z,1]T，在时刻t与时刻t+1之间的刚体运动为

(1)

式中R3×3为3个自由度的旋转矩阵，t3×3为3维平移向量。变换矩阵T可以表示为

(2)

式中T为图像帧之间的位置变换，T又称为帧间位姿；[ωx,ωy,ωz]为相对于x,y,z轴的旋转；[tx,ty,tz]为相对于x,y,z轴的平移。连续的变换矩阵T即视觉里程计。根据透视投影，x(t+1)于t+1时刻在深度图像上的投影点为

(3)

式中fL为摄像机的焦距。求得投影点x(t+1)相对于μ=[ωx,ωy,ωz,tx,ty,tz]的导数矩阵Fμ,即

(4)

以上导数可反映像素点根据Fμ在深度图像上的位置变化。点X在时刻t的深度值为d，根据LK方法，存在

d(F(x,μ),t+1)=d(x,t)

(5)

即在一定的范围内对于三维空间点X在相邻时刻的深度值恒定。求解μ，即是找到一个μ使得最小二乘误差最小，即

(6)

式中Ω为t时刻的模板区域，只有在Ω内部的像素点才考虑运动估计。因为深度图像保存了场景的景深信息，可以通过简单的二值化分离出背景，非背景区域即为Ω模板区域。

假设运动为μ+Δμ，则误差为

t+1)-d(x,t))

(7)

根据泰勒展开式

(8)

计算μ是一个迭代过程。在每次迭代中计算运动增量Δμ。然后，模板根据运动增量Δμ进行变换，变换结果用于下一次迭代。当Δμ收敛后迭代过程结束，μ通过一系列Δμ变换组合得到。μ经过矩阵变换得到帧间位姿T。

对于运动矩阵μ=[ωx,ωy,ωz,tx,ty,tz]，数值Lμ表示运动的大小，Lμ如下

(9)

利用阈值法对Lμ进行定义

(10)

式中 过小表示两图像帧间距很近，没有必要都进行保留；过大则表示两帧间距很远，缺乏关联性；适中则能保证两帧的间隔性与关联性，并且新的图像帧会被加入到关键帧序列中去。其中k1和k2的取值通过实验获得，阈值法的好处在于可以灵活地调整阈值来保证结果的正确性，针对一些复杂多变的环境是非常有效的。

2 基于图的里程计优化

计算视觉里程计实质是计算帧序列的帧间位姿序列，Tij表示第i帧与第j帧的位姿变换。引入图G={V,E}构建优化模型，E为顶点表示相机的位姿

(11)

式中V为边表示顶点间的变换或约束

(12)

未进行优化的视觉里程计的图结构如图1所示。

图1 不带路标的图结构

本文在上述链式结构的基础上，通过上文的关键帧算法，在局部添加局部回环以及随机回环，进而构造不含路标顶点的因子图。具体流程如下：

1)初始化关键帧的序列F，并将第一帧f0放入F。

2)对于新来的一帧fnew，利用关键帧提取算法计算F中最后一帧与fnew的帧间位姿T，并估计该T的大小Lμ。有以下几种可能：

a.过小，说明离前一个关键帧很近，丢弃该帧；

b.过大，说明运动太大，可能是距离太远或计算错误，同样丢弃该帧；

c.适中，运动估计正确，同时又离上一个关键帧有一定距离，则把fnew作为新的关键帧，进入回环检测程序。

3)局部回环检测：匹配fnew与F末尾n1个关键帧。匹配成功时，在图里新增一条约束边。

4)随机回环：随机在F里取n2个帧，与fnew进行匹配。匹配成功时，在图里新增一条边。一般取n2=2n1，这样取值的好处在于尽可能多地让新帧与前面的帧建立约束边，同时保证不会因为某一帧的丢失导致图链的断裂。

5)将fnew放入F末尾。若有新的数据，则回步骤(2)；若无，则图构造完成。取n1=3，n2=6时图的结构如图2所示。

图2 本文算法构造的图结构

当图结构是存在回环的非链状结构时，由于边Tij中存在误差，使得所有边给出的数据不一致，此时，优化一个不一致性误差

(13)

(14)

(15)

式中Jij为误差函数eij(x*)在x*附近的雅克比矩阵。将式(15)代入式(14)的某一项并展开

F(x*+Δx)=C+2bΔx+ΔxTHΔx

(16)

H=∑Hij为Hession矩阵的累加。为使式(16)最小，使其一阶导数等于0，得

HΔx=-b

(17)

至此，将优化问题进一步转化为线性方程组求解问题。

求解的过程通过高斯—牛顿迭代法完成。首先将关键帧匹配求得的每一帧的位姿向量[ωx,ωy,ωz,tx,ty,tz]作为图中每个顶点x的初始值x*，求得的Δx叠加x*作为新的初始值，重复执行该过程直到x*收敛，此时x*为优化后的结果。

3 实验与结果分析

为了对算法进行评估，本文利用Computer Vision Group提供的RGB-D数据集进行试验，该数据集主要包含2个部分：Kinect视频序列和真实的里程计。

3.1 算法性能评估

算法性能主要包含2方面：算法的精度和算法的实时性。算法的精度由求得的视觉里程计与真实里程计的误差决定，算法的实时性由单帧里程计平均用时决定。图3给出了视觉里程计在2维空间的投影结果。

图3 不同算法估计相机运动2D轨迹

图4(a)为不同算法在具体标准误差上的比较，绝对轨迹误差 (absolute trajectory error,ATE)用来衡量算法估计轨迹与真实轨迹的误差；相对姿态误差 (relative pose error,RPE)用来衡量位姿之间存在的误差积累程度。图4(b)为不同算法在耗费时间上的比较。可以看出，相比较传统算法，本文算法在不影响实时性的前提下，在绝对轨迹误差和相对姿态误差上，取得了较为显著的提高。

图4 性能对比

3.2 三维点云图

为进一步验证算法性能，以求得的视觉里程计为基础，实现三维场景的重构，通过点云图呈现。当然由于Kinect相机拍摄距离的局限性，对于过远的场景，由于无法得到其图像，所以没办法进行拼接还原。如图5所示。

图5 数据集的三维点云模型

3.3 真实室内环境实验

为了进一步通过实验检验算法，通过手持Kinect，在室内缓慢绕行1周，收集室内场景信息。Kinect的帧率为30 Hz，分辨率大小640×480，共收集1 800帧RGB-D图像，最终选取了230个关键帧。由于当物体离Kinect较远时，测量的深度信息可能不准确，设定只保留7 m以内的点。点云0.05 m，最小距离k1设为0.8，k2设为2，相邻检测帧数n1设为4，随机检测帧数n2设为8。实验建立的室内三维场景如图6所示。左图为整体效果，右图是对局部区域放大后的显示效果，从该图中可以看出，对于拐角、转弯等易于产生较大误差的地方，本文算法都实现了较好的优化。

图6 真实室内环境的三维点云模型

4 结束语

Kinect相机因其能够提供深度信息的优势在计算机视觉领域具有广阔的应用前景。本文以Kinect为传感器，实现基于图优化的视觉里程计设计。该方法充分利用了Kinect易于获得深度信息的特点，提出了一种新的基于深度图像的关键帧选取算法，并通过在关键帧添加局部和随机回环构建图，在图上用非线性最小二乘方法实现视觉里程计的优化。实验结果表明：运用本文方法，在不影响实时性的同时，有效减小了误差，得到了更为精确的视觉里程计结果。