CT-ICP: Real-time Elastic LiDAR Odometry with Loop Closure

作者: Pierre Dellenbach, Jean-Emmanuel Deschaud, Bastien Jacquet, François Goulette · ICRA 2022

授權條款

• 開放取用
• 論文: CC BY 4.0

以下內容皆依據論文全文，依其 CC BY 4.0 授權條款使用；圖表已標示作者。

一句話摘要

每次掃描帶有起始姿態與結束姿態、並允許掃描之間跳變的掃描對地圖 ICP，加上以高程影像為基礎的迴圈閉合；在 KITTI 排行榜上 RTE 為 0.59%，每次掃描 60 ms。

為何重要

此方法只需要點座標與逐點時間戳記，就能在同一個框架內處理裝在車上、裝在 Segway 上與手持的 LiDAR。其駕駛設定下的里程計在單一 CPU 執行緒上每次掃描執行 60-80 ms，完整的 SLAM 只使用 LiDAR 資料。

問題

旋轉式多光束 LiDAR 在移動中記錄每一次掃描，因此必須針對感測器運動校正這些點。多數里程計方法以等速模型對一次掃描做一次去扭曲，這在方向或加速度突然改變時會失敗；而採用平滑軌跡的連續時間方法，則無法跟上真實平台高頻、晃動的運動。

提出的方法

CT-ICP 以一個起始姿態與一個結束姿態描述每次掃描，依每個點的時間戳記為其內插出一個姿態（旋轉用 slerp，平移用線性內插），並求解一個點對平面的掃描對地圖 ICP，其弱約束讓起始姿態保持在前一個結束姿態附近，並限制加速度。局部地圖是儲存在稀疏體素網格中的稠密點雲（每個體素最多 20 個點），而迴圈閉合模組在彙整後局部地圖的高程影像上匹配 2D 特徵，以 ICP 精修匹配，並最佳化一個 g2o 姿態圖。

關鍵見解

• 允許一次掃描的起始姿態與前一次掃描的結束姿態不同，可以吸收平滑的連續時間軌跡無法表示的高頻運動。
• 掃描內的彈性變形在原始資料上很重要：改用等速去扭曲，會讓 KITTI-raw 上的 RTE 從 0.55% 升到 0.79%，KITTI-360 上從 0.45% 升到 0.60%。
• 法向量與平面度權重是從稠密的局部地圖、而非目前的掃描計算而得，為點對平面殘差提供更豐富的鄰域。
• 稀疏體素地圖提供常數時間的鄰域存取，而非 kd 樹的對數時間存取，讓稠密地圖能即時使用。
• 在經過運動校正的 KITTI 掃描上，所有比較的方法都很接近；差異出現在原始、未校正的掃描上。

主要結果

• KITTI 里程計排行榜（測試序列 11-21）：RTE 0.59%，在當時提供公開程式碼的方法中排名第一。
• 在有真值的序列上的平均 RTE：經運動校正的 KITTI 為 0.53%（60 ms/掃描），KITTI-raw 為 0.55%（65 ms），KITTI-360 為 0.45%（70 ms）；IMLS-SLAM、MULLS 與 pyLiDAR F2M 在 KITTI-raw 上為 0.71-1.61%，在 KITTI-360 上為 0.68-1.55%。
• KITTI-CARLA（模擬）：RTE 0.09%，其他最佳里程計為 0.81%；ParisLuco：1.11%，IMLS-SLAM 為 1.26%。
• 高頻運動設定：Newer College 上 RTE 為 0.55%（pyLiDAR F2M 為 2.20%），NCLT 2012-01-08 上為 1.17%（pyLiDAR F2M 為 19.80%），每次掃描分別耗時 430 ms 與 180 ms。
• 迴圈閉合讓 KITTI-raw 00 上的 ATE 從 6.22 m 降到 0.66 m，KITTI-360 00 上從 29.87 m 降到 1.07 m，ParisLuco 上從 29.16 m 降到 9.65 m；在 KITTI-CARLA Town01 上則維持在 0.2 m 左右（0.21 m 到 0.26 m）。
• 每次建立並匹配高程網格平均耗時 1.1 s，找到迴圈時另需 1.2 s 進行姿態圖最佳化。

限制

• 迴圈閉合要求感測器的運動大致在平面上，且外參校正須讓 z 軸與地面法向對齊，這限制了可支援的感測器配置與運動。
• 迴圈閉合在主執行緒上執行（每個高程網格 1.1 s，加上每次姿態圖最佳化 1.2 s），因此完整的 SLAM 尚未達到即時；作者表示改用獨立的執行緒就能做到即時。
• 局部地圖不會在滑動視窗中遺忘點，因此一次不良的配準就會污染它；防範此問題的強健設定會增加執行時間（Newer College 上每次掃描 430 ms，NCLT 上 180 ms）。
• 所有實驗使用的 Gauss-Newton 求解器採用手動推導的 Jacobian 矩陣，因此其約束無法更改；較有彈性的 Ceres 選項需要多個執行緒才能即時使用。
• 迴圈閉合候選的數量（10）必須依預期的漂移調整，而在 KITTI-CARLA 上，迴圈閉合之後 ATE 並未改善。

對領域的影響

入圍 ICRA 2022 傑出論文獎，301 次引用。論文以 C++ 釋出 CT-ICP 里程計（github.com/jedeschaud/ct_icp），並稱之為公開且寬鬆授權的開源程式碼；迴圈偵測與姿態圖後端則放進 pyLiDAR-SLAM（github.com/Kitware/pyLiDAR-SLAM），並以其所述的形式在線上提供所使用的資料集。

摘要

多光束 LiDAR 感測器在機器人領域的使用日益增加，尤其是在自駕車上用於定位與感知任務，而這兩者都仰賴建立精確環境地圖的能力。為此，我們提出一種新的即時、只使用 LiDAR 的里程計方法，稱為 CT-ICP（Continuous-Time ICP，連續時間 ICP），並以一種新穎的迴圈偵測程序將其擴充為完整的 SLAM。此方法的核心，是在掃描匹配中引入掃描內的連續性與掃描之間的不連續性兩者的結合。它既允許在配準時對掃描進行彈性變形以提高精確度，也藉由不連續性提高對高頻運動的強健性。

我們在這個里程計之上建立完整的 SLAM，使用一種以高程影像 2D 匹配為基礎、快速的純 LiDAR 迴圈偵測，提供帶有迴圈約束的姿態圖。為了展示此方法的強健性，我們在七個資料集上進行測試：KITTI、KITTI-raw、KITTI-360、KITTI-CARLA、ParisLuco、Newer College 與 NCLT，涵蓋駕駛與高頻運動情境。CT-ICP 里程計與迴圈偵測都已在線上公開。在 KITTI 里程計排行榜上，CT-ICP 目前在提供公開程式碼的方法中排名第一，平均相對平移誤差（Relative Translation Error，RTE）為 0.59%，在單執行緒 CPU 上每次掃描的平均時間為 60ms。

連結

• arXiv 2109.12979: https://arxiv.org/abs/2109.12979
• DOI 10.48550/arXiv.2109.12979: https://doi.org/10.48550/arXiv.2109.12979
• PDF: https://arxiv.org/pdf/2109.12979v1
• 程式碼: https://github.com/jedeschaud/ct_icp