0 引言
作为智能车系统中的一个基本模块,自主定位是智能车环境感知、自主导航、决策规划、行为控制的前提.视觉里程计利用车载相机获取环境的图像序列,提取图像序列中的一致性信息,根据一致性信息在图像序列中的位置变化估计相机的位姿参数,进而推算出车辆的运动轨迹[1],是移动机器人自主定位的一个重要方法.相比于传统的轮式里程计和惯性测量单元加GPS卫星定位,视觉里程计在价格、精度及可靠性方面具有一定的优势,因此也逐渐成为了智能车自主定位的一个重要方法.
视觉里程计遵循特征提取、特征匹配、运动估计、精度优化的理论框架.一个关键的问题是如何选择一组可靠的特征点用于运动估计,也就是寻找内点集/剔除外点的问题.随机抽样一致性(RANSAC)算法[2]是一种基于统计学假设—验证方法,通过迭代运算从噪声数据中寻找正确的内数据的经典方法.文[3-4]将RANSAC算法运用于视觉里程计的寻找内点集合.其主要原则是:在每一次迭代中,从所有的匹配特征中抽取最小数量的随机样本,构建一个运动假设,然后验证其它特征对是否符合这个运动假设,符合的点称之为内点,并统计内点个数.重复上述步骤,选择内点最多的一组计算相机的运动参数. Nister[3]提出了经典的5-point RANSAC的方法,被后续的研究普遍采用. Scaramuzza[4]提出了1-point RANSAC的方法,这种方法大大减少了运动估计的迭代次数,但是要求相机必须安装在后轮轴的中点上并且满足平面度约束.当不满足上述要求时,其性能大大下降. 5点法及1点法都是用于单目情况求位姿的方法.对于双目摄像机,文[5]提出了P3P的方法求解位姿,即在双目的情况下,只需3对点即可求出相机位姿.由于RANSAC选用随机样本,所以它并不能保证在每次迭代后都会得到一个更好的估计.为了解决这个问题,文[6-7]使用所有特征点而不是最小随机样本来计算运动估计.每次迭代时,认为当前的运动估计是可靠的,并以此为依据计算每一个特征点的重投影误差,误差值大的作为外点剔除.下一次迭代从剩余的所有特征点开始,重复这个过程,直到满足迭代终止的标准,这种方法被称为最大子集RANSAC方法.文[6]提出了一种短基线的实时立体视觉系统.通过卡尔曼滤波估计三维空间中特征点的速度和位置,最小化静态点的重投影误差得到自运动的6个自由度参数.文[7]提出了基于双目相机和惯性测量单元结合的运动估计方法,其主要贡献在于构建了两个约束窗口:时间窗口、空间窗口,并通过非线性优化的方法优化这两个窗口中涉及的变量.无论是经典的RANSAC方法还是最大子集RANSAC方法,都是使用重投影误差的固定阈值作为评判标准以去除外点,如文[8-9].但是实验及理论推导都表明,在运动估计存在误差的情况下,准确匹配的特征点的重投影误差并不是一个固定的值,它不仅与运动估计误差有关,而且与其位置有关,用一个固定的阈值作为内点判别依据是不合理的.文[10]提出将重投影误差进行归一化处理,以消除重投影误差对坐标的依赖关系,并结合最大子集RANSAC方法去除外点.文[11]进一步扩展了上述方法,仍然采用经光流归一化的重投影误差判别外点,在运动估计阶段首先通过传统的方法(非线性优化)求解旋转和平移,将旋转与平移解耦,以此补偿因旋转带来的光流.然后利用求得的旋转继续优化平移.文[12]提出了一种识别帧间误匹配的算法以去除外点.该方法通过寻找一个图像的适当旋转,使得内点的光流矢量方向大致相同,与最频繁出现的方向偏差大的特征点被视为外点.
相比于传统的将旋转与平移同时估计的方法,将旋转与平移解耦的算法[13-14]能获得更好的性能.最新的文[13]提出了一个立体视觉同时定位与地图构建(SLAM)系统,其视觉里程计部分依靠特征点的跟踪,以及将自运动估计分为旋转估计和平移估计两部分,取得了很高的精度.文[14]论述了误差可能出现在任意一个维度上,同时估计6个自由度的方法很难将外点从数据集中挑出,如果误差在6个自由度上单独估计可以获得更有效的外点剔除效果.其提出了将旋转估计和平移估计分离,此时旋转和平移的光流存在封闭解.
特征点的选择对视觉里程计的精度有很大影响.文[15]提出了一种适用于单目、双目以及RGBD相机的SLAM框架,其在视觉里程计、映射、重定位以及闭环检测四个部分都使用同一种特征ORB[16],该特征对旋转、尺度以及照明具有良好的鲁棒性.
本文提出了一种基于自适应外点剔除与解耦算法的视觉里程计方法.主要包括两部分的内容:
1) 采用归一化的重投影误差作为外点剔除的标准;
2) 采用解耦算法进行自运动估计.
在外点评判标准方面,本文借鉴了文[8-9]的归一化重投影误差的思想,但在算法上有较大改进:利用特征点的跟踪,先使用5-point RANSAC方法得到连续两帧与连续三帧之间的旋转,利用球形线性插值(SLERP)[17]对其进行融合,得到优化的旋转参数.然后采用两帧间光束法平差[15]结合归一化的重投影误差去除外点,获取平移参数.在特征选择上,本文采用目前看来非常具有代表性的实时图像特征ORB.实验部分与上述相关视觉里程计方法进行了比较,证明了本方法在外点剔除效果以及在运动估计精度上的提高.
1 运动估计算法原理及特征获取与匹配 1.1 运动估计算法原理本文采用基于双目立体视觉的视觉里程计原理,通过同步采集左右相机的图像,计算匹配特征点间的视差,以此获得特征点的深度.文[18]证明了最小化图像中的重投影误差比最小化三维空间中欧几里得距离可以得到更好的位姿估计精度.经典的最小化重投影误差的公式如下:
(1) |
式中,Rt,Tt表示t时刻的旋转矩阵和平移向量;εit表示重投影误差,需要对t时刻的特征点集χt={pit}i=1N中的每一个特征求重投影误差,其计算方法如下式:
(2) |
其中{pit-1,pit}⊂R3是t-1,t时刻图像上的特征点匹配对,用它的齐次像素坐标表示pit=[uit,vit,1]T,其对应的图像坐标表示
本文选取ORB(Oriented FAST and Rotated BRIEF)算子检测特征点. ORB算子改进了FAST算子不具有方向性的问题,并采用快速的二进制描述子BRIEF.检测出的特征在保持了旋转、尺度不变性的同时,在匹配速度方面有显著提升.详细的特征检测与匹配方法见文[15].
2 重投影误差分析及自适应判别方法重投影误差有3个来源:
1) 运动点的自运动;
2) 错误匹配;
3) 运动估计的误差.
真正的外点是由前两个因素造成的.由运动估计误差造成的外点并不是真正的外点,实际上应该保留.本文讨论的是由运动估计误差带来的重投影误差,如果运动估计与特征匹配都正确,重投影误差应该为0.
经典的确定内点的方法是在给定运动估计Rt、Tt的情况下,使用式(2)计算重投影误差,并与一个恒定阈值相比较,若大于该阈值,则将其视为运动点剔除.这样的处理方法很有可能把一些内点给剔除了,因为特征点的重投影误差不仅与运动估计误差有关,而且与其位置有关. 图 1展示了KITTI02序列第3帧中测量得到的所有特征点对以及根据传统方法得到的特征点对的位置差异. 图 2展示了其重投影误差值.从图 2可以看出,特征点的重投影误差方差较大,其波动与位置有关,依据一个固定的阈值难以将内点与外点区分开.进一步的观察可以看出距离较近的特征点趋向于具有较大的重投影误差.
理想的外点剔除方法应该采用一个对位置具有自适应性的评判标准,这个评判标准对于匹配准确的静态特征点具有相等的偏移值,因为对于所有特征点的重投影误差都是使用同一个运动估计计算得到的.这样的话,可以将由于存在运动估计误差而被认为是外点的特征点保留,而去除误匹配点和运动点.
2.1 平移运动产生的重投影误差假设有一组无误差的匹配点
(3) |
由于估计得到的自运动参数存在误差,因此即使对于静态点,误差会造成式(3)的不成立.为了选取一个合适的值作为静态点与动态点的阈值,需要计算出在给定运动估计误差的情况下的重投影误差.重投影误差可以表示为
(4) |
其中,(R′,T′)=(RΔR,T+ΔT)为实际的运动估计,其误差为(ΔR,ΔT).现在考虑车辆向前运动的情况,车辆的旋转非常小,假设R≈I,并且车辆在Z轴方向上的运动远大于在X和Y轴上的运动,因此可以认为tz≫tx,ty,即tx,ty≈0.将这两个近似关系用于式(4)中,得到:
(5) |
式(5)说明,在匹配准确且是静态特征点的前提下,重投影误差不仅与运动估计误差Δtz有关,也与特征点的深度λi以及特征点的图像坐标有关.因此,采用重投影误差固定阈值剔除外点,会把一些近距离的以及图像坐标绝对值大的特征点剔除,而对于距离较远的外点则不敏感,误把其认为是内点.这和图 2通过实验得到的结果是一致的.平移估计对于近距离的特征点更加敏感,错误的剔除近距离的特征点而保留远距离的动态点会造成平移估计误差变大.
2.2 归一化的重投影误差作为评判标准将重投影误差做归一化处理,采用二范数光流作为归一项.在只考虑相机前向平移运动的情况下,特征点的光流为
(6) |
由于帧率足够高,特征点的深度远大于相机的帧间运动距离,即
(7) |
式(7)表明归一化后的重投影误差不再与特征点的图像坐标有关,也几乎与特征点的深度无关.也就是说这个归一化的重投影误差对特征点位置具有自适应性,只取决于运动估计的误差.可以对归一化的重投影误差设置一个相对较高的固定阈值去保留由于存在运动估计误差而被认为是外点的特征点,而有效去除误匹配点和运动点,因为误匹配点和运动点的归一化重投影误差相对比较大.
2.3 基于解耦算法的自适应外点剔除方法2.2节分析的是只考虑有平移运动时的情况,其中的重投影误差是只有平移运动时产生的,其归一项是平移运动产生的光流.在这种情况下,准确匹配特征点的归一化重投影误差是相对稳定的,可以采用一个固定的阈值剔除误匹配点和运动点.但是车辆的实际运动是一个旋转与平移的耦合,因此需要将旋转与平移运动进行解耦以获得只有平移运动时产生的重投影误差以及由平移运动产生的光流.自运动估计分为两个部分:首先,先使用5-point RANSAC方法得到连续两帧与连续三帧之间的旋转,利用球形线性插值对其进行融合,得到优化的旋转参数.然后,使用旋转估计得到的结果,通过两帧间光束法平差结合归一化的重投影误差来估计平移.
采用如下解耦算法:
第1步:由于本文采用单目五点法进行旋转估计,只需使用左相机获取的图像序列,因此旋转估计不会受到左右相机标定不准确的影响.
本文将五点法与RANSAC算法结合使用,从所有的匹配点中随机取若干个5对点形成子集,计算每个子集的本质矩阵,利用求得的本质矩阵判断内点的数量,最后选取具有最大内点集的本质矩阵作为最终解,得到的旋转矩阵用Rt-1t(r).实验表明,如果继续用获得的最大内点集优化先前用五点法获得的最优本质矩阵,反而会增加平均误差.由于内点集的不同,该优化方法可能会收敛到局部极小值,从而使得到的解不准确[3].此外,最初的五点法还有两个优点:
1) 是一个封闭解,并且不依赖与初始猜测.
2) 是用最小的点集,使得外点对最终结果造成影响的概率降低.
为了达到平滑的目的,本文继续使用五点RANSAC方法计算出t时刻与t-2时刻之间的旋转矩阵,用Rt-2t(r)表示.由于前一帧的旋转Rt-2t-1,即t-2时刻到t-1时刻的旋转是已知的,因此当前帧的旋转可以用以下公式计算得到:
(8) |
Rt-1t′表示了由Rt-2t以及Rt-2t-1获得的t-1时刻到t时刻的旋转,Rt-1t(r)表示通过五点法结合RANSAC得到的t-1时刻到t时刻的旋转.将以上两种通过不同测量方式得到的当前帧的旋转使用球形线性插值(SLERP)进行融合,最终的旋转表示为Rnt:
(9) |
由于SLERP是在四元数空间中进行运算的,式(9)中的旋转矩阵用其四元数的形式表示.
第2步:将上一步中计算出的Rnt作为固定值,求解PnP,构建最小二乘问题(10),优化求解相机的平移向量T.当第m次与第m-1次得到的特征点集不变,或者到达了迭代的最大次数m=mmax,退出循环.初始特征点集为匹配得到的所有特征点,初始的位姿为上一帧计算的结果.
(10) |
迭代运算过程中,对每一个特征点计算平移引起的光流
其中,分子是在R固定,只有平移运动时产生的重投影误差,仍然用式(2)计算. h表示对坐标的最后一维进行归一化处理,σmthresh设置为所有特征点归一化重投影误差的均值.最终的运动估计(Rt,Tt)=(Rnt,Tmt).
3 实验结果本文实验在KITTI[19-20]公共数据集上进行.该数据集是车载视觉里程计系统的一个参考评估平台. KITTI数据集包含在市区、乡村和高速公路等场景中采集的20组校准过的立体图像序列,其行驶距离从500 m到5 000 m不等. KITTI提供了前11个序列的运动轨迹以及每帧图像的运动参数的真值.图像分辨率为1 240×376像素,采集帧率为每秒10帧.
系统采用C++编程,Linux Ubuntu 14.04操作系统,在主频为2.5 GHz的Intel(R)Core(TM)i5-7200U 4核处理器,内存为8 M的计算机中开发运行.
3.1 外点剔除效果图 3展示了与图 1相同场景的,根据KITTI提供真实运动参数得到的真实的内点集合.理论上来说,精确匹配的点根据真实的运动参数计算得到重投影误差应该为0,本文在计算内点集合时允许有1.5个像素的误差,也就是将阈值设置为1.5.与图 1比较,一些外点被删除,故而特征点有所减少. 图 4、图 5分别展示采用传统的重投影误差剔除外点之后保留的特征点以及采用归一化重投影误差剔除外点之后保留的特征点.传统重投影误差方法采用的阈值为2像素,归一化重投影误差的阈值为当前所用特征点集的归一化重投影误差均值的3倍. 图 4、图 5中,黄色编号表示该特征点为正确的内点,红色编号表示错误保留下来的点.
与图 3相比,图 4保留下来的正确的内点(黄色)减少了较多,尤其是距离较近的一些点,如编号为72、75、117、167等正确特征点被错误的剔除了,却保留了一些本身是错误的点(红色). 图 5保留下来的正确的内点(黄色)与图 3基本一致,减少的内点很少.可以看出图 5中仍然误保留了一些外点(红色),原因是在本文算法中,为了保证算法的实时性,设置最大迭代次数为3,如果增加迭代次数可以取得更好的效果.两种方法去除外点的效率用真阳率和假阳率来评估,统计值见表 1.实验证明,本文算法在外点剔除方面达到很好的效果.
使用KITTI数据集中的图像序列对本文所提算法进行了测试,并与P3P RANSAC[5]、RotRocc+[11]、ORBSLAM2[15]、SOFT2[13]四种算法进行比较,其中P3P RANSAC是经典的双目视觉里程计外点剔除的算法,RotRocc+也使用归一化重投影误差作为评判内点的标准,ORBSLAM2是著名的开源算法,SOFT2是KITTTI视觉里程计排行榜中第1位的算法.完整序列的运动估计误差分别由平均平移误差(ATE)和平均旋转误差(ARE)表示[19]. 表 2展示了用5种方法对KITTI数据库中前11个图像序列综合评估的平均平移误差和平均旋转误差.在KITTI数据集的性能测试中,本文算法的ATE为0.77%,ARE为0.002 2 (°)/m,相比于P3P RANSAC、ORBSLAM2、RotRocc+此三种算法,本文算法在性能上有所提高.
利用视觉里程计估计得到的每一帧的相机位姿参数可以复现车载相机的运动轨迹. 图 6、图 7展示用以上4种算法对KITTI数据库中00和01图像序列的轨迹构建以及和真实轨迹的比较. P3P RANSAC法的精度较低,ORBSLAM2、RotRocc+以及本文的算法都非常接近真实轨迹.文[21]提出采用绝对轨迹误差(RMSE)反映轨迹的跟踪精度,绝对轨迹误差用所有帧偏离真实轨迹的误差的平方和的均方根来表示.此3种算法与本文方法对不同图像序列的绝对轨迹误差比较结果见表 3,相比于其它3种方法,本文的算法绝对轨迹误差较小.
序列 | 本文算法 | RotRocc+[11] | ORBSLA2[15] | P3P RANSAC[5] |
00 | 4.398 78 | 4.972 76 | 5.366 53 | 47.021 9 |
01 | 10.909 9 | 15.309 7 | 19.809 5 | 127.116 |
02 | 4.864 13 | 7.265 32 | 7.151 13 | 69.503 9 |
03 | 1.351 21 | 1.450 27 | 1.501 72 | 4.671 79 |
04 | 0.216 46 | 0.366 05 | 0.380 37 | 2.694 17 |
05 | 1.197 68 | 1.296 48 | 1.443 49 | 19.876 4 |
06 | 2.257 45 | 2.416 31 | 2.818 03 | 4.198 32 |
07 | 0.794 24 | 0.891 24 | 0.913 41 | 5.732 52 |
08 | 5.966 28 | 6.506 14 | 6.104 09 | 44.535 |
09 | 2.026 38 | 3.013 48 | 2.409 15 | 24.340 6 |
10 | 3.619 98 | 3.819 38 | 4.338 21 | 2.986 71 |
mean | 3.498 25 | 4.282 47 | 4.748 69 | 32.061 6 |
表 4显示了本文算法与其它四种算法平均每帧图像的时间消耗.可以看出本文所提方法不仅在精度方面有所提高,实时性也不差.
4 结论用于自运动估计内点集合的确定对视觉里程计的鲁棒性及精度具有重要的影响.本文对传统的使用固定阈值的重投影误差的外点剔除算法进行了分析,采用光流归一化的重投影误差作为外点剔除评判标准,同时利用解耦算法,分别估计车辆的旋转和平移.使用5点法进行旋转估计,为了达到平滑的目的,结合了球形线性插值的方法将几帧间旋转进行了融合.估计平移向量时,利用已经得到的旋转矩阵补偿车辆的旋转运动,通过非线性优化的方法得到平移参数.在迭代过程中,采用对位置具有自适应的归一化重投影误差评估内点,选择最适合进行平移估计的特征点,以上两步结合可得到最优的运动估计.采用KITTI数据集中的场景序列对算法进行测试,实验表明该方法在外点剔除方面有显著提升,与其它同类的视觉里程计算法比较,本文算法在精度上有所提升.较之与使用重投影误差作为内点评判标准,采用光流归一化的重投影误差作为内点评判标准更为合理,但是光流本身具有不稳定的特性,光流不稳定性对精度带来的影响将作为后续的工作.
[1] | Scaramuzza D, Fraundorfer F. Visual odometry[J]. Robotics & Automation Magazine, 2011, 18(4): 80–92. |
[2] | Fischler M, Bolles R. Random sample consensus:A paradigm for model fitting with applications to image analysis and automated cartography[J]. ACM, 1981, 24(6): 726–740. |
[3] | Nistér D. An efficient solution to the five-point relative pose problem[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2004, 26(6): 756–770. DOI:10.1109/TPAMI.2004.17 |
[4] | Scaramuzza D. 1-point-RANSAC structure from motion for vehicle-mounted cameras by exploiting non-holonomic constraints[J]. International Journal of Computer Vision, 2011, 95(1): 74–85. DOI:10.1007/s11263-011-0441-3 |
[5] | Gao X S, Hou X R, Tang J, et al. Complete solution classification for the perspective-three-point problem[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2003, 25(8): 930–943. DOI:10.1109/TPAMI.2003.1217599 |
[6] | Badino H, Kanade T. A head-wearable short-baseline stereo system for the simultaneous estimation of structure and motion[C]//IAPR Conference on Machine Vision Application. Tokyo: IAPR, 2011: 185-189. |
[7] | Huai J, Toth C K, Grejner-Brzezinska D A. Stereo-inertial odometry using nonlinear optimization[C]//27th International Technical Meeting of the Satellite Division of the Institute of Navigation. Tampa, FL, USA: ION, 2015. |
[8] | Bellavia F, Fanfani M, Pazzaglia F, et al. Robust selective stereo slam without loop closure and bundle adjustment[C]//International Conference on Image Analysis and Processing. Naples, Italy: ICIAP, 2013: 462-471. |
[9] | Kitt B, Geiger A, Lategahn H. Visual odometry based on stereo image sequences with ransac-based outlier rejection scheme[C]//IEEE Intelligent Vehicles Symposium. Piscataway, NJ, USA: IEEE, 2010: 486-492. https://www.researchgate.net/publication/224167099_Visual_Odometry_based_on_Stereo_Image_Sequences_with_RANSAC-based_Outlier_Rejection_Scheme |
[10] | Buczko M, Willert V. How to distinguish inliers from outliers in visual odometry for high-speed automotive applications[C]//IEEE Intelligent Vehicles Symposium. Piscataway, NJ, USA: IEEE, 2016: 478-483. https://www.researchgate.net/publication/305709493_How_to_Distinguish_Inliers_from_Outliers_in_Visual_Odometry_for_High-speed_Automotive_Applications |
[11] | Buczko M, Willert V. Flow-decoupled normalized reprojection error for visual odometry[C]//IEEE International Conference on Intelligent Transportation Systems. Piscataway, NJ, USA: IEEE, 2016: 1161-1167. https://www.researchgate.net/publication/309731985_Flow-Decoupled_Normalized_Reprojection_Error_for_Visual_Odometry |
[12] | Adam A, Rivlin E, Shimshoni I. Ror:Rejection of outliers by rotation[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2000, 23(1): 78–84. |
[13] | Cvisic I, Cesic J, Markovic I, et al. SOFT-SLAM:Computationally efficient stereo visual slam for autonomous unmanned aerial vehicles[J]. Journal of Field Robotics, 2018, 35(4): 578–595. DOI:10.1002/rob.2018.35.issue-4 |
[14] | Mair E, Burschka D. z∞-Monocular localization algorithm with uncertainty analysis for outdoor applications[M]//Mobile Robots Navigation. London, UK: IntechOpen Limited, 2010: 107-129. |
[15] | Mur-Artal R, Tard's J D. ORB-SLAM2:An open-source SLAM system for monocular, stereo, and RGB-D cameras[J]. IEEE Transactions on Robotics, 2017, 33(5): 1255–1262. DOI:10.1109/TRO.2017.2705103 |
[16] | Rublee E, Rabaud V, Konolige K, et al. ORB: An efficient alternative to SIFT or SURF[C]//International Conference on Computer Vision. Piscataway, NJ, USA: IEEE, 2011, 58(11): 2564-2571. https://www.researchgate.net/publication/221111151_ORB_an_efficient_alternative_to_SIFT_or_SURF |
[17] | Dam E B, Koch M, Lillholm M. Quaternions, interpolation and animation[R]. Copenhagen, Danmark: University of Copenhagen, 1998. |
[18] | Nister D, Naroditsky O, Bergen J. Visual odometry[C]//IEEE Computer Society Conference on Computer Vision & Pattern Recognition. Piscataway, NJ, USA: IEEE, 2004: 652-659. |
[19] | Geiger A, Lenz P, Urtasun R. Are we ready for autonomous driving? The KITTI vision benchmark suite[C]//IEEE Conference on Computer Vision and Pattern Recognition. Piscataway, NJ, USA: IEEE, 2012: 3354-3361. https://www.researchgate.net/publication/261861888_Are_we_ready_for_autonomous_driving_The_KITTI_vision_benchmark_suite |
[20] | Geiger A, Lenz P, Stiller C, et al. Vision meets robotics:the KITTI dataset[J]. The International Journal of Robotics Research, 2013, 32(11): 1231–1237. DOI:10.1177/0278364913491297 |
[21] | Sturm J, Engelhard N, Endres F, et al. A benchmark for the evaluation of RGB-D SLAM systems[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway, NJ, USA: IEEE, 2012: 573-580. https://www.researchgate.net/publication/261353760_A_benchmark_for_the_evaluation_of_RGB-D_SLAM_systems?ev=auth_pub |