基于辅助匹配的1点RANSAC单目视觉导航算法

齐乃新, 张胜修, 曹立佳, 杨小冈

(1. 火箭军工程大学控制工程系, 陕西 西安 710025;2. 四川理工学院自动化与信息工程学院, 四川 自贡 643000;3. 人工智能四川省重点实验室, 四川 自贡 643000)

0 引 言

随着计算机视觉的发展,单目视觉导航技术已经在机器人、可穿戴计算、增强实景以及无人驾驶汽车等众多领域展现出了广阔的应用前景,成为人们研究的热点。目前,单目视觉导航方法主要分为两大类[1]:一类是基于滤波的估计方法,通过融合图像序列的观测量,不断地更新特征点和摄像机的位姿参数来估计摄像机的运动信息[2-3];另一类则是基于光束平差(bundle adjustment,BA)的估计方法,通过光流场或特征点匹配实现对载体长时间的无漂移运动估计[4-5]。

基于滤波的估计方法可以有效地利用滤波预测,估计下一帧的特征点位置,采用主动视觉匹配的方法来降低匹配的复杂度,能够实现对摄像机位姿参数的实时在线估计。帝国理工大学的Davison教授及其领导的研究团队在这一领域做出了比较突出的贡献,2003年首次提出了对自由运动摄像机的6D姿态估计方法[6]。该方法采用主动视觉匹配,具有较好的实时性和鲁棒性;采用基于粒子滤波的特征初始化算法,很好地解决了位姿估计的长时间尺度漂移问题,具有较高的精度。随后,Davison教授等人又做了大量的研究工作,推动了基于视觉同步定位与地图构建(simultaneous localization and mapping,SLAM)技术的发展[7]。在Davison教授的研究基础上,Civera[8]提出了一种基于1点随机抽样一致性(random sample consensus,RANSAC)的单目视觉导航方法,实现了对运动摄像机的位姿估计。该方法结合了RANSAC和扩展卡尔曼滤波器(extended Kalman filter,EKF),解决了视觉SLAM中的数据一致性问题,较大程度上优化了数据关联的过程[9]。同时,利用1点RANSAC[10]生成假设模型,减少了算法迭代的次数,增强了系统的稳定性。

虽然Civera提出的1点RANSAC算法在视觉导航的研究中受到了许多学者的青睐[11-13],但在实际实验过程中面临一个匹配失效的问题,尤其是在摄像机姿态或者环境的光照条件发生突变的情况下。这一问题是由算法中的主动视觉匹配和数据关联原理决定的。当摄像机的运动发生突变时,主动视觉匹配极易失效,找不到有效的匹配点,对后续的EKF内点更新过程产生较大的影响,系统滤波的稳定性以及状态估计的精度都会不同程度的降低。针对上述问题,本文提出了基于尺度不变特征变换(scale invariant feature transform,SIFT)的辅助匹配方法,采用SIFT算法提取特征点、RANSAC算法解算特征点所在图像与当前图像之间的基础矩阵,然后利用此基础矩阵求解匹配点,解决摄像机姿态突变情况下主动视觉匹配失效的问题。

1 1点RANSAC单目视觉导航算法

1.1 预测和搜索匹配点

(1)

(2)

(3)

(4)

式中,Hi是hi关于xk|k-1的雅可比矩阵;Ri是观测模型中高斯噪声的协方差矩阵。

然后,采用主动视觉匹配方法根据预测值在特征点出现概率较高(不低于99%)的区域内搜索,找到匹配点zi,即为观测量的真实值。

1.2 建立和估计1点状态假设

图1 1点RANSAC估计过程Fig.1 1-Point RANSAC estimation process

在建立假设时,RANSAC算法首先为系统的模型参数建立假设,并且挑选出获得支持率最高的一个假设。假设的建立是通过随机的从匹配点集zIC=(z1…zi…zn)T中选出m个点完成的,m的值是计算模型参数所需的匹配点数量的最小值。通过简单的计算统计所在阈值内的点的数量得到每一个假设的支持投票数,这里所用的阈值可以根据显著性参数α=0.05的χ2分布得到。包含外点的假设会得到较少的投票数,如图1的假设3。建立的假设的数量nhyp至少能够确保在测试概率p下有一个无虚假假设,可以由式(5)计算得到。

(5)

式中,e表示点集中外点的占有比例;nhyp表示需要建立的假设数量;m表示满足模型参数求解条件的最少匹配点数。

1.3 第1次部分更新

靠近假设模型较近的点,对假设的支持性较大,被近似认为是构成实际模型的点,这些特征点被称为具有小观测残差的内点集。除此之外的点称为外点,但是仍具有是内点的可能性。

在RANSAC假设的建立过程中,摄像机的旋转量假设主要依靠距离较远的特征点建立,摄像机的位移量假设主要依靠距离较近的特征点建立。由于估计的不准确性,部分距离远的外点会呈现出小观测残差的特性,而距离较近的内点也有可能呈现出大观测残差的特性。

在假设模型和内定确定以后,利用具有小观测残差的内点对系统的状态矩阵和协方差矩阵完成第1次部分更新,如式(6)～式(8)所示。

(6)

Pk|k=(I-KkHk)Pk|k-1

(7)

(8)

1.4 第2次部分更新

EKF预测中的误差和协方差中的误差在第1次部分更新完成之后得到了有效的修正,此时,存在于外点集中的具有大观测残差的内点可以被有效地恢复。

2 SIFT辅助匹配

2.1 匹配失效问题描述分析

1点RANSAC算法要求摄像机的运动是平稳连续的,这样才能够保证特征点在预测的区域中。但在实际情况下，摄像机并不能保证平稳连续运动,比如飞行器在遇到强风、障碍,车辆发生急转弯、紧急刹车等情况下,载体的速度或者姿态会发生突变。此时,特征点落在预测的区域之外或有较少的点落在预测区域之中,主动视觉匹配无法搜索到落在预测区域外的点。因此,在这种情况下会找不到有效的匹配点,即使找到,数量也很少。1点RANSAC算法做出假设至少需要一对匹配点,此时的主动匹配失效,不能提供有效的匹配点对来建立假设,也就不能完成后续的更新过程。当找到的匹配点数量过少时,不能满足概率p下确保有一个无虚假假设的条件,此时的RANSAC过程处于一种病态假设,对模型参数的估计存在较大的误差。

1点RANSAC算法中采用主动视觉匹配作为特征点的匹配策略,通过式(3)对特征点的位置进行预测,在特征点以99%概率出现的区域内进行搜索。当摄像机的姿态发生突变时,相邻两帧图像之间会有较大的差异,此时特征点将会落在预测的区域之外或有极少数的特征点落在搜索区域内。当全部的特征点都落在搜索区域外时,搜索失效,主动视觉匹配将无法找到匹配点,发生匹配失效的问题。当有极少数的特征点落在搜索区域时,由于特征点过少在后续的内点更新过程中会代入较大的误差,影响滤波估计的精度。

上述问题在本文的主动视觉匹配失效分析实验部分,通过对Civera提供的“rawoutput”测试数据集的实验分析得到了充分说明。为此,本文提出辅助匹配策略,采用SIFT特征点匹配方法解决主动视觉匹配失效的问题。

2.2 SIFT辅助匹配方法

匹配失效情况的判断依据是主动视觉匹配得到的匹配特征点数。首先,根据式(3)和式(4)预测出特征点所在的区域;然后,采用主动视觉匹配方法搜索匹配点zi;其次,对匹配点zi的数量进行统计,得到匹配点总数Nz。最后,根据匹配点总数和设定的阈值(本文中取值为5)进行判断,如果匹配点数Nz小于设定的阈值则进行辅助匹配。辅助匹配的具体过程描述如下。

2.2.1 获取SIFT特征点

SIFT算法是由Lowe等人在2004年提出的[15],该算法具有丰富的信息量,能够很好地处理尺度变换、亮度变换、旋转等情况下的图像匹配问题。为了提高SIFT算法的效率,本文采用栅格的形式在关键点的邻域内选取SIFT特征点[16],对SIFT算法的描述子进行适当地优化,如图2所示。

将关键点16×16大小区域按图2所示分成4×4的16个小区域,从中选取彼此不相邻的8个小区域(图中灰色小区域)对SIFT特征点进行描述。如此,能够将SIFT特征点128维的特征描述向量压缩为64维,降低了算法的复杂度。

图2 SIFT特征点选取方式Fig.2 SIFT feature point selection method

然后利用式(9)剔除响应值小的点

(9)

D(x,y,σ)=(G(x,y,kσ)-G(x,y,σ))⊗I(x,y)=

L(x,y,kσ)-L(x,y,σ)

(10)

L(x,y,σ)=G(x,y,σ)⊗I(x,y)

(11)

式中,I(x,y)表示一幅图像;(x,y)表示图像的像素坐标;σ是尺度空间因子;L(x,y,σ)表示图像变换后的尺度空间;G(x,y,σ)为高斯核函数; ⊗为卷积运算。

最后,通过如式(12)中2×2的Hessian矩阵H剔除边缘点,得到带有描述子的SIFT特征点。

(12)

2.2.2 匹配

分别提取到两幅图像的SIFT特征点后,采用式(13)欧式距离进行相似性度量,获取图像间潜在的匹配。

(13)

式中,向量X和Y分别定义为X=(x1,x2,…,xn)和Y=(y1,y2,…,yn)。

根据相似性度量得到的初始匹配点对中存在一定程度的误匹配,为了剔除这些误匹配点,提高算法的鲁棒性,在算法中添加了主方向约束条件[16],其具体步骤如下:

步骤1按式(14)计算匹配点集zIC=(z1…zi…zn)T的主方向差方值,即

D(i)=|O1(i)-O2(i)|2，i=1,2,…,n

(14)

式中,D(i)代表zIC=(z1…zi…zn)T的主方向差方值;O1(i)为图像1中的特征点主方向;O2(i)为图像2中的特征点主方向;n为匹配点集的大小;

步骤2计算误匹配点的剔除阈值To,即

(15)

步骤3剔除初始匹配点集zIC=(z1…zi…zn)T中的误匹配点,即

(16)

2.2.3 解算基础矩阵及匹配点求解

基础矩阵反映了两幅图像之间的变换关系,其与两幅图像的匹配特征点对存在如式(17)所示的约束关系。

u′Fu=0

(17)

假设匹配点集的大小为n,则两幅图像之间满足的约束方程为

Af=

(18)

式中,f=(f11,f12,f13,f21,f22,f23,f31,f32,f33)；A为系数矩阵。基础矩阵的求解算法对外点比较敏感,为此文中在RANSAC算法的基础上引入了Sampson加权算子来划分局内点和局外点,用以减小噪声对特征点的影响,防止由于少量误匹配所产生的误差影响匹配精度的问题产生。解算得到基本矩阵之后,根据初始特征点求解对应的匹配点,具体步骤如下:

步骤2通过加权算子

(19)

计算Sampson距离,将zIC=(z1…zi…zn)T分为局内点和局外点,对基本子集进行投票;

步骤3将当前投票数与最大投票数进行比较,保存更新对应投票数最多的基础矩阵和局内点;

步骤5根据对应投票数最大的局内点,重新计算基础矩阵F;

步骤6根据步骤5得到的基础矩阵F解算用于状态更新的有效匹配特征点。

2.2.4 EKF状态更新

辅助匹配找到有效匹配特征点之后对状态方程和协方差矩阵进行一次更新，更新方程如式(20)～式(22)所示。

(20)

Pk|k=(I-KkHk)Pk|k-1

(21)

(22)

2.2.5 建立状态假设

最后建立状态假设,完成1点RANSAC单目视觉导航算法的两次内点更新,得到状态方程的最终估计量。

3 实验验证分析

3.1 主动视觉匹配失效分析实验

为充分说明1点RANSAC算法中的主动视觉匹配失效问题,本文采用Civera提供的rawoutput测试数据集进行了实验分析,实验结果如图3～图5所示。

图3 主动视觉匹配失效帧的图像Fig.3 Image in active vision matching failure frame

图4 特征点的统计情况Fig.4 Statistics of feature points

图5 主动视觉匹配找到的匹配点少于5个时的统计情况Fig.5 Statistics of the matching points under 5 found by the active visual matching

图3给出了主动视觉匹配得到的匹配点数在5个之内(包括5个)的几帧图像,从第1～6行分别是0～5个匹配点的情况,图中蓝色表示未能找到匹配点的特征点、粉红色表示找到的匹配点是外点的特征点、红色表示找到的匹配点是内点的特征点。从图3中可以看出,虽然具有较多的特征点,但由于摄像机的运动突变以及外界环境的干扰,导致主动视觉匹配没有找到有效的匹配点,或者找到的有效匹配点不足以建立一个良态假设。

图4对整个图像序列每一帧的特征点总数、主动视觉匹配找到的匹配点总数、具有小观测残差的内点数以及具有大观测残差的内点数进行了统计。从统计情况来看,主动视觉找到的匹配点数在10个以内的情况出现次数较多,而匹配点数少于10时,RANSAC假设的建立以及利用内点更新的过程会出现病态情况,影响了算法的估计精度。由此可以知道,在1点RANSAC单目视觉导航算法中主动视觉匹配失效问题比较严重,并且会对估计精度造成一定的影响,需要采取必要的措施加以解决。

图5是对匹配点数在5个之内出现的帧数的一个统计情况,从图中可以分析出主动视觉失效是阶段性的,存在于摄像机运动发生突变,或者环境发生突变时的情况下。在本实验中主动视觉匹配失效问题在第800～1 700帧时出现比较密集,原因在于这一段中摄像机的运动突变情况较多,平滑稳定性较差,并且多次出现角度和运动方向逆转的情况。

为了进一步说明1点RANSAC算法中的匹配失效问题对整个算法的影响程度,表1对主动视觉匹配找到的匹配点少于10个时的帧数,以及在图像序列中的占比情况作了统计。

表1 匹配点数在10个以内的帧数统计结果

整个数据集总共2 169帧图像,没有找到匹配点的情况共44帧,找到一个匹配点共61帧,找到2个匹配点共75帧,找到3个匹配点共101帧,找到4个匹配点共106帧,找到5个匹配点共117帧。从统计结果来看,完全没有找到匹配点的情况占2.03%,匹配点在5个以内(包含5个)占23.25%,匹配点在10个以内(包含10个)占46.26%。可见主动视觉匹配失效的情况发生的次数较多,对算法的影响比较大,如果不采取一定的措施来解决这一问题,会较大程度上影响单目视觉导航算法的估计精度和稳定性。为此,本文提出一种基于SIFT的辅助匹配策略,解决了1点RANSAC单目视觉导航算法中的匹配失效问题。

3.2 辅助匹配效果分析实验

本文采用Civera提供的rawoutput测试数据集进行了匹配实验。取第892帧为当前图像,此时1点RANSAC算法发生了匹配失效问题,采用主动视觉匹配未能找到匹配点。取892帧时的特征点第一次被观测到时所在图像为待匹配图像,采用本文提出的辅助匹配算法查找匹配点。

实验统计可知第892帧时,特征点总数为40,SIFT匹配找到的特征点总数为35。如图6所示,分别是第98、679、739和862帧上的特征点的SIFT匹配结果,这4组图像发生了较大程度的平移、旋转、尺度缩放以及视角变化,从实验结果来看,匹配精度较高,能够有效地找到匹配点。

图6 SIFT匹配结果Fig.6 Matching results of SIFT algorithm

图7对SIFT匹配的精度进行了统计分析,从分析结果来看，35个特征点中有3个点的误差超出了10个像素,其他点的误差均在5个像素之内,分析原因为误差超过10个像素的特征点所在的图像与当前图像的重叠区域较少,SIFT匹配过程中找到的匹配点数少于15,此时RANSAC算法解算基础矩阵时不能有效地剔除误匹配,引入了较大的误差。此问题可以在SIFT辅助匹配过程,通过设定SIFT匹配点数的阈值来解决。

图7 SIFT匹配解算的匹配点误差Fig.7 Errors of matching points calculated by SIFT algorithm

采用添加辅助匹配后的1点RANSAC算法重新对Civera提供的rawoutput测试数据集进行了导航估计,并重新对整个数据集中每一帧的特征点总数、SIFT辅助找到的匹配点总数、具有小观测残差的内点数以及具有大观测残差的内点数进行了统计,结果如图8和图9所示。从图8辅助匹配后特征点的统计情况来看,不存在匹配点数在5个以下的情况。图9对应的是图2的帧数,从匹配点的显示结果来看,每一帧都能够找到有效的特征点进行算法后续的状态更新,较好地解决了1点RANSAC单目视觉导航算法中存在的主动视觉匹配失效问题。

图8 辅助匹配后的特征点统计情况Fig.8 Statistics of feature points with aided matching

图9 主动视觉匹配失效帧的辅助匹配结果Fig.9 Results of the images in active visual matching failure frame with aided matching

3.3 智能实验平台验证实验

为了验证辅助匹配方案对提高EKF算法估计精度的有效性,本文设计了一组验证性实验,实验采用载有单目摄像机的智能实验平台,如图10所示。本实验平台是根据需要自主改装的由DrRobot公司生产的X80 Pro型WiFi智能控制小车。该智能实验平台包含单目摄像机和通信控制模块,能够通过编程预定行驶轨迹模拟平台的运动轨迹,并且能够通过WiFi实时传输图像序列,用于估计平台的位姿参数。车载摄像头采用电荷耦合器件(charge coupled device,CCD)针孔摄像头,其模型为针孔摄像头模型,焦距:fx=330.524,fy=329.791;中心点:Cx=159.5,Cy=119.5;畸变参数:k1=-0.103,k2=0.138;采集到的图像大小:320像素×240像素。

图10 智能实验平台Fig.10 Intelligent experimental platform

本验证实验是在室内环境下完成的,环境相对比较复杂,包括实验仪器、办公桌、电脑等静态物体,以及人员走动、光照的变化等动态环境。实验中,机器人平台的运动轨迹分为3个阶段:第1阶段先往前直行8 m,然后进入第2阶段,进行一次90°转弯,第3阶段再往前直行6 m。分别采用本文改进算法和标准的1点RANSAC算法(原算法)对摄像机的姿态角进行估计,实验结果如图11～图17所示。机器人运行轨迹如图18所示。

图11 摄像机航向角的估计结果Fig.11 Estimation results of camera’s course angle

图12 摄像机俯仰角的估计结果Fig.12 Estimation results of camera’s pitch angle

图13 摄像机滚动角的估计结果Fig.13 Estimation results of camera’s roll angle

图14 摄像机航向角的估计误差Fig.14 Estimation errors of camera’s course angle

图15 摄像机俯仰角的估计误差Fig.15 Estimation errors of camera’s pitch angle

图16 摄像机滚动角的估计误差Fig.16 Estimation errors of camera’s roll angle

图17 算法对单帧图像的计算时间Fig.17 Computational times for single frame of the algorithms

图18 机器人运行轨迹示意图Fig.18 Sketch map of robot’s motion trail

图11～图13为本文算法和原算法对摄像机姿态角的估计结果对比图,从图中可以看出,本文改进算法的估计精度要高于原算法。图14～图16为本文算法和原算法对摄像机姿态角的估计误差对比图,改进算法估计的摄像机航向角、俯仰角和滚动角的平均误差为6.73°、3.25°和9.01°,标准的1点RANSAC算法估计的摄像机航向角、俯仰角和滚动角的平均误差为11.77°、4.46°和12.04°。由此可知,添加辅助匹配后的EKF算法提高了对摄像机的俯仰、偏航以及滚动3个姿态角的估计精度,其中航向角平均误差减小5.04°,俯仰角平均误差减小1.21°,滚动角平均误差减小3.03°。

为了比较本文算法和标准的1点RANSAC算法的计算效率,图17给出了本文算法和原算法对单帧图像的运算时间。其中，本文算法的运算时间中不包括SIFT匹配的时间,因为SIFT算法固有的复杂性,其运算比较耗时,文中仿真平台为Matlab,其效率较低,算法仿真成熟后将移植到C++平台,然后采用多线程并行处理SIFT匹配问题,不影响EKF滤波的过程,因此暂时不将SIFT算法的运算时间考虑在内。本文算法的平均耗时为0.32 s,原算法的平均耗时为0.31 s,由此可见,本文改进算法EKF滤波估计阶段保持了原算法的计算效率。

上述3组实验可以说明,标准的1点RANSAC算法中存在主动视觉匹配失效的问题,影响估计的精度,本文提出的辅助匹配方法能够找到精度较高的匹配特征点,有效地解决了1点RANSAC单目视觉导航算法中的匹配失效问题,在较大程度上提高了EKF算法对摄像机姿态角的估计精度。

4 结 论

本文从理论与实验相结合的角度出发,分析了1点RANSAC单目视觉导航算法中存在的匹配失效问题,提出了一种基于辅助匹配的1点RANSAC单目视觉导航算法。该算法采用SIFT算法提取特征点,RANSAC算法求解特征点所在图像与当前图像之间的基础矩阵,然后利用基础矩阵求解匹配点,有效地解决了摄像机姿态突变时由于数据关联失败引起的匹配失效问题,提高了EKF算法对摄像机运动姿态角的估计精度。

参考文献：

[1] STRASDAT H, MONTIEL J M M, DAVISON A J. Visual SLAM: why filter[J]. Image and Vision Computing, 2012, 30(2): 65-77.

[2] CIVERA J, GRASA O G, DAVISON A J, et al. 1-point RANSAC for EKF-based structure from motion[C]∥Proc.of the IEEE/RSJ International Conference on Intelligent Robots and Systems, 2009: 3498-3504.

[3] PIRE T, FISCHER T, CIVERA J, et al. Stereo parallel tracking and mapping for robot localization[C]∥Proc.of the IEEE/RSJ International Conference on Intelligent Robots and Systems, 2015: 1373-1378.

[4] LIM J, FRAHM J M, POLLEFEYS M. Online environment mapping[C]∥Proc.of the IEEE Conference on Computer Vision and Pattern Recognition, 2011: 3489-3496.

[5] GRASA O G, BERNAL E, CASADO S, et al. Visual SLAM for handheld monocular endoscope[J]. IEEE Tran.on Medical Imaging, 2014, 33(1): 135-146.

[6] DAVISON A J. Real-time simultaneous localisation and mapping with a single camera[C]∥Proc.of the IEEE Computer Vision, 2003: 1403-1410.

[7] HANDA A, CHLI M, STRASDAT H, et al. Scalable active matching[C]∥Computer Visionand Pattern Recognition, 2010: 1546-1553.

[8] CIVERA J, GRASA O G, DAVISON A J, et al. 1-Point RANSAC for extended Kalman filtering: application to real-time structure from motion and visual odometry[J]. Journal of Field Robotics, 2010, 27(5): 609-631.

[9] CIVERA J, DAVISON A J, MONTIEL J M M. Inverse depth parametrization for monocular SLAM[J]. IEEE Trans.on Robotics, 2008, 24(5): 932-945.

[10] FRAUNDORFER F, SCARAMUZZA D. Visual odometry: part II: matching, robustness, optimization, and applications[J]. IEEE Robotics & Automation Magazine, 2012, 19(2): 78-90.

[11] 李捐. 基于单目视觉的移动机器人 SLAM 问题的研究[D]. 哈尔滨:哈尔滨工业大学, 2013.

LI J. Reserch on SLAM problem of mobile robot with monocular vision[D]. Harbin: Harbin Institute of Technology, 2013.

[12] CONCHA A, DREWS-JR P, CAMPOS M, et al. Real-time localization and dense mapping in underwater environments from a monocular sequence[J].Oceans, 2015：1-5.

[13] SENGUPTA A, ELANATTIL S. New feature detection mechanism for extended Kalman filter based monocular SLAM with 1-Point RANSAC[C]∥Proc.of the International Conference on Mining Intelligence and Knowledge Exploration, 2015: 29-36.

[15] LOWE D G.Distinctive image features from scale-invariant keypoint[J].International Journal of Computer Vision,2004,60(2):91-110.

[16] 齐乃新, 曹立佳, 杨小冈, 等. 基于方向约束的改进SIFT匹配算法[J]. 计算机科学, 2014, 41(S1): 125-128.

QI N X, CAO L J, YANG X G, et al. The improved SIFT matching algorithm based on orientation constraint[J]. Computer Science, 2014, 41(S1): 125-128.