2024/03/05 1 GPUを利用したStein Particle Filterによる点群6自由度姿勢推定 小出健司，大石修士，横塚将志，阪野貴彦 産業技術総合研究所 (AIST) 第29回ロボティクスシンポジア＠沖縄 

2 動画 

3 提案手法：MegaParticles GPUを活用した超大量(10242=1M個)パーティクルの並列処理 柔軟な状態分布表現を背景として強力な曖昧性表現能力＆大域姿勢推定能力を実現 • Stein Variational Gradient Descent (SVGD) によるサンプリング効率向上 • Locality Sensitive Hashing (LSH) による効率的な近傍パーティクル探索 • 近傍グラフ上でのパーティクル事後確率推定 サンプリング効率・安定性を高めつつ， GPUの性能(並列性)を最大限に引き出す工夫 

4 関連研究 

5 関連研究：反復スキャンマッチング 3次元地図上での6自由度姿勢推定のデファクトスタンダード スキャン点群・地図点群間でスキャンマッチング (e.g, ICP/NDT) を反復的に行う 初期姿勢が必須 データの周期・連続性に強く依存 (誘拐対処は不可能) 縮退環境(e.g., トンネル)など曖昧性の強い状況で破綻 6自由度でも軽量・高速 IMU統合やウィンドウ最適化で頑強に [Wu+, 2022][Koide+, 2024] 初期姿勢 スキャン 地図点群 スキャン スキャン マッチング 前回姿勢 ICP 地図点群 スキャン点群 

6 関連研究：モンテカルロ自己位置推定 (MCL) 状態分布を有限個のサンプル集合で表現 (ノンパラメトリック推定) 適応的パーティクルフィルタは今でも2D地図自己位置推定では標準的に使用される (AMCL) https://youtu.be/QnhPpfX2j9 8 大域姿勢推定 柔軟な分布表現(非線形・多峰性)による曖昧性への頑強さ 大量のパーティクルによる大域姿勢推定 高処理コスト 多次元へスケール困難 (次元の呪い) サンプル集合による分布表現 3次元地図上では自由度を限定した3~4自由度(XYZ+Yaw)推定が一般的かつ大域姿勢推定は困難 [Saarinen+, 2013][Prez-Grau+, 2017] [Fox+, 2003] 

7 関連研究：6自由度モンテカルロ姿勢推定 (6DoF-MCL) 6自由度空間にモンテカルロ姿勢推定を適用する試みが出てきている 基本的な方針は賢いサンプリングによって，少数のパーティクルで効率よく状態表現を行う • 車速情報の利用 [Akai+, 2020] • 並進・回転成分分解 (PoseRBPF) [Deng+, 2021] • Stein Particle Filter [Maken+, 2022] パーティクルフィルタ + 変分推論 Stein Variational Gradient Descent (SVGD) [Liu+, 2016] 尤度関数の微分情報とパーティクルの隣接関係情報を使った効率的な状態サンプリング 少数のパーティクル (100 ~ 1000個) による効率的な6自由度状態推定 少数のパーティクルでは根本的に6自由度空間での多峰性分布を正確に表現できない 曖昧性が強くなると性能劣化＆大域姿勢推定は困難 → モンテカルロ推定の意味があまりないのでは 

8 提案手法 

9 提案手法 コンセプト： GPUで超大量のパーティクルを処理，強力な分布表現力を活用した自己位置推定手法 GPUを使えば終わり？ → 実際はそう簡単でない 6DoF空間には1Mパーティクルでも足りない SVGDによるサンプリング効率改善 SVGDの全体処理コストは 𝑂(𝑁2) 大量のパーティクルの代表値をどう決めるのか LSHによる確率的・反復近傍探索 𝑂(𝑁) 近傍グラフ上でのベイズ推定 

10 提案手法の流れ 移動量予測&状態更新 (GICP) 尤度関数へ適応 (GN-SVGD + LSH) 予測ステップ 修正ステップ 事後確率推定 (近傍グラフ上ベイズ推定) 代表姿勢抽出 • 状態分布をパーティクルの集合で表現 • 各パーティクルは現時刻における姿勢仮説を表す 現在姿勢 パーティクル パーティクル集合 • 予測ステップと修正ステップを繰り返してパーティクルを更新 • パーティクル集合から代表姿勢を抽出する 

11 提案手法の流れ 移動量予測&状態更新 (GICP) 尤度関数へ適応 (GN-SVGD + LSH) 予測ステップ 修正ステップ 事後確率推定 (近傍グラフ上ベイズ推定) 代表姿勢抽出 現在姿勢 パーティクル パーティクル集合 • 状態分布をパーティクルの集合で表現 • 各パーティクルは現時刻における姿勢仮説を表す • 予測ステップと修正ステップを繰り返してパーティクルを更新 • パーティクル集合から代表姿勢を抽出する 

12 予測ステップ 通常のPFと同様に移動量予測を基に各パーティクルを独立に更新する 直前スキャン点群・現在スキャン点群間でGICPスキャンマッチングを適用，相対移動量予測を得る 移動量予測 直前/現在点群 移動量予測を各パーティクルに適用 (ノイズの共分散はGICPのヘッセ行列から求める) 更新後姿勢 更新前 予測 予測誤差ノイズ GICP共分散 

13 提案手法の流れ 移動量予測&状態更新 (GICP) 尤度関数へ適応 (GN-SVGD + LSH) 予測ステップ 修正ステップ 事後確率推定 (近傍グラフ上ベイズ推定) 代表姿勢抽出 現在姿勢 パーティクル パーティクル集合 • 状態分布をパーティクルの集合で表現 • 各パーティクルは現時刻における姿勢仮説を表す • 予測ステップと修正ステップを繰り返してパーティクルを更新 • パーティクル集合から代表姿勢を抽出する 

14 修正ステップ：尤度関数 点群観測をもとに尤度関数にフィットするようにパーティクル集合を更新する ICP系統の中で最も精度が良い 最小二乗形式なので効率的な Gauss-Newton 最適化が使える 各スキャン点に対する最近傍地図点は事前計算しておいたグリッドマップからルックアップする 更新に用いる対数尤度関数には地図・スキャン点群間の分布対分布(GICP)距離を用いる [Segal+, 2009] 地図・スキャン点距離 地図・スキャン点の混合精度行列 対数尤度 = 最近傍地図点 スキャン点 

15 修正ステップ：パーティクル更新則 リサンプリング (通常パーティクルフィルタ) [Kitagawa, 1993][Gordon, 1993] SVGD (64 particles) Resampling (1024 particles) 尤度を重みとして新しいパーティクル集合をサンプリングする Stein Variational Gradient Descent (SVGD) [Liu+, 2016] 尤度関数の勾配情報とパーティクル近接関係情報を使って 真の分布とパーティクル集合分布のKL距離を最小化する サンプリング効率が悪い (複雑な分布には大量のパーティクルが必要) 低尤度領域のパーティクルが死滅する (サンプル衰退問題 [Arulampalam+, 2002] ) 少数のパーティクルで効率的なサンプリングが可能 低尤度領域のパーティクルも全て生存 (サンプル多様性が保たれる) 

16 Stein Variational Gradient Descent (SVGD) 次状態 現状態 更新量 更新量 カーネル＝近接度に応じた重み 対数尤度の勾配 ＝尤度の極へ向かう引力 カーネルの勾配 ＝パーティクル間に働く斥力 Repulsive force + Optimization result 全サンプル Attractive force パーティクル全体として互いの距離を保ちながら，尤度の極へ向かっていく 

17 SVGDの問題点 更新量 尤度の極への引力 パーティクル間斥力 カーネル 最急降下法なので収束が遅い (1次収束) 全サンプル L-BFGSによる定式化 [Maken+, 2022] も最初の数イテレーションは最急降下法と同一で遅い 計算量がパーティクル 𝑂(𝑁)，全体 𝑂 𝑁2 パーティクル数を大きくとることができない… (1000パーティクル程度が上限) 

18 近似 Gauss-Newton SVGD (提案手法) 近似 Gauss-Newton SVGD (提案手法) 近傍M=20サンプルに限定 (遠方はどのみち k≒0) → 計算量を𝑂 1 に → 少数のイテレーションで高速に収束 尤度関数が最小二乗形式なのを利用した Gauss-Newton法による二次最適状態更新量 全サンプル 最急降下方向 (尤度への引力) カーネル勾配 (パーティクル間斥力) Original SVGD [Liu+, 2016] カーネル 

19 近似 Gauss-Newton SVGD on Manifold (提案手法) 6自由度姿勢空間(SE3)に拡張 (リー代数のexpmap/logmapを適用) 指数カーネル on SE3 SE3 retraction パーティクルあたりの計算量は 𝑂(1)，全体計算量は 𝑂 𝑁 大量のパーティクルにスケール可能＆効率的に二次収束 https://gtsam.org 

20 近傍パーティクル探索 どうやって近傍パーティクルを見つける？ 一般的な直線探索 (e.g., LinearSearch)や空間分割 (e.g., KdTree)は適用不可能 Locality Sensitive Hashing (LSH) による定数時間O(1)で確率的に近傍候補検出 複数のフレームをまたいで反復的に各パーティクルの近傍リストを更新 確率的&反復的な近傍探索 (提案手法) 近傍探索にとって非常に条件が悪い設定 非ユークリッドな姿勢空間 パーティクル位置が動的に変化 大量のパーティクル (1024^2) 

21 Locality Sensitive Hashing (局所性鋭敏型ハッシュ ) Locality Sensitive Hashing (LSH) 安定分布に基づくLSH [Datar+, 2004] を6自由度姿勢空間(SE3)に拡張 パーティクル姿勢 基準グリッド系 テーブル毎にランダム グリッドスケール 安定分布ノイズ (e.g., ガウシアン) サンプル毎にランダム 確率的ハッシュ関数の一種で距離的に近傍のデータに対して高い確率で同じ整数値を割り振る LSHによってハッシュテーブル上の同じビンに入ったデータを近傍として検出 → Hash value グリッド上の整数座標 

22 反復的近傍パーティクル更新 確率的ハッシュ関数なので，False Positive & False Negative が含まれる パーティクル間の近傍関係は短時間で大きく変化しないと仮定 時間フレームをまたいで各パーティクルの近傍リスト(最大20個)を反復的に更新 全パーティクル並列に処理できる → GPUで高速計算可能 パーティクルあたり 全体 ：𝑂 1 ：𝑂(𝑁) 計算コスト 

23 近似 GN-SVGD + LSH近傍探索 SVGD+LSHで大量のパーティクルを全体計算量𝑂(𝑁)で尤度関数に適応させることができたが… 尤度で色付け 結果を見ても訳が分からない パーティクルがうごめいているだけでは 意味がない どうやって？ 通常のPFで使われる重み付き平均や 最密地点推定は明らかにうまくいかない 実用上，代表姿勢を取り出す必要がある 

24 提案手法の流れ 移動量予測&状態更新 (GICP) 尤度関数へ適応 (GN-SVGD + LSH) 予測ステップ 修正ステップ 事後確率推定 (近傍グラフ上ベイズ推定) 代表姿勢抽出 現在姿勢 パーティクル パーティクル集合 • 状態分布をパーティクルの集合で表現 • 各パーティクルは現時刻における姿勢仮説を表す • 予測ステップと修正ステップを繰り返してパーティクルを更新 • パーティクル集合から代表姿勢を抽出する 

25 パーティクル事後確率推定 各パーティクルの事後確率を明示的に求める カーネル 尤度 事前確率 事後確率 パーティクル毎のベイズ推定＋ランダムウォーク仮定での平滑化 (カーネル密度推定) 近似SVGDに使った近傍パーティクルリストを再利用して効率化 初期事後確率 尤度 事前確率 近傍グラフ上での 事後確率平滑化 Kernel 平滑事後確率 近傍パーティクル 事後確率 カーネル 最大事後確率を持つパーティクルを推定代表値として採用 全体計算量 𝑂(𝑁2) 

26 提案手法の流れ 移動量予測&状態更新 (GICP) 尤度関数へ適応 (GN-SVGD + LSH) 予測ステップ 修正ステップ 事後確率推定 (近傍グラフ上ベイズ推定) 代表姿勢抽出 SVGDによる効率的な状態サンプリング 近傍グラフ上でのパーティクル事後確率推定 動的かつ疎なヒストグラムフィルタ 

27 もうひとつの解釈：動的かつ疎なヒストグラムフィルタ ヒストグラムフィルタ 全状態を列挙したヒストグラム上でベイズ推定 提案手法 尤度関数にフィットするようにパーティクル位置が適応的に変化 効率的な状態サンプリング(SVGD+LSH)＋ヒストグラム上での網羅的な事後確率推定 無限のメモリと計算資源があれば理論上最強 現実は3次元以上はほぼ適用不可能 [Peng+, 2020] https://youtu.be/Nz7O-ezXMrg パーティクルを状態ビンとし，接続関係が隣接グラフで表される ヒストグラム上で事後確率伝播 

28 実験 

29 屋内実験 Intel Xeon 8360Y / NVIDIA A100 (40GB) 実験設定 使用計算機 繰り返しの多い屋内環境 MS Azure Kinect (点群のみ使用) 比較対象 (LiDAR-IMU自己位置推定) FAST_LIO_LOCALIZATION (タイトカップリング) hdl_localization (ルーズカップリング) https://github.com/HViktorTsoi/FAST_LIO_LOCALIZATION https://github.com/koide3/hdl_localization 比較対象手法のみIMU & 初期姿勢有り ※点群の前処理以外はほぼ全てGPU実装 基本性能を見るためのデータ (Easy01, Easy02) と誘拐への対処を見るデータ (Kidnap01, Kidnap02) を記録 

屋内実験：Easy01 & Easy02シーケンス 3地点仮説の重ね合わせ 1地点の2姿勢仮説の重ね合わせ 向き曖昧性の解決 事後確率分布 (カーネル密度推定) パーティクル集合 (事後確率で色付け) 従来にない強力な曖昧性表現＆大域姿勢推定能力を実現 Z+ Z- 

屋内実験：Kidnap01 & Kidnap02シーケンス 全6回の誘拐全てから数秒以内に正常復帰 

屋内実験・定量評価 +スムージング → SOTAを大きく上回る精度 提案手法 → SOTAに肉薄する精度 タイト カップリング 既存手法は全て誘拐で破綻 提案手法は常に正常復帰 ルーズ カップリング ルーズカップリング手法が破綻する環境(Easy02)でも 提案手法は安定動作 誘拐後，数秒以内に正常復帰 リアルタイム (100ms) を上回る処理速度 

33 屋外実験 

34 屋外実験 地図構築から一か月後にデータ記録 (動的物体・植生の変化有) 屋内実験と同様 1Mパーティクルを 100ms 以内でリアルタイム処理 全8回の誘拐全てから正常復帰 (状態空間：280m x 200m x 30m x Full SO3) 

35 今後の課題 

36 計算資源 かなり楽観的な見方をしている 1M(=106)パーティクルは初期値＆姿勢仮定一切無しでの完全な6自由度推定のために必要 • IMU観測を入れれば実質的には4自由度 → 104~105パーティクル (1/10 ~ 1/100) でおそらく十分 • 適切な初期姿勢分布や軽微な地面高仮定で必要なサンプル数を大きく削減できる Jetson Orin (A100性能比 1/3) や Jetson Orin Nano (1/10) など組み込み処理も十分見込める 計算量 メモリ消費 地図上の最近傍マップ → 屋外でも1.6GBで対した消費量ではない より大規模地図にも階層表現などを取り入れればスケール可能 

37 大域姿勢推定の限界 大部分の場合，誘拐状態からの復帰は数秒で可能 一方，屋外実験で復帰に長時間 (15s, 32s) かかった箇所がある 地図外の領域を含む 地図外＋植生の変化 正解地点で尤度が最大化されないと大域姿勢推定が困難・不可能になる 現状の枠組みでは根本的な対処は困難 ウィンドウ最適化[Koide+, 2024] やマッチング結果の信頼性評価[Akai, 2022] を取り入れることで改善可能？ 復帰時間 : 15s 復帰時間 : 32s 

38 まとめ 

39 まとめ データセット：https://zenodo.org/records/10122133 GPUを活用した超大量(10242=1M個)パーティクルの並列処理 • Stein Variational Gradient Descent (SVGD) によるサンプリング効率向上 • Locality Sensitive Hashing (LSH) による効率的な近傍パーティクル探索 • 近傍グラフ上でのパーティクル事後確率推定 • 動的かつ疎なヒストグラムフィルタとしての解釈 

40 姿勢スムージング 代表姿勢は時間的連続性を考慮せずに選択されるのでジッターが生じる ジッターを防ぐため，代表姿勢系列に対してスムージングを適用する 代表姿勢へのフィット 時間的連続性