大域マッチングコスト最小化とLiDAR-IMUタイトカップリングに基づく三次元地図生成 / GLIM @ Robotics symposia 2022

大域マッチングコスト最小化とLiDAR-IMUタイトカップリングに基づく三次元地図生成
第27回ロボティクスシンポジア最優秀賞受賞、2022年3月

Transcript

1. 大域マッチングコスト最小化とLiDAR-IMU タイトカップリングに基づく三次元地図生成 産業技術総合研究所 モバイルロボティクス研究チーム 小出健司 Mail: k.koide@aist.go.jp Web: https://staff.aist.go.jp/k.koide/
2. 本研究の提案
3. 本研究の提案 (1/2) • 近似の強い相対姿勢制約（ポーズグラフ）を排し，地図全体のマッチングコストを直接最小化する • 重なりのごくわずかな点群間に高精度に拘束をかけ，GPUを活用し超高速にグラフを最適化する 大域マッチングコスト最小化 [SuMa, RSS2018] Proposed
4. 本研究の提案 (2/2) LiDAR-IMUタイトカップリング • オドメトリ推定から大域最適化まで全ての最適化をLiDAR-IMUタイトカップリングで行う • 急激な動きや幾何特徴の乏しい環境に対して極めて頑強に推定を行う
5. 三次元LiDAR地図生成
6. 三次元LiDAR地図生成 (1/3) オドメトリ推定 • 直近のデータを使ったセンサ移動量の局所的な推定をリアルタイムで行う • 局所推定の繰り返しなので，推定誤差の累積（推定ドリフト）が不可避 Zhang et al., “LOAM: Lidar Odometry and mapping in real-time”, RSS2014 局所推定の繰り返し 推定誤差が累積 バックエンドで 累積誤差を補正 大域 最適化
7. 三次元LiDAR地図生成 (2/3) 大域軌跡最適化 • 地図全体を考慮し，オドメトリの推定ドリフトを補正する • 再訪地点のマッチングを行い地図の不整合が解消されるように全体最適化を行う • SuMa, RSS2018 • LeGO-LOAM, IROS2018 • LiO-SAM, IROS2020 • LiTAMIN, IROS2020 • LiTAMIN2, ICRA2021 • MULLS, ICRA2021 • R2LIVE, IEEE RA-L, 2021 • R3LIVE, IEEE RA-L, 2022 • S4-SLAM, Autonomous Robots, 2021 ほとんどの全ての手法の大域最適化は ポーズグラフ最適化で行われている
8. 三次元LiDAR地図生成 (3/3) 頂点：各時刻におけるセンサ姿勢 辺 ：時刻間のセンサ相対姿勢制約 ポーズグラフ最適化
9. 目的関数 三次元LiDAR地図生成 (3/3) 頂点：各時刻におけるセンサ姿勢 辺 ：時刻間のセンサ相対姿勢制約 ポーズグラフ最適化 グラフ全体の相対姿勢誤差を最小化 相対姿勢観測＝平均 情報行列＝共分散行列^-1 時刻i-j間の相対姿勢 相対姿勢制約を正規分布としてモデル化 全体の制約を考慮した最尤推定 𝐞𝐞𝑖𝑖𝑖𝑖 = log � 𝐓𝐓𝑖𝑖𝑖𝑖 −1𝐓𝐓𝑖𝑖 −1𝐓𝐓𝑗𝑗 𝑓𝑓 ℱ, 𝒯𝒯 = � 𝑖𝑖,𝑗𝑗∈ℱ 𝜌𝜌(𝐞𝐞𝑖𝑖𝑖𝑖 𝑇𝑇 𝛀𝛀𝑖𝑖𝑖𝑖 𝐞𝐞𝑖𝑖𝑖𝑖 )
10. ポーズグラフの問題点 共分散のモデリング • 相対姿勢(6DoF)の平均と共分散が必要 • スキャンマッチング結果の平均と分散…? 明示的に相対姿勢の平均を求める • 戻ってきたときに点群に十分な重なりがない • スキャンマッチングが不安定になる 実用上，共分散推定は難しい 多くの実装は正確に共分散を設定していない [CELLO-3D, ICRA2019] ループ制約を作ることができない 無理に作ると推定精度に悪影響を及ぼす
11. 大域マッチングコスト最小化 ポーズグラフ最適化 大域マッチングコスト最小化 • 部分部分で最適化した結果を正規分布近似 • 正規分布の集合として全体最適化 • 相対姿勢の正規分布近似を避ける • 直接マッチングコストをグラフ全体で最小化 スキャンマッチング (重なり誤差最小化) 相対姿勢制約 (SE3) ポーズグラフ ファクタグラフ (全体最適化) マッチングコスト制約 大域マッチングコスト最小化=マルチスキャンレジストレーション • 高精度（正規分布に落とさない） • 大きな重なりを必要としない（全体として拘束があればよい）
12. 過去の大域マッチングコスト最小化手法 (1/2) Lu and Milios, “Globally Consistent Range Scan Alignment for Environment Mapping”, Autonomous Robots, 1997 最初期のグラフSLAMの研究 • 全体マッチングコスト最小化として定式化 • グラフ構造を利用して効率に最適化 Borrman et al., “Globally Consistent 3D Mapping with Scan Matching”, Robotics and Autonomous Systems, 2008 Sprickerhof et al., “A Heuristic Loop Closing Technique for Large-scale 6D SLAM”, Automatika, 2011 • ループクローズを明示的にハンドリング 三次元化 なぜ使われなくなった…? めちゃくちゃ処理が重たい • 最適化のイテレーション毎に地図の全ての箇所で再度スキャンマッチングを評価 • 数千～数万FPSの速さのスキャンマッチングが必要
13. 過去の大域マッチングコスト最小化手法 (2/2) Reijgwart et al., “Voxgraph: Globally Consistent, Volumetric Mapping using Signed Distance Function Submaps”, IEEE RA-L, 2020 Voxgraph • サブマップをSDFで表現しマッチングコスト計算を高速化 • サブマップ間のマッチング誤差を地図全体で最小化 • コスト計算は依然重たく，マッチング誤差制約はランダムに選ばれた5%のみ使用 • 最適化には相対姿勢制約による補助が必要
14. 提案手法
15. 提案手法 GPUを用いたリアルタイム大域マッチングコスト最小化 全最適化ステージをLiDAR-IMUタイトカップリングで構築 • Generalized ICPアルゴリズムをGPU処理に適した形に変形 (Voxelized GICP) • GPUオーバーヘッドを最小化するように最適化システム全体を構築 • 大域マッチングコスト最小化＋IMUファクタ • オドメトリ推定から大域最適化まで全てタイトカップリングで構築
16. 提案手法 GPUを用いたリアルタイム大域マッチングコスト最小化 全最適化ステージをLiDAR-IMUタイトカップリングで構築 • Generalized ICPアルゴリズムをGPU処理に適した形に変形 (Voxelized GICP) • GPUオーバーヘッドを最小化するように最適化システム全体を構築 • 大域マッチングコスト最小化＋IMUファクタ • オドメトリ推定から大域最適化まで全てタイトカップリングで構築
17. 1. 各点の共分散の推定 2. KdTreeで最近傍点探索 3. 分布間距離が最小化されるように姿勢更新 Point residual: Point residual distribution: Matching cost: D2D matching cost Point residual Fused cov Generalized ICP 分布間距離に基づくICPアルゴリズム Segal et al., “Generalized ICP”, RSS2005 Red: source 𝒂𝒂𝒊𝒊 Blue: target 𝒃𝒃𝒊𝒊
18. 1. 各点の共分散の推定 2. KdTreeで最近傍点探索 3. 分布間距離が最小化されるように姿勢更新 Point residual: Point residual distribution: Matching cost: D2D matching cost Point residual Fused cov Generalized ICP 分布間距離に基づくICPアルゴリズム Segal et al., “Generalized ICP”, RSS2005 Red: source 𝒂𝒂𝒊𝒊 Blue: target 𝒃𝒃𝒊𝒊 線形代数なのでGPUで高速に計算可能 条件分岐が多数発生するので GPU (SIMT)に不向き
19. 1. 各点の共分散の推定 2. ターゲット点群をボクセル化 3. ボクセルハッシングで最近傍ボクセル探索 4. マルチ分布間距離が最小化されるように姿勢更新 Voxelized GICP ボクセル対応付けGICPアルゴリズム Koide et al., “Voxelized GICP for Fast and Accurate 3D Point Cloud Registration”, ICRA2021 Matching cost: Point residual: GICP: VGICP: Point residual distribution: GICP: Weight (# of points) Mean of points/covs Red: source 𝒂𝒂𝒊𝒊 Blue: target 𝒃𝒃𝒊𝒊 Code: https://github.com/SMRT-AIST/fast_gicp
20. 1. 各点の共分散の推定 2. ターゲット点群をボクセル化 3. ボクセルハッシングで最近傍ボクセル探索 4. マルチ分布間距離が最小化されるように姿勢更新 Voxelized GICP ボクセル対応付けGICPアルゴリズム Koide et al., “Voxelized GICP for Fast and Accurate 3D Point Cloud Registration”, ICRA2021 Matching cost: Point residual: GICP: VGICP: Point residual distribution: GICP: Weight (# of points) Mean of points/covs Red: source 𝒂𝒂𝒊𝒊 Blue: target 𝒃𝒃𝒊𝒊 Code: https://github.com/SMRT-AIST/fast_gicp ボクセル内の平均分布を 事前計算しておく
21. ボクセル化手法の違い 点が疎になる遠方ボクセルにおいても 正確な分布が得られる NDT：ボクセル内の点を統合 VGICP：事前計算した「点の分布」を統合 Deficient Valid Koide et al., “Voxelized GICP for Fast and Accurate 3D Point Cloud Registration”, ICRA2021 GICPと同等精度でGPU上で超高速に計算可能
22. GPU上での効率的なファクタ線形化処理 • 単純に逐次線形化するとCPU-GPU間同期が頻発しGPUの最高性能を発揮できない • タスク発行やデータ転送を全ファクタ一括で行うことで同期を最小限に抑える GTSAMのファクタグラフ線形化処理 ファクタを一つずつ線形化して足していく CPU-GPU同期がファクタごとに起きるので まったく速度がでない https://github.dev/borglab/gtsam/blob/develop/gtsam/nonlinear/NonlinearFactorGraph.cpp
23. GPU上での効率的なファクタ線形化処理 Nonlinear Factor Graph Task List GPU RAM Nonlinear Factor Nonlinear Factor Nonlinear Factor Serialized Memory Blocks Nonlinear Factor GPU-based Factors Linearized Factor Graph • 単純に逐次線形化するとCPU-GPU間同期が頻発しGPUの最高性能を発揮できない • タスク発行やデータ転送を全ファクタ一括で行うことで同期を最小限に抑える GPU CPU CPU-GPU Synchronization CPU-GPU Synchronization Linearization Linearization Linearization Linearization Issue Linearized factors Linearization point
24. 大規模な問題になればGPUオーバーヘッドが隠蔽されてCPU-GPU速度差はさらに大きくなる 5サブマップの同時アラインメント 4 + 3 + 2 + 1 = 10ファクタ 最適化速度比較 アルゴリズム 時間 [msec] 速度(ICP比) ICP 4620 1.0 GICP 1900 2.4 VGICP (CPU) 830 5.6 VGICP (GPU) 20 ~ 40 116 ~ 231 𝑥𝑥0 𝑥𝑥1 𝑥𝑥2 𝑥𝑥3 𝑥𝑥4
25. 重なりの小さな点群間のループ (LIO-SAM) データ提供：Map IV, inc. LIO-SAM ポーズグラフベースでは筋違いの道でループを閉じるのが難しい 上下方向誤差 ループを作れない
26. 重なりの小さな点群間のループ (提案手法) 僅かな重なりさえあれば，ループを閉じてドリフトを補正可能 (重なり率5%以上でマッチングコストファクタを生成) Proposed データ提供：Map IV, inc.
27. 提案手法 GPUを用いた完全なリアルタイム大域マッチングコスト最小化 全最適化ステージをLiDAR-IMUタイトカップリングで構築 • Generalized ICPアルゴリズムをGPU処理に適した形に変形 (Voxelized GICP) • GPUオーバーヘッドを最小化するように最適化システム全体を構築 • 大域マッチングコスト最小化＋IMUファクタ • オドメトリ推定から大域最適化まで全てタイトカップリングで構築
28. LiDAR-IMUフュージョン ルーズカップリング • LiDARとIMUそれぞれで最適化が行われる • それぞれの最適化結果を統合して最終姿勢を得る IMU LiDAR 誤差関数 𝐼𝐼 誤差関数 𝐿𝐿 姿勢 𝐼𝐼 姿勢 𝐿𝐿 最終姿勢 最適化 最適化 長所：計算が軽い システムが簡単 短所：精度が悪い (近似が強い) 縮退に弱い 例：典型的なEKFなど タイトカップリング • LiDARとIMUの誤差関数を合成した目的関数を直接最適化 IMU LiDAR 誤差関数 𝐼𝐼 誤差関数 𝐿𝐿 最終姿勢 合成誤差関数 最適化 長所：高精度 縮退に強い 短所：計算が重い
29. 近年のLiDAR-IMUオドメトリ手法 最近のLiDAR-IMUオドメトリは 大体タイトカップリング • lio-mapping, ICRA2019 • LINS, ICRA2020 • FAST-LIO, IEEE RA-L, 2021 • FAST-LIO2, IEEE RA-L, 2021 計算量の問題でオドメトリ推定のみ ポーズグラフを使用すると必然的に LiDAR/IMUの最適化が分離してしまう マッチングコスト最小化グラフを使い，大域最適化もLiDAR-IMUタイトカップリングで構築する Tightly-Coupled LiDAR-IMU Odometry
30. 提案手法 (LiDAR-IMU タイトカップリング) 全最適化ステージをLiDAR-IMUタイトカップリングで構築 オドメトリ推定 (Fixed-lag smoothing) 大域最適化 (iSAM2でインクリメンタル全体最適化) • オドメトリ推定はキーフレームベースマッチングで低ドリフトな推定を実現 • 大域軌跡最適化はマッチングコストファクタをIMUファクタで補助 • 分散を小さく保つために，サブマップの最初・最後のフレーム状態間にIMUファクタを生成 ※最新フレーム(𝑥𝑥9 )に関わるファクタのみ表示
31. 提案手法 (LiDAR-IMU タイトカップリング) 幾何特徴の少ない環境 重なりの小さいフレーム 変則的なセンサ配置でも問題なくループを作ることが可能 回転型(velodyne)以外の任意の種類の距離センサにも対応可能
32. 手法評価
33. http://www.cvlibs.net/datasets/kitti/eval_odometry.php Koide et al., “Globally Consistent 3D LiDAR Mapping with GPU-accelerated GICP Matching Cost Factors”, IEEE RA-L, 2021 評価実験 (LiDAR単体) Test Seq. Valid Seq. 1位 2位 (LiDAR単体) 5位 (Visual含む全体140手法中)
34. Ablation Study (LiDAR単体) 精度劣化 • マッチングコストを相対姿勢ファクタに置換 • 相対姿勢はGICPで求める • 初期姿勢はマッチングコストグラフの最適化 結果から与える • 情報行列はGICPのヘッセ行列を用いる • Huberカーネルで外れ値を除去する ポーズグラフとの比較 相対姿勢制約と推定値との誤差 Green：Small error Red：Large error 重なりの小さい遠方フレーム間では スキャンマッチングに失敗 Koide et al., “Globally Consistent 3D LiDAR Mapping with GPU-accelerated GICP Matching Cost Factors”, IEEE RA-L, 2021
35. 評価実験 (LiDAR-IMU) 精度比較 (Newer College Dataset) 実行時間評価 (KAIST Urban Dataset) https://ori-drs.github.io/newer-college-dataset/ https://sites.google.com/view/complex-urban-dataset 1秒未満で大域最適化可能 Proposed LIO-SAM
36. まとめ
37. まとめ 大域マッチングコスト最小化 • 近似の強い相対姿勢制約（ポーズグラフ）を排し，地図全体のマッチングコストを直接最小化 • 重なりのごくわずかな点群間に高精度に拘束をかけ，GPUを活用し超高速にグラフを最適化 LiDAR-IMUタイトカップリング • オドメトリ推定から大域最適化まで全ての最適化をタイトカップリングで構築 • 急激な動きや幾何特徴の乏しい環境に対して極めて頑強に推定 コード公開準備中 Follow our twitter account!! @MR2T_AIST • 任意の距離センサに適用可能