GPUを利用したReservoir Stein Particle Filterによる点群6自由度モンテカルロSLAM

Transcript

1. GPUを利用したReservoir Stein Particle Filterによる 点群6自由度モンテカルロSLAM 中尾拓道*1*2 小出健司*2 高野瀬碧輝*2 大石修士*2 横塚将志*2 伊達央*1 *1筑波大学 *2産総研
2. 1 10万パーティクルリアルタイム処理 表示点群地図：代表値パーティクル 橙点群：現在スキャン点群 10倍速表示 1倍速表示 10倍速表示 1倍速表示 5倍速表示
3. 2 研究背景  自己位置推定・SLAMにおける不確実性表現の重要性 [Thrun+, 2005] これまでの観測情報 における確率分布 現在の観測情報のみ における確率分布
4. 3 研究背景  自己位置推定・SLAMにおける不確実性表現の重要性 [Thrun+, 2005] これまでの観測情報 における確率分布 現在の観測情報のみ における確率分布
5. 4 研究背景  自己位置推定・SLAMにおける不確実性表現の重要性 [Thrun+, 2005] これまでの観測情報 における確率分布 現在の観測情報のみ における確率分布
6. 5 研究背景  SLAMにおける不確実性表現の重要性 [Thrun+, 2005] これまでの観測情報 における確率分布 現在の観測情報のみ における確率分布 正規分布近似に依存している以上不確実性の高い状況での推定が困難 不確実性の高い状況例 • どこも同じような風景の環境（森林内，長い廊下，トンネル） • 誘拐状態（センサを目隠しした状態で移動させる） 確率分布を正確に推論し不確実性の高い状況に対処したい
7. 6 関連研究 https://www.youtube.com/watch?v=JHTOd8yRJJc  RBPF SLAM[Griseti+, 2007] （モンテカルロSLAM） 軌跡にノイズを付与してサンプルを生成，サンプル集合から最適なサンプルを選び出す手法
8. 7 関連研究 https://www.youtube.com/watch?v=JHTOd8yRJJc  RBPF SLAM[Griseti+, 2007] （モンテカルロSLAM） 軌跡にノイズを付与してサンプルを生成，サンプル集合から最適なサンプルを選び出す手法
9. 8  SVGD（変分推論）[Liu+, 2016] （関連研究：Stein Particle Filter[Maken+, 2022] ） 確率分布の勾配情報（引力）とパーティクル近接関係情報（斥力）用いて 確率分布にフィットするようにパーティクルを輸送 サンプリング効率が高い 低確率パーティクルを保持できる 高次元になると破綻する[Ramdas+, 2015] （SLAMへのそのままの適用は難しい） 関連研究 SVGD (64 particles) 従来のパーティクル更新則 Resampling (1024 particles) MegaParticles[Koide+, 2024]
10. 9 関連研究  これまでの提案手法（ICRA2025，RSJ2026） 1. RBPF SLAM 2. パーティクル毎の大域軌跡最適化（ループクロージャ） 3. Stein Particle Filterの導入（最新姿勢へのSVGD適用） 確率分布の多峰性を柔軟に表現 SVGDでの二次勾配利用の理論的不足 計算量𝑂(𝑁2)… 課題 1000パーティクルで 6自由度RBPF SLAMを実現
11. 10  提案システムのキーアイディア 1. Matrix SVGD[Wang+, 2018] を導入し二次勾配（ヘッセ行列）を利用 高速な収束を図る 2. Weighted Reservoir Sampling[Efraimidis+, 2006] をSVGDに導入 計算量𝑂(𝑁2)から𝑂(𝑁)に 3. 各パーティクル毎に大域マッチングコスト最小化（グラフ最適化） 小ファクタ数でGLIM[Koide+, 2024] を10万パーティクルで実行 4. 大域マッチングコストを代表値抽出に利用 提案手法 多峰性表現 高精度化 安定化
12. 11 提案手法 点群 IMU LIOによる サブマップ 構築 予測 ノイズ付与 代表値抽出 SVGDによる最新姿勢更新 多峰性表現 ループクロージャ 高精度化 GPUを利用して10万パーティクルを並列処理  問題設定 𝑁個のパーティクル集合により状態（完全SLAM）の事後確率分布を推定し，MAP解を求める パーティクル状態： 姿勢系列 地図  システム概要
13. 12 代表値抽出 SVGDによる最新姿勢更新 多峰性表現 ループクロージャ 高精度化  
14. 13 ループクロージャ 高精度化  
15. 14 各パーティクル毎のモデル化 𝑥𝒌 1 𝑥𝒌+𝟏 1 𝑥𝒌+𝟐 1 𝑥𝑡−1 1 𝑥𝑡−2 1 𝑥𝑡 1 Particle 1 𝑥𝑙 𝑁 𝑥𝒍+𝟏 𝑁 𝑥𝒍+𝟐 𝑁 𝑥𝑡−1 𝑁 𝑥𝑡−2 𝑁 𝑥𝑡 𝑁 Particle 𝑁 VGICP Factor (Scan Matching) パーティクル毎に異なるファクタグラフを持つ  モデル化 軌跡全体をスキャンマッチングファクタで繋ぐ大域マッチングコスト最小化 GPU並列化が効くVGICPを採用
16. 15 各パーティクル毎の最適化と正規分布近似  iSAM2[Kaess+, 2011] 一般的なSLAMでのグラフ最適化手法 グラフ全体を知らないと最適化できない GPU並列処理に向かない  Gaussian Belief Propagation (GBP) 局所的なメッセージ（正規分布）のやり取りから 最適化＆正規分布近似 グラフの局所情報だけからメッセージを計算 大量のメッセージを並列演算可能 [Ortiz+, 2021]
17. 16 Gaussian Belief Propagation • Belief Update • Factor-to-Variable Message Passing • Variable-to-Factor Message Passing 𝑥𝑘 𝑓𝑙 各変数・ファクタノードが隣接するノードからの情報（メッセージ（正規分布））のみから最適化
18. 17 Gaussian Belief Propagation • Belief Update • • 𝑥𝑘 𝑓𝑙 各変数・ファクタノードが隣接するノードからの情報（メッセージ（正規分布））のみから最適化
19. 18 Gaussian Belief Propagation • • Factor-to-Variable Message Passing • 𝑓𝑙 各変数・ファクタノードが隣接するノードからの情報（メッセージ（正規分布））のみから最適化
20. 19 • • • Variable-to-Factor Message Passing 𝑥𝑘 Gaussian Belief Propagation 各変数・ファクタノードが隣接するノードからの情報（メッセージ（正規分布））のみから最適化
21. 20 Gaussian Belief Propagation • Belief Update • Factor-to-Variable Message Passing • Variable-to-Factor Message Passing 𝑥𝑘 𝑓𝑙 再帰的にメッセージを更新することで 真の事後分布に近づいていく 計算は情報行列形式 各変数・ファクタノードが隣接するノードからの情報（メッセージ（正規分布））のみから最適化 2姿勢のみ繋ぐVGICPファクタだけ考えればOK SE(3)空間でのGBPをGPUの並列演算能力を利用して高速に実行[Murai+, 2024]
22. 21 SVGDによる最新姿勢更新 多峰性表現  
23. 22 スカラカーネル 対数尤度の一次勾配 Matrix SVGD SVGD (64 particles)  Vanilla SVGD：変分推論に基づいて効率的にパーティクルを輸送するアルゴリズム 引力項 斥力項  Matrix SVGD Initialization SVGD Matrix SVGD [Wang+, 2018] 少ない反復で収束 行列値カーネル 対数尤度の二次勾配を含む
24. 23 Matrix SVGDを最新姿勢に適用  更新式 引力項 斥力項 スカラカーネルの和 MegaParticlesに倣い少ない反復でパーティクルを輸送ため 行列値カーネル スカラカーネル 最新姿勢近傍の姿勢インデックス 対数尤度の一次勾配 GBPより対数尤度の微分を得る
25. 24 Reservoir SVGD Matrix SVGDをStein Particle Filterに組み込めたが未だに計算量𝑂(𝑁2)  関連研究 MegaParticles：Locality Sensitive Hashing (LSH) を利用して，各パーティクルのSVGD更新に その近傍パーティクルのみを利用して計算量𝑂(𝑁)に[Koide+, 2024] 移動距離に伴って分散の変動が大きいSLAMでは不十分 とはいえ，推定初期では不確実性はほぼないので全パーティクルを利用する必要もない LSHとWeighted Reservoir Sampling (WRS) を組み合わせて近傍パーティクル分布を近似的に得る
26. 25 Reservoir SVGD  Weighted Reservoir Sampling (WRS) [Efraimidis+, 2006] 常に𝑘個のサンプルのみを保持するが，投げられたサンプル数に従って近似精度が向上 ? 未知の母集団 Reservoir （貯水池） サンプル候補をLSHによる 近傍サンプルに限る
27. 26 Reservoir SVGD  更新式 計算量𝑂(𝑁2)から𝑂 𝑁𝑁′ 𝑁′はWRSの近傍サンプル数 大量のパーティクル数（𝑵 = 𝟏𝟎𝟎, 𝟎𝟎𝟎）で近似的にMatrix SVGDの効果を得ることが可能に
28. 27 代表値抽出  
29. 28 代表値抽出 大域マッチングコストを利用し，代表値抽出に利用 VGGT[Jianyuan+, 2025] スキャンマッチングは確率モデルとして不十分 例：オーバーラップのないフレーム間は評価できない （正確な確率モデルを作るなら事前知識が必要） 最終的にループは閉じることを仮定 大域マッチングコスト ファクタ数 （ループを重視） 対応点数 （対応点数の違いを平滑化） 重み：
30. 29 森林内でのループクローズを模した実験  使用機器 • センサ ： Livox MID-360 • GPU ： NVIDIA GeForce RTX 4090  実験設定 • LiDAR座標系でX座標が負の点を除去（左半分の点群のみを利用） • 雑木林に面した道路でループクローズ • 真値と推定軌跡との絶対軌跡誤差で評価  比較手法 • SLICT[Nguyen+, 2023] • DLIOM[Wang+, 2022] • GLIM[Koide+, 2024] • RBPF SLAM (セルフ実装) • 前回までの提案手法 (ICRA 2025) 屋外環境
31. 30 森林内でのループクローズを模した実験 提案手法が一番の推定精度 他のグラフベースSLAM手法は誤った姿勢 間に制約を張ったため，推定精度が悪化
32. 31 4自由度誘拐実験  実験設定 • IMUにより重力方向はある程度分かることを仮定して，4自由度誘拐 • 青色の線の20秒間誘拐 • 既存手法ではまず問題設定 20秒間計測データを完全に遮断
33. ここからはより議論を発展させるため 投稿後に，より発展的な定量評価を行った結果と考察
34. 33 考察  提案システムの課題 1. 代表値抽出がヒューリスティックに頼っている 2. SVGDを上手く組み入れられているか分からない
35. 34 考察 最終的にループは閉じることを仮定 大域マッチングコスト ファクタ数 （ループを重視） 対応点数 （対応点数の違いを平滑化） 重み：  代表値抽出（再掲） VGGT[Jianyuan+, 2025] スキャンマッチングは確率モデルとして不十分 例：オーバーラップのないフレーム間は評価できない
36. 35 SVGDの効果検証  森林内を模したデータセットでAPEが3.0以下のパーティクル数で比較 • GBPによる大域マッチングコスト最小化 + SVGD • GBPによる大域マッチングコスト最小化  結果 各パーティクル毎のグラフ最適化のみ（パーティクル間の作用なし） のほうが正解パーティクル数が多い  考察 指折り数えられる程度の多峰性しかない問題なため， 各パーティクルが局所解に落ちる方が効率的になった
37. 36 考察  今回の実験以外に想定される問題 • 廊下やトンネルのような極度の縮退 • マルチループシーケンス front-end, back-endを分けないかつ，軌跡全体にSVGDを適用する必要がある  確率推論として評価ができていない 現状は確率推論を用いて最適値を得ることを目的としている 点群レジストレーションの確率モデリングを精緻化しSVGDの効果を厳密に検証していく
38. 37 まとめ • GPUを利用して10万パーティクルを並列処理し，4自由度誘拐に対処 • Matrix SVGDとStein Particle Filterを組み合わせて二次勾配情報（ヘッセ行列）を利用可能に • LSHとWRSを組み合わせて近似的なStein Particle Filterを提案し，パーティクル数を増大可能に • 投降後の定量的な実験と考察を述べた