利用稀疏的语义视觉特征进行道路建图和定位(ICRA2021)（2）

下图展示了这篇文章所构建的因子图，这篇文章针对场景中需要构建的各个不同种类的路标各自建立了不同的参数模型，并通过图优化的方式对各个路标进行状态更新。

Initialization of ground and pole objects:

下面介绍上述的5个参数模型的初始化方法：

1.当GNSS-VIO轨迹给定时，通过三角化特征点的方式获取位姿估计。

2.我们使用它们所包含的deep points在XOY平面上进行二维线拟合。

3.之后，如果没有检测到地面初始化的地面标志，我们就使用每一帧中检测到的车道凸包内的传统特征点，并应用RANSAC三维平面拟合策略来去除移动车辆上的关键点。

Inilization of splines:

Offline mapping case

在这些变量被初始化后，我们根据常见的视觉-惯性测距约束推导出一个因子图优化，为了数值稳定，在上述因子上添加了Cauchy损失函数，并将第一帧的位姿固定。所有的关键帧和检测到的实例都参与到最后的BA调整中，以共同解决位姿和位置问题。

Online localization case

在在线定位过程中，我们从语义地图中反序列化固定的语义地标，即spline的控制点和常规的三维点，并将它们固定在公式4和5中，以增加对摄像机和地图坐标之间的相对位置的约束。在这个阶段，不再需要像公式6那样的共面约束。我们将在第II-I节中进一步介绍这些因素是如何通过提议的地图查询策略构建的。

Re-Identification and Feature Merging

我们进行3D-3D关联来重新识别语义对象，而不是进行框架式的词包查询。原因是重复对象的密度（几十米）相对于测绘过程中GNSS-VIO测距的定位不确定性要稀疏一些，而且这些标准化的道路元素之间的视觉外观过于相似，无法区分。在实例化的物体和车道关联过程中，我们将其中心点之间的距离小于5.0米（或车道为0.5米）的三角形物体视为相同的物体，然后以匈牙利策略逐级合并其包含的深层点和经典点的观测结果。深层点的语义类型被用于拒绝不匹配。而对于每个伴随的GFTT点，我们使用它们在多帧中的FREAK描述符进行投****。我们使用union-find算法来合并它们的观察结果，并进行另一轮全局状态优化。

Data Structure of Semantic Maps

对于每个观测c，我们在全局坐标TC中存储其估计姿态，以及用于在线GPS查询的最近的GNSS测量。对于每个分层语义地标，我们存储语义标签和所包含的深度和GFTT点的三维位置。在我们的语义地图中，既不存储FREAK描述符，也不存储框架性描述符。

Localization Based on Semantic Maps

我们使用上图所示的状态机来评估在线定位的位姿质量，并相应地执行不同的策略。从地图未初始化的状态开始，在这个状态下，从地图坐标到当前全局坐标TM的全局变换是未知的，我们使用粗略的GPS测量来检索相应的地图分区，以获得相应的观测。

在局部搜索模式下，我们可以获得地标子集LM=G(OM)，用于通过索引的深层物体关键点建立二维三维PnP-Ransac关联。要接受这样一个估计的PnP姿态是有效的，至少需要来自5个不同实例的12个传入点，以切换到两个跟踪状态并初始化TM。

我们使用两个阶段的跟踪状态，根据其位姿质量应用不同的阈值。在这两种跟踪状态下，重建的关联将通过公式4和5被添加到滑动窗口优化中，其中帧到全局变换的原始TC被帧到地图变换的TCM取代，而TM被视为优化过程中的一个额外的可优化变量。

在这样的两种跟踪模式中，我们将所有的地图实例投射到在线检测的帧c上，并使用以下标准，通过匈牙利式匹配接受投影关联，如。

Experimental Evaluation

Datasets

本文所构建的系统在KAIST数据集以及作者自己录制的两个数据集上进行了测试。

Performance

结果显示了我们训练的深度模型在KAIST数据集上的表现，由于它们在图像上的不同表现，盒子里的物体被分为三个有代表性的子类型（电线杆、交通标志和地面标志）。我们在测试集上分别评估这些任务，得出它们的分类精度、检测召回率，另外，表中的提取像素误差，下图为像素误差的分布直方图。

Evaluations on Localization and Mapping

在这个测试中，我们对每个序列运行两次，分别进行测绘和地图辅助定位。图7列出了测试序列的结果。我们提出的方法所产生的语义图需要的文件大小要少得多，同时提供了有竞争力的定位性能。例如，KAIST-38的语义地图占据了1.54MB的磁盘空间来达到0.46米的定位精度，而传统的地图在设置K=8时需要7.60MB来达到0.75米。对杭州序列的测试也反映了同样的趋势。总的来说，我们提出的方法中的语义图要比那些设定的覆盖点特征图小近5倍，才能达到类似的定位性能。

Cross-Relocalization

下表显示了这种对等的交叉定位结果。可以看出，我们提出的含有方框物体和道路车道的语义地图的方法在所有交叉验证测试中都完成了成功的定位。对于传统地图，我们用包括全尺寸地图在内的多种设置进行测试。不幸的是，由于在不同季节和时间段捕获的视觉特征的严重变化，在28序列的地图上运行KAIST-38序列的定位始终失败。因此，集合覆盖稀疏化需要应用更大的K值来进行交叉重定位，以保持足够可靠的特征。对于Maplab来说，由于保留的信息量大的三维点通常与更多的局部视觉描述符有关，因此结果的地图大小比预期的要大。相比之下，我们提出的方法依赖于从标准化和持久化的道路元素中提取的深层特征。总之，这种语义替换在交叉定位上比传统地图和Maplab都有更大的优势，对杭州序列的实验也反映了同样的趋势，这表明在紧凑性和定位精度上都有优势。

Modular and Efficiency Analysis

下表表示我们的定位所涉及的两种跟踪状态的统计数据。在大多数情况下，我们的方法在紧密跟踪模式下运行，这反映了更好的定位精度。

下表显示了来自不同语义实例的平均用点数量。由于相机可以观察和检测到比地面物体相对较远的有效的电线杆和标志物，这些物体对定位的贡献最大。

我们还分析了不同定位模式下语义对象关联的时间消耗。在在线定位期间，感知和地图查询模块都在一个独立的线程中运行。在感知模块中，如果单独检测，车道需要17.6毫秒/693MB，车道线检测需要7.5毫秒/422MB，其他物体和关键点检测需要17.3毫秒/1177MB。在地图查询模块中，本地搜索模式的平均时间消耗约为300毫秒，而对于占主导地位的松耦合跟踪或紧耦合跟踪模式，它减少到不到1毫秒。总体时间消耗低于我们设计的定位查询频率（1Hz）。

Stability of Semantic Mapping

下表显示了经典和语义建图方法中的建图误差。这表明引入语义对象并不明显影响建图的质量

Conclusion

本文提出了一个语义建图和定位流程。包括电杆、标志和公路车道在内的实例被检测到并被参数化，以形成一个紧凑的语义图，从而实现高效和准确的定位。