基于压缩地图的间接法视觉定位研究

王泽威，曾春年，杨 旭，罗 杰，胡锦敏

(1.武汉理工大学 自动化学院，湖北 武汉 430070；2.深圳市路畅智能科技有限公司武汉分公司，湖北 武汉 430205)

基于已知地图的定位是自动驾驶、移动机器人[1-2]、增强现实等应用领域的重要组成部分。出于经济性考虑，这些应用通常使用嵌入式主板作为计算平台，而已知地图由于数据量大，会占用大量存储空间。传统意义上，基于已知地图的视觉定位可以看成是一个在地图中的特征检索问题。通过使待定位图片的局部特征与地图中关键帧的局部特征建立匹配关系，经过几何校正后基于RANSAC算法求解PnP问题就能得到待定位图片的六自由度位姿。

视觉定位根据步骤的差异可分为间接法与直接法。间接法定位过程中需要通过图像召回得到地图中与待定位图片相似的地图关键帧，使待定位图片的局部特征与召回的地图关键帧进行特征匹配，从而建立关联。SCHONBERGER等[3]给出了完整的视觉建图与定位的框架Colmap，对单目相机采集的图像提取描述子并进行特征匹配，基于增量式运动恢复结构进行三维稀疏重建。在视觉定位方面，Colmap基于词汇树来计算待定位图像与地图关键帧之间的相似分数，并进行图像召回和后续的2D-2D图像匹配。而基于单目视觉的三维稀疏重建在实际应用过程中容易建图失败，无法得到符合真实物理世界的尺度，且在增量式运动恢复结构过程中当前三维结构的尺度容易发生漂移，导致最终得到的三维模型扭曲、不完整，同时Colmap依赖SIFT[4]特征进行图像召回和特征匹配，使得定位速度较慢。ORB SLAM[5]的回环检测部分采用词袋模型进行图像召回，然后提取ORB特征[6]使召回的图像与待定位图像进行两两匹配。由于采用了二进制描述子，该方法实时性较好，且对在线地图进行重定位效果较好,但二进制描述子很难胜任光照变化明显的场景。与间接法不同的是，SATTLER等[7]在直接法中舍弃了图像召回这一步，通过词汇树和最近邻搜索使待定位图像的局部特征与地图中的特征建立关联。直接法利用事先训练好的词汇树，基于最近邻搜索使地图中的特征向量与待定位图像中的特征向量与词汇树中的词汇产生关联，并将这种关联作为特征匹配的先验，从而减小了特征搜索的范围，提高了匹配速度。SATTLER等[8]又对直接法进行了补充，在得到待定位图片与地图中路标点的2D-3D对应关系后，利用KD-Tree[9]在已匹配上的3D点附近寻找其他未建立匹配的3D点，然后通过一个比较粗糙的词汇树反向搜索待定位图片中的特征并进行匹配，从而增加有效匹配的数目，提高定位成功率。总之，间接法更适合大尺度地图中的定位，直接法在小尺度地图定位中定位速度更快。

基于大尺度地图的构建、压缩和间接法视觉定位问题，已有研究存在单目建图算法建图成功率低、地图数据冗余且文件庞大、定位速度慢等不足。因此，笔者在Colmap单目稀疏重建的基础上进行改进，提出一种基于双目视觉的三维稀疏重建算法，给出一种地图关键帧和路标点的筛选策略和新的地图数据组织格式，使得在不影响定位成功率的前提下把地图文件尺寸降低到原来的20%以下，并在视觉定位过程中把描述子从SIFT替换成HF-Net[10],加入共视过滤，提高了定位成功率。

1 地图构建与压缩

1.1 稀疏三维重建

地图构建是一个离线过程，主要分为稀疏三维重建和地图关键帧局部特征的计算。其中，地图的稀疏三维结构主要通过运动恢复结构(structure from motion)得到[11]。运动恢复结构是利用三维结构在不同视角图像上的投影来进行重建的过程。增量式运动恢复结构则是一个包含了迭代步骤的串行流程:先提取图像的特征并匹配，再进行几何校正去除匹配中的错误匹配；将得到的匹配关系图作为稀疏重建的输入，选择两个关键帧并计算三维结构，得到初始模型；增量式地注册新图像，三角化特征点，过滤外点，利用集束调整(bundle adjustment)对重建结果进行优化，最终得到整个地图的稀疏三维结构。

特征匹配能够找到输入图像I={Ii|i=1，2，…,Ni}中的共视场景，并得到同一个点在不同图像中的投影，最终得到一系列经过几何校正的图像对和一个包含每个路标点到地图关键帧投影的关系图。对于每张图片Ii，提取Ii的特征点和对应的描述子，得到特征点和描述子的集合Fi={(xj,fj)|j=1,2,…,Ni}，其中i表示对图像的索引；j表示对一个图像中的特征点和描述子的索引；xj∈R2表示特征点在图像中的坐标；fj表示对应的描述子向量。描述子具备尺度不变性和旋转不变性，从而使得运动恢复结构过程能够在不同图像中对描述子进行识别。然后，将描述子作为对图像外观的描述，检测图像中看到相同场景的图像。由于图像序列是按时间顺序采集的，故可以根据时间顺序对图像进行特征匹配。这是因为在图像的采集过程中，时间上接近的图像通常有共同观测到的场景，它们之间可以顺利地建立特征点匹配。因此对于图像Ii，使Ii与Ii+1,Ii+2,…,Ii+r按序进行匹配。然而图像的采集可能多次经过同一场景，这使得有些图像在时间上不接近，但是它们的采集地点相似，依然可以顺利地建立特征匹配。对于这种情况，需要搜索采集时间早于Ii的图像Ii′(i′

Si,i′=‖h(Ii)-h(Ii′)‖2

(1)

原始的特征匹配存在错误匹配，会对稀疏重建的精度带来负面影响，因此需要通过几何校正去除错误匹配，从而保证所有互相匹配的特征点都对应三维空间中的同一个路标点。几何校正根据随机抽样一致算法估计每一组图像对的投影变换，使得图像上的特征点能够根据几何关系进行互相映射。根据每一对图像的几何特点，选择不同的映射描述它们的几何关系。单应矩阵H描述了一个旋转或者移动的相机拍摄一个平面的几何关系，对极几何通过本质矩阵E或者基础矩阵F描述了一个移动的相机在不同位置上拍摄的图像之间的几何关系。如果一个映射能够将一对图像中足够数目的特征点进行互相映射，那么这些能够互相映射的特征点就被认为是通过几何校正的正确匹配。通过先后估计单应矩阵H和本质矩阵E,几何校正保留了满足单应变换和对极几何的特征匹配，去除了不满足条件的匹配，而保留下来的特征匹配作为输出，用于后续的增量式运动恢复结构。

增量式运动恢复结构的输入是图像的匹配关系图，输出是每张图像的六自由度位姿和三维路标点的集合。运动恢复结构首先需要进行初始化，初始化选择两张特征匹配充足、空间分布合理的左目图像用于两视图单目重建，然后基于双目约束计算尺度，并对两视图的重建结果基于尺度进行相似变换。初始化过程中两张图像的选择十分重要，错误的初始化会导致最终的稀疏重建失败。

初始模型示意图如图1所示，C1和C2、C3和C4分别是两个双目相机的左目和右目,初始化时将C1坐标系作为世界坐标系，点P是根据x1和x3的匹配关系得到的世界坐标系下的3D点，它不具备真实物理世界的尺度，x1、x2、x3代表了C1、C2、C3对点P观测的齐次坐标。则尺度的最优解∂*可通过优化式(2)所示目标函数得到。

图1 初始模型示意图

(2)

式中：TRL为左相机坐标系到右相机坐标系的相对变换矩阵；TC1W为世界坐标系到C1坐标系的变换矩阵；n为C1、C2、C3坐标系共同观测到的3D点的个数；Pk为C1、C2、C3坐标系共同观测到的3D点中第k个3D点的齐次坐标向量；K为相机内参矩阵；x2,k为Pk在C2坐标系中的坐标向量；π(·)为投影函数。由于初始化时C1坐标系就是世界坐标系，因此TC1W为单位矩阵。

(3)

(4)

得到初始的三维结构后，对其他图像进行注册，根据特征匹配关系和当前重建结果输出的3D点，得到未注册图像与当前重建结果的2D-3D匹配关系，通过求解PnP问题得到其他图像的六自由度位姿。由于2D-3D匹配关系通常存在错误匹配，因此仍然需要通过随机抽样一致算法进行计算。新注册的图像必须观测到当前重建结果中的路标点，同时能够通过三角化增加路标点。根据新注册图像与已注册图像之间的匹配关系和已知的六自由度位姿，通过三角化得到新的路标点并加入到当前重建结果。三角化是运动恢复结构中的重要步骤，可提高当前重建结果的稳定性，增加新注册图像与当前重建结果之间的2D-3D匹配关系。

尽管图像注册和三角化的输出数据紧密相关，但它们的步骤是各自独立的，相机位姿的不确定性会传递到三角化后的路标点，反之亦然。如果没有进一步的优化，运动恢复结构通常会产生严重的漂移，导致重建失败。因此，需要引入集束调整(bundle adjustment)，通过最小化重投影误差来对相机位姿和路标点坐标进行联合优化。联合优化分为局部优化和全局优化，局部优化是对新注册图像附近的局部重建结果进行联合优化，减小局部累积误差；全局优化是对当前整个重建结果进行联合优化，减小全局累积误差。由于采用的是双目图像，故在进行局部优化和全局优化时还需要加入右目约束。右目残差定义为：

ER=π(TRL(TC1WPW,K))-x2

(5)

稀疏三维重建流程框图如图2所示，图像通过特征提取、特征匹配和几何校正建立图像之间的2D-2D匹配关系，然后选择两对图片进行初始化，得到一个初始的三维模型。在其余图像中，选择新的图像进行注册，根据新图像与已有三维模型之间的匹配关系三角化新的特征点，然后进行局部优化并剔除重投影误差较大的3D点(outlier)，完成局部优化后，若当前模型比上一次执行全局优化时的模型有较大增长，则执行全局优化并再次剔除outlier。当所有图像均完成注册后，则可输出一个完整的三维模型。

图2 稀疏三维重建流程框图

1.2 图像特征处理

近年来,基于深度学习的图像特征被广泛运用于多个领域，成为传统手工描述子的替代。卷积神经网络能够快速计算稠密像素级别的图像特征，为图像匹配和定位提供鲁邦的描述子表示。然而，硬件资源受限的计算平台难以匹配稠密特征，需要提取稀疏特征点，并对稠密特征进行稀疏采样。采样后的基于卷积神经网络的图像特征比传统手工描述子在抗光照变化上有更强的性能。

图像特征通过卷积神经网络HF-Net进行提取。HF-Net可以计算图像的特征点、稀疏局部特征、稠密局部特征和全局特征。对于地图关键帧中的特征点，需要通过双线性插值方法从稠密局部特征中进行插值，从而得到与特征点对应的256维局部特征向量。HF-Net能够计算地图关键帧的全局描述子，其维度达到4 096维，过高的维数导致全局描述子存在维度冗余。为了减少描述子维度，降低地图尺寸，需要通过主成分分析法进行降维。设地图中共有Z个关键帧，第i个关键帧对应的全局特征向量为gi，则全局特征向量的均值为：

(6)

(7)

建图与定位流程框图如图3所示，离线部分代表地图的预处理部分，地图中的关键帧经过HF-Net处理后得到稠密的局部特征和全局特征向量，结合2D点对稠密局部特征进行双线性插值，得到与2D点一一对应的稀疏局部特征。全局特征经过PCA降维后得到低维的全局特征和对应的投影矩阵。在线部分描述了对待定位图像进行定位的过程。待定位图像经由HF-Net处理得到对应的全局特征、特征点和稀疏局部特征。对全局特征进行降维后，利用N近邻搜索算法在地图关键帧的全局特征中搜索若干个接近的对象，即图像召回。根据召回图像的共视关系进行聚类，使待定位图像与每个类别的召回图像进行块匹配并利用RANSAC PnP算法求解位姿。

图3 建图与定位流程框图

1.3 地图稀疏化

增量式运动恢复结构输出的稀疏重建包含了密集的地图关键帧和大量3D路标点，导致地图文件尺寸过大，数据冗余。为了减小地图尺寸，去除冗余数据，需要对地图关键帧和3D路标点进行稀疏化，从而得到一个紧凑的地图，使地图仅保留对视觉定位最有用的信息。

间接法视觉定位需要对地图关键帧进行图像召回，即先计算待定位图像的全局描述子与地图关键帧的全局描述子的欧式距离，再通过排序取前几张欧式距离最小的地图关键帧作为结果返回，并进行后续的特征匹配。密集的地图关键帧增加了图像召回的耗时和地图信息冗余。地图关键帧的稀疏化是根据相邻关键帧的关系对关键帧进行筛选实现的，即遍历地图关键帧，对于当前帧判断以下指标：①当前地图关键帧与上一个被选中的关键帧之间匹配的特征点数目nt；②当前地图关键帧与上一个被选中的关键帧之间的距离l；③当前地图关键帧与上一个被选中的关键帧之间的关键帧数目y。如果nt小于阈值，或者l、y大于阈值，则选中当前帧，作为保留下来的地图关键帧。

对地图关键帧进行稀疏化后，需要对3D路标点进行稀疏化。通常情况下，每个3D路标点被观测的次数越多就越稳定，对视觉定位越有利，反之则越不稳定，是噪点的概率越大。对3D点的稀疏化需要尽可能保留被观测次数较多的点，舍弃被观测次数较少的点。3D点的稀疏化可以直观地构建成一个整数线性规划问题。整数线性规划的求解复杂度会随着待求变量维度的增加而呈几何倍数的增加，若不进行其他处理，则会导致压缩大尺度地图的过程十分缓慢。为了适用于大尺度地图，采用图分割算法将地图分成若干个子地图，再运用整数线性规划对每个子地图的3D路标点进行稀疏化。

图分割前需要将地图转化成一个拓扑图，以地图关键帧为节点，以地图关键帧之间的共视关系为边，将共视的3D点数作为边的权值，构建一幅拓扑图Gt。根据多路归并算法将Gt分割成若干个子图，使得每个子图尽可能独立，即连接不同子图之间的边最短。有的地图关键帧之间存在公共观测，即共视关系，根据关键帧的共视关系可以得到一个由顶点(关键帧)和边(共视关系)组成的无向图。利用多路归并算法将无向图分割成若干个连续的子图，每个子图都由地图中部分空间上连续的关键帧组成。图分割之后，对于每个子图，选取其中被观测次数最多的3D点用于视觉定位。为了使筛选的点在地图中均匀分布，设置阈值b和u,使每个子图中筛选的点不少于b，不大于u。通过最大化被观测次数，求解以下整数线性规划问题：

maxqTs

(8)

(9)

GTs≤u

(10)

s∈{0,1}M

(11)

设当前子地图中共有M个3D路标点,构造待求变量s=[s0,s1,s2,…,sM-1]T和参数变量q=[q0,q1,q2,…,qM-1]T，Ai=[ai,0,ai,1,ai,2,…,ai,M-1]T、G=[1,1,1,…,1]T。q为一个M维向量，保存了每个路标点被观测的次数，qm为第m个路标点被观测的次数；sm为二进制标量,sm=1表示第m个路标点被选中，sm=0表示第m个路标点未被选中；ai,m为二进制标量，ai,m=1表示子地图中第i个关键帧Ci观测到了第m个路标点；G为元素为1的M维常数向量，用来对s中非0元素进行求和；GTs表示被选中的路标点总数。构造完整数线性规划问题后，通过开源线性规划求解库Lpsolve对上述问题进行求解,可得到最优目标变量s。

1.4 地图格式设计

视觉地图要求在视觉定位过程中能够对特征进行快速检索，用高效精简的数据格式进行保存。Colmap使用易于检索的Sqlite数据库存储地图的2D信息，把稀疏重建的结果用另外的文件进行存储。然而数据库文件尺寸较大且不易压缩，同时在存储稀疏重建结果时对部分数据进行了二次存储，造成了数据冗余。因此把地图数据分成了路标点、观测、描述子和映射关系，并使用矩阵格式以二进制文件的形式进行存储，避免了数据冗余，提高了地图加载速度。

地图数据主要有3D路标点、2D观测、局部描述子、全局描述子、局部描述子到关键帧的映射和局部描述子到路标点的映射6部分。其中，描述子在地图文件中占据了大部分，故有效压缩描述子能够在很大程度上减小总体地图文件尺寸。一个HF-Net描述子是一个256维的浮点型向量，浮点型描述子的每一维都需要用4个字节来存储，这增加了文件尺寸，且常用的文件压缩算法对浮点型数据的压缩比1较低。因此把原始描述子的每一维乘以权值，并加上偏置，量化成单字节整型数据进行存储，可大幅度减小地图尺寸。

f′=wf+b

(12)

式中：f为原始的浮点型描述子；w为权值；b为偏置。

2 视觉定位

2.1 图像召回

图像召回是在地图关键帧中搜索与待定位图像相似的若干个地图关键帧。利用式(7)对待定位图像的原始全局特征gq进行降维得到g′q，再基于欧氏距离di衡量与地图中关键帧的相似度。

di=‖g′q-gi‖2

(13)

2.2 共视关系计算与聚类

图像召回后，对得到的候选关键帧计算共视关系并聚类。聚类的目的是划分不同的共视场景并提高匹配速度。一次有效的图像召回得到的地图关键帧中，会有相当数量的地图关键帧分布在待定位图像在地图中的正确位置附近，且通常互相共视。经过共视关系聚类后，有相同共视场景的关键帧被分成一类，于是就可以根据不同的共视场景，依次与待定位图像进行特征匹配。基于共视关系的聚类通过广度优先搜索算法实现，分类后根据每个类中关键帧的数目从大到小依次排序。图像召回得到的地图关键帧可能存在与待定位图像不符合的情况，这些关键帧通常单独成类，而关键帧数目较多的类则与待定位图像相符合的概率较大。

2.3 块匹配

块匹配是一种直接建立待定位图像与地图路标点之间2D-3D匹配关系的匹配方法。基于地图关键帧的分类结果，遍历每个类，取当前类中所有关键帧的局部描述子和对应地图中的3D路标点索引，分别构建矩阵Wf、Wl，Wf中每一行表示一个256维的局部描述子向量，Wl中每一行表示一个3维的路标点向量。设当前待定位图像的描述子矩阵为Wq,Wq中每一行表示待定位图像中的一个256维局部描述子，使Wf中的行向量与Wq中的行向量进行匹配，根据Wl中对应的3D路标点的坐标向量，通过低比例测试(即满足式(14)的匹配被认为是有效匹配)，建立2D-3D匹配关系，进而通过随机抽样一致算法计算待定位图像的六自由度位姿。

‖f′j-f0‖2<0.7‖f′j-f1‖2

(14)

式中：f′j为待定位图像Wq的第j个行向量；f0、f1为Wf中与f′j最接近的两个行向量，且来自两个不同的路标点。

两两帧的特征匹配需要进行大量的2D-2D几何校正来保证匹配的准确性，而2D-2D几何校正正是特征匹配过程中最耗时的步骤。相较于两两帧的特征匹配，块匹配基于低比例测试直接建立2D局部特征与3D点之间的匹配关系，无需进行2D-2D几何校正，故块匹配更高效。

3 实验研究

3.1 实验设计

实验分为3个部分：①对开源数据集和自采数据集进行基于双目的稀疏建图实验，并与Colmap原版单目重建进行对比，其中自采数据集是2020年4月24日在某产业园区采集的数据；②对双目稀疏重建得到的视觉地图进行压缩，对比压缩前后数据量；③对压缩前后的地图进行视觉定位，观察分析定位结果。视觉定位实验需要保证测试定位用的图片在地图范围以内采集，因此在实验时需要在不同时间对同一场景多次采集数据。实验采用的数据分别为2020年4月24日、4月26日、5月14日在同一个产业园区采集的数据。4月24日采集的数据用于双目建图，4月26日、5月14日采集的数据用于测试定位。实验平台环境：CPU为i7-9700KF，RAM为16 GB，GPU为RTX 2070 Super，操作系统为Ubuntu 16.04。

3.2 实验结果及分析

(1)基于双目图像的稀疏建图实验。原版Colmap单目建图和改进后双目建图的结果对比如图4所示。原版Colmap单目建图在一些数据集上不能得到完整的三维结构，从而导致建图失败，而改进后的基于双目视觉的建图算法能够在这些数据集上建图成功，得到具有全局一致性的稀疏三维结构。

图4 稀疏重建对比

(2)地图压缩实验。对基于双目视觉建图得到的地图进行压缩，分析关键帧数目、路标点数目和地图文件尺寸。地图压缩前后结果对比如表1所示，可知压缩后地图中的关键帧数目和路标点数目明显减少，地图文件尺寸也大幅度减少。

表1 地图压缩前后结果对比

对EuRoC-MH04序列数据集进行双目重建和地图压缩的结果如图5所示，可知压缩后地图中的关键帧和3D点明显均匀变少，但是地图的整体轮廓依然完整。

(3)视觉定位实验。不同时段对同一场景采集图像的视觉定位实验对比结果和轨迹图分别如表2和图6所示。由表2可知，改进后的定位比原版Colmap定位的成功率更高，定位速度更快，所需地图文件尺寸更小。从图6可以看出，对于A组实验，Colmap原版定位和改进后方法的定位轨迹基本一致；对于B组实验，改进后方法的定位轨迹有更好的覆盖率，在一些定位困难的地方依然能够得到靠近轨迹的定位结果。

表2 不同时间对同一场景采集图像的视觉定位实验对比结果

图6 视觉定位实验轨迹图

4 结论

(1)建图对比实验证明了改进后的双目建图算法比单目建图能够明显提高建图的成功率。

(2)通过图分割和整数线性规划对地图进行稀疏化处理，能够有效去除地图中的冗余信息，保留对视觉定位有利的路标点和关键帧，且结合单字节整型量化后能够大幅度减小地图文件尺寸。

(3)利用地图关键帧的共视关系对召回图像进行聚类，并与待定位图像进行块匹配，加速了定位过程。而且，地图压缩过程中减小了噪点密度，改善了定位过程中的有效定位结果。

(4)地图无法在定位过程中进行更新，而实际环境变化多样，地图场景也会随着时间变化，久而久之会降低定位成功率，因此如何在定位过程中实时更新地图，是未来值得探索的一个方向。