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

0 引言

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

均定位误差始终保持最小,表明本文改进算法优于其他定位算法,验证了本文灰狼算法改进方法的有效性以及本文定位算法的有效性。信标节点的成本比较高,信标节点的数量直接影响整个网络的成本。本文定位算法当信标节点所占比例大于30%以后,平均定位误差变化趋于平缓,在相同定位误差下,本文算法需要的信标节点的数目较少,能有效的节省网络成本。

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

由于闭环检测应用场景越来越复杂，环境规模越来越庞大，其对图像信息的压缩要求也越来越高，因此满足此要求的基于图像特征相似性的闭环检测在实际应用中占据了主导地位[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)

这种对像素直接比对的方法易受噪声和局部表观变化的影响，进行预高斯光滑可以在一定程度上使其得到缓解，但是这也意味着在存在特征较多的高频区域内信息的缺失。LATCH二进制描述子引进了三元组像素块的比对方法，从而确定二进制中相应位的取值。考虑t=1,…,n个三元组序列，S转化为,pt,1,pt,2]}t=1,…,n，pt,a，pt,1以及pt,2给出了大小为k×k像素块中心像素的定位。将Pt,a定义为主像素块，Pt,1和Pt,2是其伴随像素块，通过计算之间的F范数来评估主像素块和伴随像素块的相似程度，由式(3)确定二进制串中的对应位的值。将g替换式(2)中函数f就确定了基于三元组方法的二进制串bW。

(3)

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

1.2 建立视觉词袋

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

本文采用LATCH二进制描述子构建视觉词袋，对训练集进行特征提取并将所有特征通过K-means聚类，从而所有特征被分为K类,构成层级词袋的第一层，以此类推,采用同样方法对每层聚类空间分为K类,构建出其对应的子层级，直至第Lq层为止，最终所有的Q个叶节点在Lq层形成K的Lq次方类，对每一类用其中的所有特征的平均特征作为代表(称为单词)，从而构建了一个如图1所示的具有Q个单词和个节点的树形视觉词典。树结构中的中间节点供快速查找给定特征对应单词时使用，只需将其与中间节点的聚类中心比较(一共Lq次)，即可查找到对应单词，保证了查找效率。

图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在整个训练图像库中出现的次数。

实际做法：按照两人一球的方式，让学生进行组合锻炼，先让两个学生之间进行背靠背的方式夹球尝试移动，然后再用头靠头的方式夹球尝试移动。移动的距离均控制为30 m。

投射函数π定义为

一是大幅放宽市场准入。率先在自贸试验区全面实施市场准入负面清单制度，分步骤推广至全市其他区域，抓紧完成事业单位改革、机构改革和产业园区改革。全面优化政府职能，理顺自贸试验区、金普新区、产业园区管理体制，把自贸试验区管委会机构职能做实。推进组建市场监管局，建立以信用为基础的跨部门信息共享平台，在各级政务服务大厅推广复制自贸试验区窗口无否决权 “OK”服务举措，为企业创新试错提供宽容审慎审批监管服务。

异步传输方式应用主要用于异步数据（报文数据及监控数据等）的传输。例如，民航自动转报系统从终端上扫描到登机乘客的身份信息，以报文格式通过异步口传送到FA16-T传输设备上，然后FA16-T传输设备再通过广域网传送到另外一台FA16-T传输设备上，该设备把数据再还原回异步数据，通过异步口发送给异地的自动转报系统，从而完成异步数据（报文格式）的传输过程。

2 关键帧闭环检测与验证

2.1 关键帧的选取

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

γ=αcolorhk-hc+αkeypoint·

(5)

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

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

(6)

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

2.2 关键帧闭环检测

——中共湖南省委党校副教授毛文璐撰文指出，党员干部组织和参与封建迷信活动，究其根源，是理想信念“缺钙”，党性修养缺失。而党员干部思想的“病变”，会引发其他“病症”。每一名党员都应坚守理想信念，抵制封建迷信，带头弘扬科学，把稳思想之舵。（《中国纪检监察报》11月8日）

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

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

(7)

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

(8)

式中：αcolor为RGB直方图匹配的权重，αkeypoint为LATCH特征匹配的权重，且αcolor+αkeypoint=1；mM为两幅图像匹配的特征点数目；mk,mc分别为第k个关键帧与当前帧的特征点数目；和为匹配的LATCH特征描述向量；hk，hc分别表示两幅图像的RGB统计直方图，采用的特征跟踪方法参见文献[15]。

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

(9)

以三才阵出战的袁安、上官星雨、李离三人，也在阵中迎来了七绝逍遥阵的第三变。三人既顺势而为，已经明白今夜七圣并非是以阵杀人，而是布阵调教佳子弟，如果发动杀阵，转动死门，以七圣之能，三人早已魂飞魄散，死去不知多少回了。

除此之外，SEW还带来了从德国引进的最新的五轴联动并联机器人。这是在传统三轴并联机器人的基础上，增加两根平行旋转轴的从而推出的五轴机器人，通过一个轴转动，同时另一个轴使产品翻转，低齿隙行星伺服减速机保证机器人的控制精度达到毫米级以上。据靳弃疾先生介绍，其通过SEW五个伺服电机，全新一代运动控制器，多轴伺服驱动器及高动态伺服电机，控制终端的五轴联动，实现五个自由度的运行。靳弃疾先生特别强调，SEW除了五轴并联机器人外，还能够根据客户个性化需求，为客户定制化数学模型，实现不同客户独特的的机器人运行。

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

确定考核结果后，人力资源部以书面的形式将审核结果传达给基层领导，该领导在一周内传达给部门员工，多种沟通方式相结合，帮助其保持现有成绩、认清自己的弱点，就绩效改进措施以及下阶段工作计划达成一致。主管人员及时做好反馈记录，人力资源部门用抽样调查法，对考核对象进行面谈，征集员工意见，力求更加科学合理的评定员工绩效。

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

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

通常情况下，不同的地区有着各自不同的土质，不同土质的稳定性也有着很大的差距，例如，陕西地区就属于黄土土质，又因为大多处于特殊的地质构造处，所以极其容易发生地面沉降的事故。另外，陕西地区的湿陷性黄土地基也较为严重，对地基处理不当是造成地基基础下沉的主要原因[3]。

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

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

定义图像对{Ia，Ib}各自有{N，M}个特征点，χ={w1，w2，wi，…，wN}是从图像 Ia到图像 Ib 中所有点最近的特征匹配，又定义{a，b}为图像{Ia，Ib}所分割出来的子区域，并且在两个子区域中各自拥有{n，m}个特征点。vi为第i个特征匹配，fa为在图像Ia部分区域a中的一个特征，Tab为区域{a，b}表示的是同一位置，Fab为{a，b}表示的是不同位置。表示fa的匹配的是一个正确的匹配表示 fa的匹配是错误的匹配为最近的一个特征是在b区域中。

2) 根据点集Pc和Pl分别确定的平面法向量nc=Pc1×Pc2和nl=Pl1×Pl2，以及单位法向量和

3) 令两平面间夹角φ满足

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

(10)

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

(11)

4) 定义变量和可得到平面内两组点集间的旋转角分别为

(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以及由此引导查找到满足一定数目的匹配点，则此闭环候选帧被接收为闭环关键帧，实现对闭环验证。为对当前关键帧位姿进一步优化，将闭环关键帧的局部地图点投影到当前帧并最小化重投影误差。

温泉水电站坝址河谷呈V形，河流纵坡6.9‰。谷底高程870 m左右，两岸山顶高程1 182～1 600 m，右岸自然坡角多在70°～80°之间，局部近于直立。坝址分布下石炭统东图精河组和第四系地层，岩体呈厚层状—块状。坝址基岩强风化层厚2～5 m，弱风化层厚10～15 m。左岸卸荷带水平宽度 3～15 m，顺河长 95 m，垂直深度约65 m；右岸卸荷带水平宽度2～5 m。坝址区地震基本烈度Ⅷ度，大坝抗震设防烈度Ⅷ度。

通过最小化世界坐标系下三维路标点Xi∈R3和当前关键帧的特征点之间匹配的重投影误差来实现对相机的旋转R∈SO(3)和位置l∈R3的优化，即

(13)

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

(14)

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

(15)

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

根据播种方法而定，一般条播用种量每667平方米0.7～0.8千克，撒播用种量667平方米1～1.5千克。

在相机的初始位姿已知情况下，利用一阶泰勒展开线性化目标函数F(x)，最终建立线性化系统，需求解的方程为

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,SOL J.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.