GLIM とMegaParticles：正規分布近似の限界とタイトカップリング＆パーティクルフィルタの進展 / GLIM and MegaParticles : Progress of the distribution representation in SLAM

ROBOMECH2026ワークショップ：SLAMの現在地：到達点・課題・展望
2026年6月29日

Transcript

1. GLIM と MegaParticles ： 正規分布近似の限界とタイトカップリング＆パーティクルフィルタの進展 産業技術総合研究所 インテリジェントシステム研究部門 スマートモビリティ研究グループ 小出健司 2026 年6月29 日 ワークショップ： SLAM の現在地：到達点・課題・展望 （ …と Feed Forward Geometry Transformers ）
2. 2 SLAM の研究は終わったのか？ 良く聞く意見 • 最近のSLAM は高精度すぎる • SLAM の研究で何したらいいかわからない • ニューラルネットワークで解決されたと聞いた 良く受ける質問： 「 SLAM の研究でまだやることありますか？」 本日はこれまでの SLAM 研究の経験と、最近の手法を見て思った個人的な見解を共有します
3. 3 今日の話の流れ Foundation ：確率ロボティクス Classic ： EKF SLAM hdl_graph_slam ：ポーズグラフ最適化 GLIM ：大域マッチングコスト最小化 MegaParticles ：ノンパラメトリック分布推定 VGGT： Feed Forward Geometry Transformer
4. 4 今日の話の流れ Foundation ：確率ロボティクス Classic ： EKF SLAM hdl_graph_slam ：ポーズグラフ最適化 GLIM ：大域マッチングコスト最小化 MegaParticles ：ノンパラメトリック分布推定 VGGT： Feed Forward Geometry Transformer
5. 5 確率ロボティクス 実世界で行動するロボット を実現するには 観測や状態の不確かさ に向き合う必要がある • 現実世界の確定的な情報は観測できない • ロボットの状態も確定的に求めることはできない 観測・状態の曖昧さ（不確かさ）を考慮・伝搬して、信頼できるロボットを作る MBA -VO: Motion Blur Aware Visual Odometry https://www.photomodeler.com/photomodelers - new -point -cloud -meshing -capabilities/ Probabilistic Robotics モーションブラー 点群ノイズ 決定不可能な位置状態
6. 6 確率ロボティクスと SLAM Full SLAM 問題（確率ロボティクス 式10.2 ） 抽象度が高く、非常に懐の深い定式化 • 状態・環境・観測を拡張して、あらゆる SLAM問題を包含できる • 推定した状態分布を信念分布として POMDP などの行動計画にもつなげられる 𝑝 𝑥1:𝑡 , 𝑚 𝑧1:𝑡 , 𝑢1:𝑡 ) 推定状態 （姿勢） 周囲環境 （地図） 観測 （センサデータ） 入力 （車輪など） この考えを突き詰めれば「真に知的な行動」ができるエージェントが実現できる
7. 7 確率ロボティクスと SLAM 素朴な疑問：定式化が完了しているのになぜ SLAM は未だに難しいのか 𝑝 𝑥1:𝑡 , 𝑚 𝑧1:𝑡 , 𝑢1:𝑡 ) 状態 環境 観測 入力 SLAMの研究はこの数式（分布表現・推定）を現実世界で実現するための取り組みそのもの 今日はこれまでの SLAMの研究の流れを追いかけて、今後のことも考えてみる 現実世界の複雑さを数式で表現しきれない • 状態＆観測の次元が膨大 • 各要素に絞っても複雑な分布形状 • ロボット上で使える計算機 の性能の限界
8. 8 今日の話の流れ Foundation ：確率ロボティクス Classic ： EKF SLAM hdl_graph_slam ：ポーズグラフ最適化 GLIM ：大域マッチングコスト最小化 MegaParticles ：ノンパラメトリック分布推定 VGGT： Feed Forward Geometry Transformer
9. 9 古典： EKF SLAM 観測・状態遷移分布を正規分布で近似 https://www.youtube.com/watch?v=vLmMtbc7804 https://jp.mathworks.com/videos/understanding -kalman - filters -part -3-optimal -state -estimator -- 1490710645421.html 任意分布を正規分布で局所近似 正規分布の積で状態を更新 姿勢＆環境状態を同時推定 エレガントな定式化でモデル化できている範囲ではよく動作する
10. 10 EKF SLAM の弱点：スケール性の悪さ 状態数の2乗でメモリ＆計算量が増大する 大規模なマッピングには適用不能
11. 11 今日の話の流れ Foundation ：確率ロボティクス Classic ： EKF SLAM hdl_graph_slam ：ポーズグラフ最適化 GLIM ：大域マッチングコスト最小化 MegaParticles ：ノンパラメトリック分布推定 VGGT： Feed Forward Geometry Transformer
12. 12 ファクタグラフ最適化 最適化対象となる変数 変数 : 𝑥1 , 𝑥2 , 𝑥3 ファクタ（誤差関数） : 𝑓0 𝑥1 , 𝑓1 𝑥1 , 𝑥2 ; 𝑜1 , 𝑓2 𝑥2 , 𝑥3 ; 𝑜2 グラフ内の全てのファクタを足し合わせたもの 𝑔 𝑥 = 𝑓0 𝑥1 + 𝑓1 𝑥1 , 𝑥2 ; 𝑜1 + 𝑓2 𝑥2 , 𝑥3 ; 𝑜2 目的関数 : 𝑔 𝑥 𝑥1 𝑥2 𝑓0 (𝑥1 ) 𝑓1 (𝑥1 , 𝑥2 ; 𝑜1 ) 𝑥3 𝑓2 (𝑥2 , 𝑥3 ; 𝑜2 ) 一個以上の変数を取りその変数間の観測に対する整合性誤差を定義する 局所性・疎性を持つ最適化問題をモデリングする強力な枠組み 変数間の関係性(制約)をグラフとして表現
13. 13 ファクタグラフ最適化 線形化（正規分布近似） 多変数間の疎性を考慮した正規分布（疎な情報行列）で近似する • 非線形ファクタグラフを線形化すると、 疎な情報行列で表現される正規分布 と等価になる • 疎行列分解を使うと要素数に比例する程度の計算量で、効率的に最適化を行うことができる 線形ファクタグラフ ＝疎な情報行列 ＝正規分布 非線形ファクタグラフ
14. 14 hdl_graph_slam ：ポーズグラフ最適化 点群間の相対姿勢を順次正規分布近似していき、それを全体で掛け合わせる 点群整合性誤差分布 𝑒𝑅𝐸𝐺(𝒫𝑖 , 𝒫𝑗 , 𝑇𝑖𝑗 ) 相対姿勢誤差分布 𝑒𝑅𝐸𝐿 ෨ 𝑇𝑖𝑗 , 𝑇𝑖𝑗 ෨ 𝑇𝑖𝑗 = argmin 𝑇𝑖𝑗 𝑓 𝒫𝑖 , 𝒫𝑗 , 𝑇𝑖𝑗 局所最適化 𝒫𝑖 𝒫𝑗
15. 15 hdl_graph_slam ：ポーズグラフ最適化 点群間の相対姿勢を順次正規分布近似していき、それを全体で掛け合わせる 𝑟 マッチング 点群整合性誤差分布 相対姿勢誤差分布 𝑒𝑅𝐸𝐺(𝒫𝑖 , 𝒫𝑗 , 𝑇𝑖𝑗 ) ෨ 𝑇𝑖𝑗 = argmin 𝑇𝑖𝑗 𝑓 𝒫𝑖 , 𝒫𝑗 , 𝑇𝑖𝑗 𝑒𝑅𝐸𝐿 ෨ 𝑇𝑖𝑗 , 𝑇𝑖𝑗 局所最適化 𝒫𝑖 𝒫𝑗 𝑇1 𝑇2 𝑇3 … 𝑇12 𝑇1 𝑇2 𝑇3 𝑇12 … Λ = 𝜂 = 相対姿勢正規分布を並べた正規分布（情報行列） 𝜇 = Λ−1𝜂 状態更新量
16. 16 hdl_graph_slam ：ポーズグラフ最適化 点群間の相対姿勢を順次正規分布近似していき、それを全体で掛け合わせる 点群整合性誤差分布 相対姿勢誤差分布 𝑒𝑅𝐸𝐺(𝒫𝑖 , 𝒫𝑗 , 𝑇𝑖𝑗 ) ෨ 𝑇𝑖𝑗 = argmin 𝑇𝑖𝑗 𝑓 𝒫𝑖 , 𝒫𝑗 , 𝑇𝑖𝑗 𝑒𝑅𝐸𝐿 ෨ 𝑇𝑖𝑗 , 𝑇𝑖𝑗 局所最適化 𝒫𝑖 𝒫𝑗 𝑇1 𝑇2 𝑇3 … 𝑇12 𝑇1 𝑇2 𝑇3 𝑇12 … Λ = 𝜂 = 相対姿勢正規分布を並べた正規分布（情報行列） 𝜇 = Λ−1𝜂 状態更新量
17. 17 hdl_graph_slam ：ポーズグラフ最適化 Koide+, A Portable 3D LIDAR-based System for Long-term and Wide-area People Behavior Measurement, International Journal of Advanced Robotic Systems, 2019
18. 18 問題点：ファクタグラフ最適化のポテンシャルを引き出せていない 共分散行列のモデリング • 相対姿勢(6DoF) の平均と共分散 が必要 • スキャンマッチング結果の平均と分散 …? 実用上， 共分散推定は難しい 多くの実装は正確に共分散を設定していない [CELLO-3D, ICRA2019] ループ制約を作ることができない 無理に作ると 推定精度に悪影響を及ぼす 使用せず捨ててしまう点群情報が多い • 戻ってきたときに点群に十分な重なりがない • スキャンマッチングが不安定になる
19. 19 GLIM ：大域マッチングコスト最小化（マルチスキャンレジストレーション） ポーズグラフ最適化 (従来手法) 大域マッチングコスト最小化 (提案手法) • 部分部分で最適化した結果を正規分布近似 • 正規分布の集合として全体最適化 • 相対姿勢の正規分布近似を避ける • グラフ全体で 直接点群マッチング誤差を最小化 スキャンマッチング (相対姿勢を確定) 相対姿勢制約 (SE3) ポーズグラフ ファクタグラフ (全体最適化) マッチングコスト評価 大域マッチングコスト最小化 =マルチスキャンレジストレーション • 高精度（正規分布に落とさない） • 大きな重なりを必要としない （全体として拘束があればよい） 相対姿勢制約（ BetweenFactor ）を捨てて、グラフ上で直接点群マッチング誤差を最小化する Koide+, GLIM: 3D Range-Inertial Localization and Mapping with GPU-Accelerated Scan Matching Factors, Robotics and Autonomous Systems, 2024
20. 20 GLIM ：大域マッチングコスト最小化（マルチスキャンレジストレーション） ポーズグラフ最適化との違い • 全ての点群のマッチングを同時に行う → 情報の不足（縮退）した点群間の情報も利用できる 縮退した誤差分布 なだらかな誤差分布 十分な勾配を持った誤差 分布 わずかな重なりしかない 点群間の情報も利用可能 全点群整合性誤差 正規分布上で最適化 全点群整合性誤差 • 任意誤差分布の正規分布近似を何度もやりなおす → 誤ったマッチングが起きても修正可能 • 姿勢を決定できない曖昧な状況があっても、後から修正できればいい Koide+, GLIM: 3D Range-Inertial Localization and Mapping with GPU-Accelerated Scan Matching Factors, Robotics and Autonomous Systems, 2024
21. 21 大域マッチングコスト最小化の利点 [SuMa, RSS2018] Proposed • 従来のポーズグラフでは正確な相対姿勢を決定できる 重なりの大きな点群間 にしか制約を作れない • 大域マッチングコスト最小化ではグラフ全体として拘束がかかっていればよいので、 わずかな重なりしかない点群間 にも制約を作ることができる ポーズグラフ (従来手法) マッチングコスト最小化グラフ (提案手法) Koide+, GLIM: 3D Range-Inertial Localization and Mapping with GPU-Accelerated Scan Matching Factors, Robotics and Autonomous Systems, 2024
22. 22 重なりの小さな点群間のループ (LIO -SAM) LIO -SAM ポーズグラフベースでは筋違いの道でループを閉じるのが難しい 上下方向誤差 ループを作れない Koide+, GLIM: 3D Range-Inertial Localization and Mapping with GPU-Accelerated Scan Matching Factors, Robotics and Autonomous Systems, 2024
23. 23 重なりの小さな点群間のループ (提案手法) 僅かな重なりさえあれば，ループを閉じてドリフトを補正可能 (重なり率 5% 以上でマッチングコストファクタを生成 ) Proposed Koide+, GLIM: 3D Range-Inertial Localization and Mapping with GPU-Accelerated Scan Matching Factors, Robotics and Autonomous Systems, 2024
24. 24 変則的なセンサ配置でのループクローズ 幾何特徴の少ない環境 重なりの小さいフレーム 変則的なセンサ配置でも問題なくループを作ることが可能 回転型(velodyne )以外の任意の種類の距離センサにも対応可能 Koide+, GLIM: 3D Range-Inertial Localization and Mapping with GPU-Accelerated Scan Matching Factors, Robotics and Autonomous Systems, 2024
25. 25 弱点：単一仮説に絞れないときの弱さ 手動ループ制約の追加 • 何度もマッチングをやり直すことで誤りを訂正する機会があるといっても、 自力では自然に動ける範囲しか修正できない • 推定値と大域解の間に大きなギャップがある場合は、乗り越えるのに外部の力が必要 別のモードに移動するには大きな力が要る ScanContext の併用
26. 26 今日の話の流れ Foundation ：確率ロボティクス Classic ： EKF SLAM hdl_graph_slam ：ポーズグラフ最適化 GLIM ：大域マッチングコスト最小化 MegaParticles ：ノンパラメトリック分布推定 VGGT： Feed Forward Geometry Transformer
27. 27 パーティクルフィルタ 任意分布関数を有限個のサンプル集合で近似する（ノンパラメトリック推定） 適応的パーティクルフィルタは今でも 2D地図自己位置推定では標準的に使用される (AMCL) https://youtu.be/QnhPpfX2j98 大域姿勢推定 サンプル集合による分布表現 柔軟な分布表現(非線形・多峰性 )による曖昧性への頑強さ 大量のパーティクルによる大域姿勢推定 高処理コスト 多次元へスケール困難 (次元の呪い) 3次元地図上では自由度を限定した 3~4 自由度(XYZ+Yaw )推定が一般的かつ大域姿勢推定は困難 [Saarinen +, 2013][Prez -Grau +, 2017]
28. 28 MegaParticles ：多重仮説推定 変分推論（微分情報）でサンプリング効率向上 ＋ GPU 並列処理で100 万個のパーティクルを並列処理 従来は困難であった、初期値なしでの大域姿勢推定＆誘拐状態からの復帰を実現 Koide+, MegaParticles: Range-based 6-DoF Monte Carlo Localization with GPU-Accelerated Stein Particle Filter, ICRA2024
29. 29 屋内実験：超強力な曖昧性表現能力 3地点仮説の重ね合わせ 1地点の2姿勢仮説の重ね合わせ 向き曖昧性の解決 事後確率分布 (カーネル密度推定 ) パーティクル集合 (事後確率で色付け) 超強力な分布表現さえあれば、従来にない曖昧性表現＆大域姿勢推定能力 が実現できる Z+ Z- Koide+, MegaParticles: Range-based 6-DoF Monte Carlo Localization with GPU-Accelerated Stein Particle Filter, ICRA2024
30. 30 多重仮説推定SLAM エレベーター移動のような極限の曖昧状況からも無事に復帰 Nakao+, Range-based 6-DoF Monte Carlo SLAM with Gradient-guided Particle Filter on GPU, ICRA2024
31. 31 クラシック最適化に基づく SLAM の弱点 曖昧性の評価が難しい • せっかく表現できた曖昧性を評価する方法・指標がない • 結局、位置誤差などの評価に終始してしまう • 状態を把握して行動するロボットの実現に発展できていない モデル化の難しい状況への対処が困難 • 点群の局所形状マッチングに立脚しているので、重なりの無い 点群間の関係はモデル化のしようがない • 何をどのように最適化しようが全く対処のしようがない 3地点仮説の重ね合わせ 全く重なりの無い点群
32. 32 今日の話の流れ Foundation ：確率ロボティクス Classic ： EKF SLAM hdl_graph_slam ：ポーズグラフ最適化 GLIM ：大域マッチングコスト最小化 MegaParticles ：ノンパラメトリック分布推定 VGGT： Feed Forward Geometry Transformer
33. 33 DUSt3R ：ニューラルネットワークによる常識情報の活用 The impossible match DUSt3R: Geometric 3D Vision Made Easy • Vision transformer を使ってそのまま三次元再構築を解かせる（フレーム毎のポイントマップ推定） • 姿勢やカメラパラメータは推定されたポイントマップから逆算できる 世界の構造や見え方などの「常識情報」まで使ったマッチングが可能 https://x.com/JeromeRevaud/status/1763495315389165963 Wang+, Dust3r: Geometric 3d vision made easy, CVPR2024
34. 34 VGGT : Feed Forward Geometry Transformer 従来のSfMパイプラインをまるごと置き換える機能を獲得 VGGT: Visual Geometry Grounded Transformer • グローバル（フレーム間）・ローカル（フレーム内）アテンションを交互に適用 • コード行数が全体 8k 行（ ORB_SLAM 36k 行、 VINS-Mono 28k 行） • 帰納バイアスを避けて、コーナーケースまで含めて画像入力から幾何情報出力まで E2Eで学習 疑問： VGGTなどの Feed Forward Geometry Transformer は曖昧性を考慮できているのか？ Wang+, VGGT: Visual geometry grounded transformer, CVPR2026
35. 35 Feed Forward Geometry Transformer は曖昧性を考慮しているのか？ 曖昧性の解決が難しいシーケンスを与えてみる（モデルは VGGT-Omega ） どういう推定結果（センサ軌跡）になるか考えてみてください！ Wang+, VGGT-Omega, CVPR2026
36. 36 全部入り 最初抜き（比較用） 最後抜き（比較用） 推定が大きく 乱れている 全部入り＋ディスラプター さらに大きく乱れる Visual SLAM や SfM のバンドル調整に誤った対応付けが混じったときの結果にとても良く似ている
37. 37 パッチ間アテンションの可視化 Layer5/23 ：ローレベル類似度？ Layer10/23 ：ハイレベル類似度？（局所形状） Layer13/23 ：ハード対応付け？（背景情報混み） Layer22/23 ：姿勢推定？ クエリ画像＆パッチ 後続研究で報告されているのに 近いレイヤーごとの機能性を 確認できる Sun+, AVGGT: Rethinking Global Attention for Accelerating VGGT, CVPR2026
38. 38 パッチ間アテンションの可視化 クエリ画像＆パッチ Layer13/23 ：ハード対応付け？（背景情報混み） 強力な対応付けを獲得しているが、状態推定としては旧来とそれほど変わらないようにも見える 始点と終点間で 誤った対応を 作っている？
39. 39 どうしてこうなるのか 良く考えてみれば当たり前 • 曖昧さを解決不能なデータはラベル付けもできないので、データセットにほとんど含まれない • モデルの学習ロスに対応付けの曖昧性を考慮できるような項はない • モデルは強制的に単一の姿勢を出力するように学習される 一方で、従来の最適化ベース手法から大きく変わらないクエスチョン • Attention を媒介する要素はパッチ・フレームで良いのか？地図上の地点では？ • 生物が自然と考慮・対処できるレベルの曖昧さを解決することはできるのか？ • 超長期の記憶管理はどうするのか？ クラシックな SfM パイプラインで人手で作っていた機能を E2E で学習しているのは驚異的 • 物体形状などを考慮した頑健な特徴量抽出 • コンテキストを考慮した強力な対応付け • （ロバスト？）最小二乗法的な挙動を見せる姿勢最適化
40. 40 今日の話の流れ Foundation ：確率ロボティクス Classic ： EKF SLAM hdl_graph_slam ：ポーズグラフ最適化 GLIM ：大域マッチングコスト最小化 MegaParticles ：ノンパラメトリック分布推定 VGGT： Feed Forward Geometry Transformer 単純な正規分布近似 大規模正規分布近似 繰り返し正規分布近似 ノンパラメトリック E2E アテンション学習
41. 41 SLAM の研究は終わったのか？ 私の答え： 「 SLAM 単体で定義できる問題は解決されつつある」 • 入力観測から周囲地図と姿勢を出力する、という枠組みで測れる性能は飽和しつつ ある • これ は学習ベース、最適化ベース、どちらに関しても同様 良く受ける質問： 「 SLAM の研究でまだやることありますか？」 「 …が、 SLAM 単体で定義しきれない問題がたくさん残っている」 • 真に知的なエージェントを実現するのに必要な曖昧性表現や長期記憶はまったく解決できていない • これらは SLAMの枠組みだけでは目的関数が設計できない • SLAMと行動の統合こそをもっとやらないといけない 「これまで以上に面白い研究がまだまだ残っているよ！！」