UcoSLAM

Muñoz-Salinas 2019 · 論文

一行要約 — 自然特徴点と正方形平面フィデューシャルマーカー（ArUco）を1つのSLAMフレームワークに融合し、マーカーの信頼性・メトリックスケールと、自然特徴点によるカバレッジを両立させる。

問題

多くのSLAMシステムは特徴点のような自然ランドマークを使用するが、これらは「時間的に不安定であったり、多くの場合繰り返しパターンを持つか、ロバストな追跡には不十分（例えば屋内の建物など）」であり、BoW（Bag-of-Words）による再ローカライゼーションは視点変化や繰り返しパターンの下では性能が限られる。マーカーのみによるSLAM（著者らの以前の研究であるSPM-SLAM）は明確なデータ対応付けと正しいメトリックスケールを与えるが、画像ごとに少なくとも2つのマーカーが見えることを要求するため、大規模環境にスケールしない。UcoSLAMは両方のランドマークタイプを1つのマップに統合する。1つのマーカーが見えた時点でスケールが確定し、マーカーは戦略的な位置にのみ配置すればよく、それ以外の全ての場所では特徴点がカバレッジを提供する。

手法とアーキテクチャ

マップは$\mathcal{W}=\{\mathcal{K},\mathcal{P},\mathcal{M},\mathcal{G},\mathcal{D}\}$で構成される。キーフレーム$\mathcal{K}$、三角測量されたマップ点$\mathcal{P}$、マーカー$\mathcal{M}$（それぞれ辺の長さ$s$、ポーズ$\mathrm{M}\in\mathbb{SE}(3)$、既知の4隅を持つ）、共視性グラフ$\mathcal{G}$（共有される点はエッジに重み1を加え、共有されるマーカーは重み4を加える）、そしてFBoWキーフレームデータベース$\mathcal{D}$。パイプラインは古典的なORB-SLAM式のループ——初期化、追跡、キーフレーム挿入、局所/全体最適化、ループ閉じ込み、再ローカライゼーション——であり、並行動作するMap Managerスレッドと、デプロイのためのマップ保存/読み込み機能を備える。

• 初期化は特徴点ベース（ORB-SLAM2同様のホモグラフィ対エッセンシャル行列）とマーカーベース（SPM-SLAM）の初期化を並行して実行する。マーカーが同点の場合に優先され、即座にメトリックスケールが得られる。マーカーのポーズ曖昧性（遠方の平面マーカーに対する2つのほぼ同一の再投影）は複数の視点を用いて解決される。
• 追跡は点とマーカーの隅の再投影誤差を同時に最小化する。再投影誤差を$e(\mathrm{T},\mathbf{x},\delta,\mathbf{u})=\Psi(\mathrm{T},\mathbf{x},\delta)-\mathbf{u}$（投影関数$\Psi$、カメラ内部パラメータ$\delta$）とすると、フレームポーズは

$$\mathbf{f}_{\mathrm{T}}=\arg\min_{\mathrm{T}}\left(\mathbf{w_p^f}\,H(\Upsilon_p^{\mathbf{f}},\mathrm{T})+\mathbf{w_m^f}\,H(\Upsilon_m^{\mathbf{f}},\mathrm{T})\right),$$

ここで$H(\Upsilon_p^{\mathbf{f}},\mathrm{T})$は点の再投影誤差のHuberロバスト化された合計（$\Omega_{\mathbf{g}}=\eta^{-\mathbf{g}_l}\mathrm{I}$で重み付けされ、粗いピラミッドレベルからの特徴点の重みが下げられる）であり、$H(\Upsilon_m^{\mathbf{f}},\mathrm{T})$は各有効なマーカーの4隅の再投影誤差の平方和である。点の数はマーカーの数を大きく上回るため、バランスは以下で設定される:

$$\mathbf{w_m^f}=\frac{1}{2}\min\left(1,\frac{\mathbf{n_f}}{\tau_m}\right),\qquad \mathbf{w_p^f}=1-\mathbf{w_m^f},$$

$\mathbf{n_f}$はフレーム内の有効なマーカー数であり、$\tau_m=5$がデフォルト値である。

• マップ最適化はキーフレームポーズ、点位置、マーカーポーズにわたる疎なLevenberg-Marquardtバンドル調整である: $\arg\min_{\mathbf{k}_{\mathrm{T}},\mathbf{p}_{\mathbf{x}},\mathbf{m}_{\mathrm{M}}} E(\Upsilon_p)+E(\Upsilon_m)$。キーフレーム挿入後は局所的（共視キーフレーム）に、ループ閉じ込み後は全体的に実行される。
• ループ閉じ込みと再ローカライゼーションはマーカー優先で行われる。参照キーフレーム付近では見られなかったマーカーが再観測されると、即座かつ明確なループ閉じ込みが発生する——これは、マーカーが追跡に許可される前に$\mathrm{Sim}(3)$ポーズグラフ最適化を通じて修正される——一方、特徴点によるループ閉じ込みはFBoWを介して非同期に実行される。再ローカライゼーションはまず既知のマーカーを試し、それに失敗した場合はBoW+RANSAC PnPにフォールバックする。
• カリングは各マーカーについて、それを観測している最も遠い3つのキーフレームを保持し、その後、マッチした点が他の少なくとも3つのキーフレームで観測されているキーフレームを削除する。

実験結果

KITTI（20モノキュラーシーケンス）、EuRoC-MAV（20）、TUM RGB-D（10）、SPM（8）上でORB-SLAM2およびLDSOと比較。提案されたペアワイズスコア$\mathbf{S}_{\rho}(\mathbf{a},\mathbf{b})\in[-1,1]$を用いる。これはATE（共通して追跡されたフレームで計算）と追跡されたフレームの割合を組み合わせたものである。ORB-SLAM2は信頼度レベル$\rho$全体を通じてUcoSLAMに対し$-0.10$から$-0.14$のスコアを付ける（UcoSLAMがわずかに優位）。LDSOはUcoSLAMに対し$-0.37$から$-0.40$のスコアを付ける（明確に劣位）。マーカーを配置したSPMデータセットでは、特徴点+マーカーの組み合わせがマーカーのみ（それに対して$\mathbf{S}$は$-0.187$から$-0.625$）と特徴点のみ（例: video1では特徴点のみの場合、追跡率64.5%でATE 0.601 mに対し、追跡率100%でATE 0.057 m）の両方を上回る。平均速度（追跡フレームに対するfps）: UcoSLAMは2.6（SLAM）/19.8（追跡）であり、ORB-SLAM2は1.6/12.5、LDSOは3.0/2.4であり、Wilcoxon検定はORB-SLAM2に対する有意差を確認する。反復パターンの多いオフィスビル（天井マーカー50個、約20 mの通路4本、12,000フレームの携帯電話シーケンス）では、ORB-SLAM2、LDSO、そして単一ランドマークのUcoSLAMの両モードすべてが失敗した。組み合わせた特徴点+マーカーモードのみが一貫したマップを構築でき、BoW再ローカライゼーションはフレームあたり最大6つの誤った候補を返すが、マーカーIDがこれを解消する。

SLAMにおける意義

UcoSLAMは、人工的なランドマークと自然なランドマークが補完的であることを示す。マーカーはメトリックスケール、ドリフトのないアンカー、信頼できる再ローカライゼーション/ループ閉じ込みを提供し、特徴点はそれらの間の連続的なカバレッジを提供する。これにより、倉庫、実験室、産業用ARセットアップなど、環境を制御できる場面で非常に実用的になる——数個のマーカーを配置することは安価な保険であり、そのマップ保存/読み込み設計は、当時の研究システムとしては珍しく展開に適したものであった。

関連ノート

• ORB-SLAM2 — UcoSLAMが基盤とする特徴点ベースSLAM設計
• Scale ambiguity — マーカーが無償で解決するモノキュラーの問題
• Visual Place Recognition (VPR) — マーカーが代替する外観ベースのループ閉じ込み
• Landmark — マップランドマークの一般的な概念
• LDSO — 比較対象であったdirect法ベースライン