S-PTAM

Pire 2017 · 論文

一行要約 — S-PTAMはPTAMの並列トラッキング・アンド・マッピング(parallel tracking-and-mapping)パラダイムをステレオカメラに拡張し、ベースラインからのメトリックスケールと外観ベースのループ閉じ込みを備えた、完全でリアルタイムなROS公開済みステレオSLAMシステムを実現した。

問題

単眼SLAMは、ブートストラップ問題(遅延を伴う、繊細なマップ初期化)、スケールの不定性とスケールドリフト、動的物体や高速な動きに対する脆弱性に悩まされる。ステレオはこれらすべてを解決する。単一のステレオビューから深度を三角測量することで、点は低い不確かさで、遅延なく初期化され、かつ実スケールを回復できる。これは「周囲のワークスペースとインタラクトする必要があるロボットにとって不可欠」である。欠けていたのは、屋内、屋外、動的物体、変化する照明、大きなループに対して頑健で、成熟した、リアルタイムの特徴点ベースのステレオSLAMであり、しかもロボティクス研究室が実際にROSを通じて展開できるものだった。S-PTAMはそのギャップを埋める。

手法とアーキテクチャ

トラッキング、ローカルマッピング、ループクロージングの3スレッドがマップを共有する。グローバルフレームは最初のカメラ位置に設定され、初期マップは最初のステレオペアから三角測量される(ブートストラップなし)。

• 特徴点: GFTTコーナーをバイナリBRISK記述子で記述する — 論文中のKITTIにおける網羅的な検出器・記述子評価から選ばれたもの。2枚の画像の特徴抽出は2つの並列スレッドで実行される。
• トラッキング(フレームごと): 予測フラスタム内(車輪オドメトリまたは減衰速度モデルからの予測)にあるマップ点を投影し、グリッドセル内(空間ハッシング)でハミング距離により特徴とマッチングする。近傍キーフレームの共視性領域内の点のみが考慮され、フラスタムカリングと45°の視線角テストが適用される。その後、位置は、再投影誤差に対するロバストGauss-Newton法で求めた相対運動$\mu = (t_x, t_y, t_z, \theta_{roll}, \theta_{pitch}, \theta_{yaw})^T$を前の位置に合成することで洗練される。 $$\mu' = \operatorname*{argmin}_{\mu} \sum_{i \in S} \rho\left(J_i\, \mu - \Delta z_i(\mu_{\text{prev}})\right),$$ Huberコスト$\rho$を用い、Levenberg-Marquardt法(g2o)で最小化する。$J$は射影の導関数と、位置の導関数$\partial x_i^C / \partial \mu_j = G_j\, E^{CW}_{\text{prev}}\, x_i^W$を連鎖させたものである。
• キーフレームと即時の点生成: 追跡された点数が最後のキーフレームで追跡された点数の90%を下回ると、フレームはキーフレームになる。マッチしなかったステレオ特徴は三角測量され、トラッキングスレッド内で即座にマップへ挿入される — これはPTAMとは異なり、PTAMではこれをマッピングスレッドに委ねるため、キューが混雑しているとトラッキングを失うことがある。
• ローカルマッピング: ローカルバンドル調整は、キューに入った最大10個のキーフレームと、その近傍のすでに洗練済みのキーフレームおよびそれらから見える点を洗練する。同じロバストな再投影誤差を位置と点について同時に最小化する: $\left(\mu'_{j=2\ldots N}, x'^W_{i=1\ldots M}\right) = \operatorname{argmin} \sum_j \sum_{i \in S_j} \rho_{\sigma_T}(\psi_{ji})$。最初のキーフレームは固定される(それがワールドフレームを定義するため)。悪い点は除去され、グラフを強化するために新しい点-キーフレーム計測が能動的に探索される。
• ループクロージング: キーフレームはバイナリワードのbag(DBoW2、$\alpha = 0.3$)として記述される。候補は正規化類似度スコア$\eta(v_i, v_j) = s(v_i, v_j) / s(v_i, v_{i-1})$をクリアしなければならない。ここで$s(v_i, v_j) = 1 - \frac{1}{2}\left\lVert \frac{v_i}{|v_i|} - \frac{v_j}{|v_j|} \right\rVert$は$[0,1]$のL1類似度である。幾何検証は3D-2D対応に対するP3P RANSACで行われ(インライア率80%の場合のみループが受理される)、続いてフルPnPと非線形リファインメントで相対変換$T_{C^C C^\ell}$が得られる。補正は、その変換をSlerp補間された重み付け(ループ点付近で強くなる)によりループ沿いに伝播させ、その後ポーズグラフ最適化(g2o)とマップ点補正が続く — 「安全なマッピングウィンドウ」により、トラッキングとLBAは動作を継続でき、最終的なマップ更新のためにのみ短時間停止する。

実験結果

KITTI(00–10)および屋内Level 7 S-Blockデータセットで、i7 @ 2.8 GHzと16GB RAMを搭載したラップトップにて実験した。

• KITTIベンチマーク: 並進誤差は1.19%で、ORB-SLAM2(ステレオ)の1.15%、S-LSD-SLAMの1.20%と比較される。回転誤差は0.0025 deg/mで3者中最良(ORB-SLAM2は0.0027、S-LSD-SLAMは0.0033)。
• ループクロージング: 幾何検証はすべてのループシーケンス(00, 02, 05, 06, 07, 09)で誤検出を100%の精度で拒否した。約4kmのシーケンス00の軌道全体で、最大絶対位置推定誤差は15m未満に保たれた。
• 実行時間: トラッキングスレッドは、ループクロージングのオーバーヘッドを含めて約18Hzで動作する。
• Level 7 S-Block(車輪型ロボット、30分以上のオフィスループ): ORB-SLAM2に匹敵する精度で、同じ領域で似た誤差ピークを示した。

SLAMにおける意義

S-PTAMはPTAM時代と現代のステレオSLAMをつなぐ橋である。トラッキング/マッピングの分解がステレオと大規模な屋外軌道にきれいにスケールすることを示し、ロボティクス研究室がROSを通じて展開できる、ループクロージングを備えた最初期の完全なオープンソースステレオSLAMシステムの一つだった。ORB-SLAM2が支配的な特徴点ベースの参照実装になる前は、地上ロボットの共通ベースラインとして使われ、トラッキングスレッドでの即時点生成や安全ウィンドウによるループ補正といった詳細は今なお有用なエンジニアリング上の教訓として残っている。

関連ノート

• PTAM
• ORB-SLAM2
• StereoMSCKF
• Disparity vs Depth
• Stereo rectification
• Visual Place Recognition (VPR)