作者：陶博士，復睿微劍橋研究所 ADS算法科學家
本篇，我們討論另一個定位的關鍵技術，基于環(huán)境特征的地圖匹配定位。地圖匹配定位是根據(jù)車輛相對于當?shù)氐貥说奈恢脕磉M行定位。在許多情況下，我們有提供當?shù)氐貥说牡貓D作為參考框架?；谙闰灥貓D，我們匹配歷史中最相似的地圖子集（圖像/點云/特征點），根據(jù)匹配到的地圖子集所提供的歷史位姿真值、特征點坐標真值，計算點對間的變換矩陣，求解當前定位。地圖匹配定位通常會輸出相對的XYZ坐標，滾動/俯仰/偏航數(shù)值，或表示方向和空間自由的四元數(shù)。最流行的地圖匹配定位算法有迭代最近點 (ICP) 以及正態(tài)分布變換 (NDT)。

為了進行地圖匹配定位，我們必須有某種形式的傳感器輸入，例如激光雷達，雷達，相機等。使用激光雷達，可以會得到相對于基于點云的地圖 (Point-cloud-based map) 的定位；使用雷達，可以得到相對于基于返回的地圖 (Return-cloud-based map) 的定位；使用具有分類和深度的相機的位置，可以得到相對于基于對象的地圖 (Object-based map) 的定位。

本文討論主要集中在基于點云地圖的 ICP 和 NDT 算法的原理和性能區(qū)別，以及相應拓展的深度學習?法。

01

基于點云的地圖匹配方法
自動駕駛汽車中，攝像頭被廣泛應用，基于大量圖像數(shù)據(jù)的深度學習應用的方式和功效已被廣泛研究。然而，由于相機拍攝的圖像只能記錄二維的環(huán)境，不能用于準確估計三維環(huán)境，激光雷達（LiDAR）因此引起了業(yè)界的關注。在適當條件下，相比其他環(huán)境傳感器，如攝像頭和雷達，激光雷達更加精確和穩(wěn)健，能夠獲取誤差率在一厘米以內(nèi)的三維距離信息。

在實際操作中，由于城市和郊區(qū)環(huán)境會不斷變化，會影響匹配算法的精度。這些變化發(fā)生在不同的時間尺度上，包括動態(tài)和短暫的對象（例如停放的汽車），季節(jié)性的變化（植被、雪、灰塵）和人類影響，例如建造。因此，算法面對這些真實環(huán)境變化時的魯棒性至關重要。其次，由于自動駕駛系統(tǒng)必須根據(jù)車輛行駛過程中環(huán)境變化做出實時決策，低延遲性極其關鍵。典型城市車速15~35mph，超過 100 毫秒的延遲將導致可能導致米級別的車輛位置不確定性，導致安全問題。因此，匹配算法需要同時考量精度和速度。

圖 1. a: 實際地圖 b: 點云地圖 
作為業(yè)界最流行的三維環(huán)境點云匹配算法，ICP 和 NDT 已廣泛用于地圖構建和三維重建。圖 2 展示了正態(tài)分布地圖與點云地圖之間的地圖匹配過程。該過程對點云中的所有點執(zhí)行匹配過程并重復，直到獲得準確的汽車姿態(tài)估計。

圖 2. 地圖匹配流程 

迭代最近點 (ICP)

ICP 是最簡單且易于實現(xiàn)的匹配算法，它用迭代的方法不斷地最小化傳感器數(shù)據(jù)和參考環(huán)境地圖之間的點到點的歐幾里得距離。在每一步中，選擇離每個掃描點最近的參考點，并用最小化距離平方和來分別計算旋轉和平移。

圖 3. ICP（Udacity Self Driving Car Nanodegree）

正態(tài)分布變換 (NDT)

NDT 不像 ICP 那樣使用點云的各個點，而是將位于三維像素內(nèi)的 3D 數(shù)據(jù)點轉換為正態(tài)分布。它把環(huán)境用局部概率密度函數(shù) (PDF) 建模成一個平滑表面。參考點被分組為固定大小的單元格，形成三維像素網(wǎng)格，并用牛頓迭代法匹配最優(yōu)的網(wǎng)格區(qū)間。

圖 4. NDT（Udacity Self Driving Car Nanodegree）

ICP 與 NDT 的性能對比

相關的研究顯示，當在小范圍并有足夠數(shù)量的特征時，這兩種方法都表現(xiàn)得非常穩(wěn)健，但 NDT 的運行速度比 ICP 快得多，并且定位誤差要小得多。與 ICP 中關注單個點相比，將環(huán)境進行三維像素概率化的表達使得 NDT 能夠獲得環(huán)境的整體認知而不只是細節(jié)?；?NDT 的方法具有更好的能力來處理實際變化，包括靜態(tài)和動態(tài)的環(huán)境變化，以及計算效率更高。研究同時顯示，NDT 的性能很大程度取決于三維像素分辨率的選擇。換句話說，需要根據(jù)傳感器精度以及環(huán)境條件，仔細地調(diào)整三維像素的大小，才能保證其性能。
然而，在進行大規(guī)模的環(huán)境匹配時，ICP 和 NDT 作為傳統(tǒng)的點云匹配方法，都顯示無法勝任所需的通用性和可擴展性，計算上也缺乏效率。

圖 5. ICP 與 NDT 在不同像素下的性能對比

圖 6. ICP 與 NDT 在轉彎時的性能對比

02

地圖匹配深度學習
基于深度學習的地圖匹配一直存在著相當?shù)奶魬?zhàn)性，這與現(xiàn)有解決方案中常用的模型的局限性有關。現(xiàn)有方案把地圖匹配理解為圖神經(jīng)網(wǎng)絡（GNN）或序列到序列（seq2seq）結構，然而，這些方案無法完全捕捉 3D 點云中的空間分布和上下文線索，也無法進行并行處理，因而地圖匹配的精度和速度都不理想。而且，現(xiàn)有解決方案常常會對原始密集的 3D 點云進行下采樣操作，而導致局部幾何信息的退化。深度學習的最新發(fā)展，尤其是在 Transformer 方面的研究，開始改變這個現(xiàn)狀。
Transformer 同時考慮數(shù)據(jù)點的內(nèi)部相關性軌跡，以及輸入軌跡和輸出路徑之間的外部關系，通過并行的方式使用自我和多頭注意機制提取多組特征表達，從而實現(xiàn)更精確和快速的匹配。以下，我們將細節(jié)討論基于 Transformer 的 ICP 和 NDT 的深度學習模型。

DCP：基于 Transformer 的 ICP

ICP 及其變體為地圖匹配提供了簡單且易于實現(xiàn)的迭代方法，但是由于算法本身的局限性，常常收斂到虛假的局部最優(yōu)。來自于自然語言處理和計算機視覺的最新發(fā)展 Transformer 提供了新的靈感。DCP 設計了一個模塊，在獲取本地特征值之后，通過捕獲自我注意力和有條件的注意力機制來學習上下文線索。

圖 7. DCP 網(wǎng)絡架構

DCP 由三部分組成：

（1）DGCNN 模塊將輸入 3D 點云映射到固定的表征排列，

（2）基于 Transformer 的注意力模塊，結合指針網(wǎng)絡預測點云之間的軟匹配，

（3）奇異值分解層（SVD）預測運動轉變。

注意力模塊的加入使得 DCP 模型能足夠可靠地一次性提取對齊兩個輸入 3D 點云間的運動所需的對應關系，并且還可以通過經(jīng)典 ICP 的迭代方法繼續(xù)提高。相應的實驗展示 DCP 不僅高效，而且優(yōu)于 ICP 和它的變體。

圖 8. DCP 性能對比
NTD-Transformer：基于 Transformer 的 NDT

NDT-Transformer 可以實現(xiàn)基于 3D 點云的實時和大規(guī)模地點識別。它采用三維空間的概率分布變換來壓縮原始、密集的 3D 點云作為 NDT 單元，以提供幾何形狀描述表達。然后，利用 Transformer 網(wǎng)絡從一組 NDT 單元中學習全局描述表征。受益于 NDT 和 Transformer 網(wǎng)絡， NDT-Transformer 學習到的全局描述表征同時豐富了幾何和上下文信息，然后使用查詢數(shù)據(jù)庫進行表征匹配檢索，實現(xiàn)地點識別。與單獨的 NDT 相比，NDT-Transformer 性能有明顯的提升。

圖 9. NDT-Transformer 網(wǎng)絡架構

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

1）由點變換和不確定性反向傳播組成的 NDT 表達模塊，

2）殘差 Transformer 編碼器，

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

在將 3D 點云 NDT 化之后，與大多數(shù)人的現(xiàn)有方法利用 KNN 分組對上下文信息建模的方法不同， NDT-Transformer 使用注意力機制來學習底層地標（NDT 單元）之間的上下文，并采用殘差 Transformer 編碼器聚合一個 NDT 單元與其他 NDT 單元的上下文線索，以增加表征的獨特性。然后， NDT-Transformer 使用 NetVLAD 代替最大池化層來提高 3D 點云描述的排列不變性。NetVLAD 通過記錄本地特征與參照特征之間區(qū)別的統(tǒng)計信息以及總和，將一組本地描述聚合生成一個全局描述表征向量。最后使用一個多層感知器 (MLP)，將一組 NDT 單元描述表征融合為一個固定大小的全局描述表征向量。
NDT-Transformer 通過分層地聚合和增強局部特征，從而在運行時間和匹配結果上獲得極好的表現(xiàn)。在 Oxford Robocar 數(shù)據(jù)集上的實驗結果表明 NDT-Transformer 能夠成功地找到 SOTA 性能和運行時間之間的妥協(xié)。

圖 10. NDT-Transformer 性能對比

04

小結
現(xiàn)有的研究中，并沒有對都是基于 Transformer 的 DCP 和 NDT-Transformer 精確性進行直接對比，但理論上來講， NDT-Transformer 在計算效率上具有更高的通用性和可擴展性。雖然因為篇幅的原因，我們無法詳盡地含括所有的深度學習在地圖匹配方面的應有，但令人欣喜的是最新的深度學習發(fā)展，尤其是 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.