作者：陶博士，复睿微剑桥研究所 ADS算法科学家
本篇，我们讨论另一个定位的关键技术，基于环境特征的地图匹配定位。地图匹配定位是根据车辆相对于当地地标的位置来进行定位。在许多情况下，我们有提供当地地标的地图作为参考框架。基于先验地图，我们匹配历史中最相似的地图子集（图像/点云/特征点），根据匹配到的地图子集所提供的历史位姿真值、特征点坐标真值，计算点对间的变换矩阵，求解当前定位。地图匹配定位通常会输出相对的XYZ坐标，滚动/俯仰/偏航數值，或表示方向和空间自由的四元数。最流行的地图匹配定位算法有迭代最近点 (ICP) 以及正态分布变换 (NDT)。

为了进行地图匹配定位，我们必须有某种形式的传感器输入，例如激光雷达，雷达，相机等。使用激光雷达，可以会得到相对于基于点云的地图 (Point-cloud-based map) 的定位；使用雷达，可以得到相对于基于返回的地图 (Return-cloud-based map) 的定位；使用具有分类和深度的相机的位置，可以得到相对于基于对象的地图 (Object-based map) 的定位。

本文讨论主要集中在基于点云地图的 ICP 和 NDT 算法的原理和性能区别，以及相应拓展的深度学习⽅法。

01

基于点云的地图匹配方法
自动驾驶汽车中，摄像头被广泛应用，基于大量图像数据的深度学习应用的方式和功效已被广泛研究。然而，由于相机拍摄的图像只能记录二维的环境，不能用于准确估计三维环境，激光雷达（LiDAR）因此引起了业界的关注。在适当条件下，相比其他环境传感器，如摄像头和雷达，激光雷达更加精确和稳健，能够获取误差率在一厘米以内的三维距离信息。

在实际操作中，由于城市和郊区环境会不断变化，会影响匹配算法的精度。这些变化发生在不同的时间尺度上，包括动态和短暂的对象（例如停放的汽车），季节性的变化（植被、雪、灰尘）和人类影响，例如建造。因此，算法面对这些真实环境变化时的鲁棒性至关重要。其次，由于自动驾驶系统必须根据车辆行驶过程中环境变化做出实时决策，低延迟性极其关键。典型城市车速15~35mph，超过 100 毫秒的延迟将导致可能导致米级别的车辆位置不确定性，导致安全问题。因此，匹配算法需要同时考量精度和速度。

图 1. a: 实际地图 b: 点云地图 
作为业界最流行的三维环境点云匹配算法，ICP 和 NDT 已广泛用于地图构建和三维重建。图 2 展示了正态分布地图与点云地图之间的地图匹配过程。该过程对点云中的所有点执行匹配过程并重复，直到获得准确的汽车姿态估计。

图 2. 地图匹配流程 

迭代最近点 (ICP)

ICP 是最简单且易于实现的匹配算法，它用迭代的方法不断地最小化传感器数据和参考环境地图之间的点到点的欧几里得距离。在每一步中，选择离每个扫描点最近的参考点，并用最小化距离平方和来分别计算旋转和平移。

图 3. ICP（Udacity Self Driving Car Nanodegree）

正态分布变换 (NDT)

NDT 不像 ICP 那样使用点云的各个点，而是将位于三维像素内的 3D 数据点转换为正态分布。它把环境用局部概率密度函数 (PDF) 建模成一个平滑表面。参考点被分组为固定大小的单元格，形成三维像素网格，并用牛顿迭代法匹配最优的网格区间。

图 4. NDT（Udacity Self Driving Car Nanodegree）

ICP 与 NDT 的性能对比

相关的研究显示，当在小范围并有足够数量的特征时，这两种方法都表现得非常稳健，但 NDT 的运行速度比 ICP 快得多，并且定位误差要小得多。与 ICP 中关注单个点相比，将环境进行三维像素概率化的表达使得 NDT 能够获得环境的整体认知而不只是细节。基于 NDT 的方法具有更好的能力来处理实际变化，包括静态和动态的环境变化，以及计算效率更高。研究同时显示，NDT 的性能很大程度取决于三维像素分辨率的选择。换句话说，需要根据传感器精度以及环境条件，仔细地调整三维像素的大小，才能保证其性能。
然而，在进行大规模的环境匹配时，ICP 和 NDT 作为传统的点云匹配方法，都显示无法胜任所需的通用性和可扩展性，计算上也缺乏效率。

图 5. ICP 与 NDT 在不同像素下的性能对比

图 6. ICP 与 NDT 在转弯时的性能对比

02

地图匹配深度学习
基于深度学习的地图匹配一直存在着相当的挑战性，这与现有解决方案中常用的模型的局限性有关。现有方案把地图匹配理解为图神经网络（GNN）或序列到序列（seq2seq）结构，然而，这些方案无法完全捕捉 3D 点云中的空间分布和上下文线索，也无法进行并行处理，因而地图匹配的精度和速度都不理想。而且，现有解决方案常常会对原始密集的 3D 点云进行下采样操作，而导致局部几何信息的退化。深度学习的最新发展，尤其是在 Transformer 方面的研究，开始改变这个现状。
Transformer 同时考虑数据点的内部相关性轨迹，以及输入轨迹和输出路径之间的外部关系，通过并行的方式使用自我和多头注意机制提取多组特征表达，从而实现更精确和快速的匹配。以下，我们将细节讨论基于 Transformer 的 ICP 和 NDT 的深度学习模型。

DCP：基于 Transformer 的 ICP

ICP 及其变体为地图匹配提供了简单且易于实现的迭代方法，但是由于算法本身的局限性，常常收敛到虚假的局部最优。来自于自然语言处理和计算机视觉的最新发展 Transformer 提供了新的灵感。DCP 设计了一个模块，在获取本地特征值之后，通过捕获自我注意力和有条件的注意力机制来学习上下文线索。

图 7. DCP 网络架构

DCP 由三部分组成：

（1）DGCNN 模块将输入 3D 点云映射到固定的表征排列，

（2）基于 Transformer 的注意力模块，结合指针网络预测点云之间的软匹配，

（3）奇异值分解层（SVD）预测运动转变。

注意力模块的加入使得 DCP 模型能足够可靠地一次性提取对齐两个输入 3D 点云间的运动所需的对应关系，并且还可以通过经典 ICP 的迭代方法继续提高。相应的实验展示 DCP 不仅高效，而且优于 ICP 和它的变体。

图 8. DCP 性能对比
NTD-Transformer：基于 Transformer 的 NDT

NDT-Transformer 可以实现基于 3D 点云的实时和大规模地点识别。它采用三维空间的概率分布变换来压缩原始、密集的 3D 点云作为 NDT 单元，以提供几何形状描述表达。然后，利用 Transformer 网络从一组 NDT 单元中学习全局描述表征。受益于 NDT 和 Transformer 网络， NDT-Transformer 学习到的全局描述表征同时丰富了几何和上下文信息，然后使用查询数据库进行表征匹配检索，实现地点识别。与单独的 NDT 相比，NDT-Transformer 性能有明显的提升。

图 9. NDT-Transformer 网络架构

NDT-Transformer 主要由三部分组成：

1）由点变换和不确定性反向传播组成的 NDT 表达模块，

2）残差 Transformer 编码器，

3) 局部聚合描述表达的描述向量 (NetVLAD)。

在将 3D 点云 NDT 化之后，与大多数人的现有方法利用 KNN 分组对上下文信息建模的方法不同， NDT-Transformer 使用注意力机制来学习底层地标（NDT 单元）之间的上下文，并采用残差 Transformer 编码器聚合一个 NDT 单元与其他 NDT 单元的上下文线索，以增加表征的独特性。然后， NDT-Transformer 使用 NetVLAD 代替最大池化层来提高 3D 点云描述的排列不变性。NetVLAD 通过记录本地特征与参照特征之间区别的统计信息以及总和，将一组本地描述聚合生成一个全局描述表征向量。最后使用一个多层感知器 (MLP)，将一组 NDT 单元描述表征融合为一个固定大小的全局描述表征向量。
NDT-Transformer 通过分层地聚合和增强局部特征，从而在运行时间和匹配结果上获得极好的表现。在 Oxford Robocar 数据集上的实验结果表明 NDT-Transformer 能够成功地找到 SOTA 性能和运行时间之间的妥协。

图 10. NDT-Transformer 性能对比

04

小结
现有的研究中，并没有对都是基于 Transformer 的 DCP 和 NDT-Transformer 精确性进行直接对比，但理论上来讲， NDT-Transformer 在计算效率上具有更高的通用性和可扩展性。虽然因为篇幅的原因，我们无法详尽地含括所有的深度学习在地图匹配方面的应有，但令人欣喜的是最新的深度学习发展，尤其是 Transformer，通过引入自注意力机制和上下文的信息提取，给地图匹配算法提供了新的灵感和研究方向。

参考文献：

Development of a GPU-Accelerated NDT LocalizationAlgorithm for GNSS-Denied Urban Areas，Keon Woo Jang, Woo Jae Jeong and Yeonsik Kang, Sensors, 2022.

NDT-Transformer: Large-Scale 3D Point Cloud Localisation using the Normal Distribution Transform Representation，Zhicheng Zhou, Cheng Zhao, Daniel Adolfsson, Songzhi Su, Yang Gao, Tom Duckett and Li Sun, Arxiv, 2021.Deep Closest Point: Learning Representations for Point Cloud Registration, Yue Wang and Justin M. Solomon, Arxiv, 2019.Map-Matching Algorithms for Robot Self-Localization: A Comparison Between Perfect Match, Iterative Closest Point and Normal Distributions Transform, Heber Sobreira, Carlos Miguel Correia da Costa, Ivo M. Sousa and Luís F. Rocha, Springer, 2019.3D Scan Registration based Localization for Autonomous Vehicles - A Comparison of NDT and ICP under Realistic Conditions, Su Pang, Daniel Kent, Xi Cai, Hothaifa Al-Qassab, Daniel Morris and Hayder Radha, Arxiv, 2018.