基于几何约束的视觉闭环检测和位姿优化

李 卓， 刘洁瑜， 周 伟

(火箭军工程大学导弹学院，西安 710025)

0 引言

视觉里程计(Visual Odometer,VO)仅利用摄像机的输入图像对载体的位姿进行估计[1]，近十几年来已广泛应用于各类机器人自主导航[2-3]。VO一般将局部运动估计级联得出摄像机的全局导航信息，从而使得误差不断积累繁殖，在长时间大范围情况下导航精度不高。

为实现大规模复杂环境下的准确导航，消除载体位姿估计的累计误差，闭环检测成为了视觉导航领域的基础问题和研究热点。所谓闭环检测就是为了使相机识别出曾经到达过的场景，进而校正累积误差。

由于闭环检测应用场景越来越复杂，环境规模越来越庞大，其对图像信息的压缩要求也越来越高，因此满足此要求的基于图像特征相似性的闭环检测在实际应用中占据了主导地位[4]，且大多采用视觉词袋(BoVW)[5-9]技术。其中：ANGELI等[5]研究了增量式视觉词典构建问题；CUMMINS等[6-7]采用Chow-Liu树继承关系的闭环概率方法；LABBE等[8-9]设计了内存管理机制,提高了实时性。由于采用不同的特征提取和描述算子，基于SURF算子的FAB-MAP算法[10]无法保证实时性；GALVEZ-LOPEZ提出了基于FAST检测算子和BRIEF特征算子的二进制视觉词袋技术，计算效率得到了显著提升[11]；MUR-ARTAL等[12]采用具有旋转不变性的ORB特征构建词袋，保证实时性的同时提高了稳定性；2016年LEVI[13]提出了LATCH二进制描述子，将以往二进制描述子的像素点对的比对转变为三元组图像块的F范数比对，在保证二进制描述子实时性的同时提高了鲁棒性。另外在闭环验证方面，基于文献[11]的DBoW2库只考虑图像中的特征描述，缺乏特征的几何约束。

基于以上问题与分析，本文建立基于图像关键帧数据库，并融合特征跟踪率与RGB直方图匹配的关键帧选取方法，降低信息冗余。采用视觉词袋查找当前帧的候选闭环关键帧，利用RANSAC-HORN运动估计方法来验证闭环，最终进行当前帧位姿优化。在KITTI数据集进行仿真实验，验证了该闭环方法的有效性，进一步提高了VO精度。

1 LATCH二进制描述子与视觉词袋

1.1 LATCH二进制描述子

通常二进制描述子由抽样模式、方向补偿以及抽样比对3部分组成，目前的几种二进制描述子在抽样的策略和辨别能力上均有所不同。

先前的二进制描述子以检测到的特征点为中心选取一定大小的图像块W，一个二进制描述子bW由T对抽样坐标序列S={st}t=1,…,n={[pt,1,pt,2]}t=1,…,n组成，其中，pt,1=(xt,1,yt,1)和pt,2=(xt,2,yt,2)定义在W坐标系。索引t既与W中的一对坐标关联，又与高斯光滑核σt=(σt,1,σt,2)t=1,…,n关联。对于每一抽样对st，比较pt,1和pt,2经过光滑后的灰度，从而设置二进制中的相应位的值，即

(1)

式中，(W(pt,1,σt,1),(W(pt,2,σt,2))是图像块W中坐标pt,1(pt,2)经标准差σt,1(σt,2)高斯滤波后的值。

最终的二进制串bW定义为

(2)

(3)

在运行时间方面，LATCH二进制描述子保持了二进制描述子的优势，比基于直方图描述子快一个数量级；在鲁棒性方面，LATCH二进制描述子在大多数数据集上的效果优于其他二进制描述子，缩小了与基于直方图描述子的差距[13]。

1.2 建立视觉词袋

在计算机视觉领域，图像通常以特征点及其描述来表达，以特征描述看作单词构建出相应的词袋模型，图像便可以转化为低维稀疏的数字向量，便于大量图像信息的处理，图像间的相似度转化为向量间的相似度，其本质是一个信息压缩的过程。

图1 K叉树视觉字典示意图Fig.1 Sketch map of the K-ary tree visual dictionary

根据文献[14]所采用的频率-逆文档频率(TF-IDF)方法为每个叶节点赋予权重。频率-逆文档频率为

(4)

式中:niIt为单词wi在图像中It出现的次数;nIt为It中的单词总数;N为训练数据库的图像总量;ni为单词i在整个训练图像库中出现的次数。

TF部分的思想是某单词在一幅图像中的出现次数越高，其区分度就越高。IDF思想是某单词在字典中出现的频率越低，则分类图像时区分度越高。TF-IDF越大，表明该单词在某图像中出现的频率高，并且在其他图像中较少出现，因此该单词具有很好的区别分类性。

2 关键帧闭环检测与验证

2.1 关键帧的选取

与传统的基于运动变化的关键帧选取方法不同，本文选取方法采用当前帧与上一关键帧之间的相似度。为提高相似度的可靠性，融合特征跟踪率与RGB直方图匹配评判相似度，相似度函数定义为

γ=αcolorhk-hc+αkeypoint·

(5)

在式(5)中可根据αcolor和αkeypoint来调整特征匹配和色彩直方图匹配的权重，鉴于特征数目较少时随机因素容易对关键帧的选取产生影响，因此本文根据特征数目适当调整αkeypoint，即

(6)

式中,me为期望的特征提取数量。

2.2 关键帧闭环检测

根据式(4)可计算图像It中特征点对应的单词及其权重，从而获得该图像的BoW，即

A={(w1,η1),(w2,η2),…,(wN,ηN)}vA

(7)

通过词袋可用向量vA描述图像A，而向量vA是稀疏向量，非零部分表达了图像中含有的单词种类，且这部分的值为TF-IDF的值。对于给定vA和vB,采用1范数形式来表示向量间的差异(图像间的距离),即

(8)

从式(8)可以得出：两图像越相似，s(vA,vB)得分越高(距离越小)，而当两图像无差别时，距离s(vA,vB)=1。为进一步提高评分的可靠性，在先验相似度s(vt,vt-Δt)基础上进行归一化，即

s(vt,vtj)′=s(vt,vtj)/s(vt,vt-Δt)

(9)

先验相似度表示某时刻关键帧图像与前一时刻关键帧的相似性，从而s(vt,vtj)′反映了当前帧与之前的关键帧间在先验相似度基础上归一化的相似度。当归一化相似度达到一定阈值条件时，对应的关键帧构成了闭环候选帧。

基于词袋的闭环检测依赖于外观而未利用任何几何信息，从而易造成外观相似的图像被检测为闭环，为了保证闭环检测的可靠性，需要对闭环进行验证，以及进行时间和空间一致性的检验后,闭环候选帧才会被认为是正确的。时间一致性需要设立闭环缓存机制，在一段时间内持续检测到闭环；空间一致性是对检测到的闭环两帧进行特征匹配，估计摄像机帧间运动，最后将该运动级联检查是否满足闭环要求。

2.3 基于RANSAC-HORN运动估计的闭环验证

与此同时也看到,词袋只考虑了图像中的特征描述，丢失了特征的几何约束。文献[11]采用随机抽样一致性(RANSAC)方法计算两幅图像之间的基本矩阵，判断其内点数是否足够，从而对当前帧和闭环候选帧进行几何一致性校验。由于双目相机不存在尺度问题，本文采用基于RANSAC-HORN运动估计的闭环验证方法。

HORN方法最少可以使用3对匹配的三维点求取图像间的运动信息，具体实现为：

1) 设当前关键帧的3个三维点质心坐标为Poc，则将三维点减去质心坐标得到质心坐标系下的新坐标Pc{Pc1,Pc2,Pc3}，同法可得到闭环候选帧3个三维点在其质心Pol坐标系下的新坐标Pl{Pl1,Pl2,Pl3}；

(10)

则此夹角对应的四元数向量为

(11)

(12)

5) 由qa和ql得到旋转矩阵Ra和Rl，则总旋转矩阵为Rz=Rl×Ra，平移T=Pol-RPoc。

RANSAC算法可实现对外点的滤除，对输入数据进行多次随机采样并确定模型，根据确定的模型对所有数据进行划分。经过多次随机抽样试验后会找到符合误差范围内最多点的集合，利用该内点集合确定最终的模型。考虑RANSAC算法的抽样点数尽量小的原则，根据HORN方法确定抽样点数的最小集为3对三维点。当将RZ和T确定后，可引导当前关键帧特征点在闭环候选帧中的大致区域，以及闭环候选帧中特征点在当前关键帧中的大致区域，查找更多匹配以弥补在词袋匹配环节的漏匹配，当匹配特征数目达到足够点数时，该闭环候选帧才被接收为成功闭环，从而实现了闭环验证。

3 闭环位姿优化

经过2.3节基于RANSAC-HORN运动估计的闭环验证后，已知当前关键帧与闭环关键帧之间的旋转矩阵RZ和平移T以及由此引导查找到满足一定数目的匹配点，则此闭环候选帧被接收为闭环关键帧，实现对闭环验证。为对当前关键帧位姿进一步优化，将闭环关键帧的局部地图点投影到当前帧并最小化重投影误差。

(13)

式中：ρ为具备鲁棒性的Huber代价函数;Σ为与特征点尺度相关的协方差矩阵，目标函数为

(14)

投射函数π定义为

(15)

式中：(fx,fy)为摄像机焦距；(cx,cy)为投影中心；a为基线长度。以上参数均能通过相机的标定来获取。

HΔx=-b

(16)

式中：b为系数向量；H为系数矩阵(信息矩阵)。通过求解式(16)线性方程组可得最小值Δx*，新的状态向量估计值可依据

(17)

进行更新，进而迭代直到满足预定要求。

常用的非线性最小平方算法有高斯-牛顿算法(GN)，列文伯格-马夸尔特算法(LM)等，均可以直接用来求最后的最优解。由于系数向量和系数矩阵在形式上都是稀疏的，一些开源的稀疏矩阵运算算法结合非线性最小二乘算法可提高计算效率。本文利用在图优化通用框架(g2o)[16]中的LM算法来实现非线性优化。

4 实验与分析

本文实验数据采用KITTI数据集[17-18]，该数据集是具备组合测量系统的车辆进行跑车实验而采集得到的，包括搭载有Flea2 Firewire摄像机采集的高质量双目图像序列(已预先矫正)和OXTS RT 3000 IMU/GPS测量系统采集的地理数据(作为真值)。运行计算机为DELL OPTIPLEX 7010台式电脑(Intel Core i5-3470 CPU，主频3.20 GHz，4.00 GB内存，64位的Windows7操作系统)。

在KITTI数据集中,00-10号图像序列是测试序列，包括了城市道路、小镇、乡村、高速多种环境，00,02,05,06,07,09号图像序列含有闭环，包含较多的相似场景。

图2所示为本文算法在KITTI 数据集上的重建轨迹，图中，红色线段为闭环检测到的路段，可以看出，在含有闭环的00,02,05,06,07,09号图像序列中全部正确识别闭环。

图2 本文算法在KITTI数据集的重建轨迹Fig.2 Reconstructed trajectory of the proposed algorithm on KITTI data set

经本文算法闭环优化后的重建轨迹,平移和旋转误差均显著减小。在含闭环的00,02,05,06,07,09序列下，双目VO平均平移误差1.071 4%，平均旋转误差为0.003 050 (°)/m；而本文提出基于闭环检测的VO平均平移误差0.835 8%，平均旋转误差为0.002 712 (°)/m。

在实时性方面，由于闭环检测采用关键帧和词袋技术，同时优化只是当前帧位姿优化而非全局优化，因此在实时性上略有下降,但并不影响VO的实时导航。

综上实验结果对比分析，本文提出的闭环检测和位姿优化算法能够有效地对闭环进行准确检测和验证，并对VO导航过程的误差积累进行闭环位姿优化，提高了视觉导航精度。

5 结束语

本文提出了一种基于几何约束的视觉闭环检测和位姿优化方法，旨在提高视觉导航精度。首先建立基于LATCH二进制描述子的视觉词袋，并采用视觉单词向量描述图像进而归一化相似度的闭环检测方法，然后对闭环候选关键帧和当前关键帧进行基于RANSAC-HORN运动估计的闭环验证，最后将闭环关键帧的局部地图点投影到当前帧并最小化重投影误差以使得位姿得到优化。数据集实验表明，本文提出的闭环检测和位姿优化算法能够有效、准确地对闭环检测和验证，并对VO导航过程的误差积累进行闭环位姿优化，提高了视觉导航精度。下一步将在本文基础上探索闭环验证后位姿的全局优化，以实现对闭环前路径的优化。

参 考 文 献

[1] YOUSIF K,BAB-HADIASHAR A,HOSEINNEZHAD R.An overview to visual odometry and visual SLAM:applications to mobile robotics[J].Intelligent Industrial Systems,2015(1):289-311.

[2] KONOLIGE K,AGRAWAL M,SOLJ.Large-scale visual odometry for rough terrain[J].International Journal of Robotics Research,2011(66):201-212.

[3] ANNAIYAN A,YADAV M,OLIVARES-MENDEZ M A,et al.Visual odometry based absolute target geo-location from micro aerial vehicle[C]//IEEE International Conference on Robotics,Automation,Control and Embedded Systems，2015:1-7.

[4] WILLIAMS B,CUMMINS M,NEIRA J,et al.A comparison of loop closing techniques in monocular SLAM[J].Robotics and Autonomous Systems,2009,57(12):1188-1197.

[5] ANGELI A,FILLIAT D,DONCIEUX S,et al.A fast and incremental method for loop-closure detection using bags of visualwords[J].IEEETransactionsonRobotics, 2008,24 (5):1027-1037.

[6] CUMMINS M,NEWMAN P.Probabilistic appearance based navigation and loop closing[C]//IEEE International Conference on Robotics and Automation，2007:2042-2048.

[7] CUMMINS M,NEWMAN P.Highly scalable appearance-only SLAM-FAB-MAP 2.0[C]//Robotics:Science and Systems,2009.doi:10.15607/RSS.2009.V.039.

[8] LABBE M,MICHAUD F.Memory management for real-time appearance-based loop closure detection[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems,2011:1271-1276.

[9] LABBE M,MICHAUD F.Appearance-based loop closure detection for online large-scale and long-term operation[J].IEEE Transactions on Robotics,2013,29(3):734-745.

[10] CUMMINS M,NEWMAN P.Appearance-only SLAM at large scale with FAB-MAP 2.0[J].International Journal of Robotics Research,2011,30(9):1100-1123.

[11] GALVEZ-LOPEZ D,TRADOS J D.Bags of binary words for fast place recognition in image sequences [J].IEEE Transactions on Robotics,2012,28(5):1188-1197.

[12] MUR-ARTAL R,TARDOS J D.Fast relocalisation and loop closing in keyframe-based SLAM[C]//IEEE International Conference on Robotics and Automation(ICRA),2014:846-853.

[13] LEVI G,HASSNER T.LATCH:learned arrangements of three patch codes [C]//IEEE Winter Conference on Applications of Computer Vision (WACV),2016:202-210.

[14] CUMMINS M,NEWMAN P.Accelerating FAB-MAP with concentration inequalities[J].IEEE Transactions on Robotics,2010,26(6):1042-1050.

[15] MUR-ARTAL R,MONTIEL J M M,TARDóS J D.ORB-SLAM:a versatile and accurate monocular slam system[J].IEEE Transactions on Robotics,2015,31(5):1147-1163.

[16] KUMMERLE R,GRISETTI G,STRASDAT H,et al.G2o:a general framework for graph optimization [C]//IEEE International Conference on Robotics and Automation (ICRA),2011:3607-3613.

[17] PAZ L M,PINIES P,TARDOS J D,et al.Large-scale 6-DOF SLAM with stereo-in-hand[J].IEEE Transactions on Robotics,2008,24(5):946-957.

[18] SCARAMUZZA D,FRAUNDORFER F.Visual odometry part Ⅰ:the first 30 years and fundamentals[J].IEEE Robotics & Automation Magazine,2011,18(4):80-92.