Basalt

Usenko 2020 · 論文

一行要約 — 視覚慣性推定をリアルタイムのオドメトリ層とマッピング層に分割し、非線形因子回復(NFR)を用いて、VIOの線形化されたマージナリゼーション事前分布を、大域バンドル調整が自由に再線形化できる小規模な非線形の相対姿勢・ロールピッチ因子の集合へ変換する手法。

問題

カメラとIMUは相補的だが、それらを組み合わせて大域的に整合したマッピングを行うのは単純ではない。バンドル調整は大きなベースラインと長い時間間隔を持つキーフレームを望むが、「一方で慣性データは区間の持続時間とともに急速に劣化し、数秒間の積分の後には典型的にはほとんど有用な情報を含まなくなる」。そのため、キーフレーム間でIMUデータを直接プレインテグレーションするシステムは、キーフレーム間隔に上限を設け、速度とバイアスを大域問題に持ち込まざるを得ないが、疎なキーフレームではこれらを十分に制約できない。一方、スライディングウィンドウ型VIOは履歴をシューア補元による事前分布に圧縮するが、これはその線形化点で凍結されてしまう。オドメトリの累積情報を、線形化点を凍結したり生の計測値をすべて持ち運んだりせずに、大域最適化へ引き継ぐにはどうすればよいか。

手法とアーキテクチャ

VIO層(固定ラグ・スムーザー)。 FASTコーナーは50画素グリッド内で抽出され(80〜120個のアクティブ特徴)、ピラミッド逆合成KLTによって、局所スケールSSDノルム(輝度スケール不変)を用いた $\mathrm{SE}(2)$ パッチワープを推定しながら追跡される。ソースフレームへの逆追跡により外れ値が除去される。ランドマークはホストキーフレームに対する相対量として、ステレオグラフィック射影座標 $(u,v)$ の単位方位ベクトルと逆距離 $d$ で保存され、次の再投影残差を与える:

$$\mathbf{r}_{it} = \mathbf{z}_{it} - \pi_{c(t)}\big(\mathbf{T}_{t}^{-1} \mathbf{T}_{h(i)}\, \mathbf{q}_{i}(u,v,d)\big)$$

これは $d = 0$ でも数値的に安定である。IMU計測値は再帰的に伝播される共分散とバイアスヤコビアンを伴って、擬似計測値 $(\Delta\mathbf{R}, \Delta\mathbf{v}, \Delta\mathbf{p})$ にプレインテグレーションされる。例えば回転残差は $\mathbf{r}_{\Delta\mathbf{R}} = \mathrm{Log}(\Delta\tilde{\mathbf{R}}\, \mathbf{R}_{j}^{\top} \mathbf{R}_{i})$ である。各フレームは以下を最小化する:

$$E = \sum_{i\in\mathcal{P},\; t\in\mathrm{obs}(i)} \mathbf{r}_{it}^{\top} \boldsymbol{\Sigma}^{-1}_{it} \mathbf{r}_{it} + \sum_{(i,j)\in\mathcal{C}} \mathbf{r}_{ij}^{\top} \boldsymbol{\Sigma}^{-1}_{ij} \mathbf{r}_{ij} + E_{\text{m}}$$

これは7個のキーフレーム姿勢と、直近の3個の完全状態(姿勢・速度・バイアス)からなるウィンドウ上で行われる。古い状態はシューア補元による部分的マージナリゼーション($\mathbf{H}^{\text{m}}_{\alpha\alpha} = \mathbf{H}_{\alpha\alpha} - \mathbf{H}_{\alpha\beta}\mathbf{H}_{\beta\beta}^{-1}\mathbf{H}_{\beta\alpha}$)によって除去され、初期推定ヤコビアン(First-Estimate Jacobians)が零空間を保存する。

NFRを用いたマッピング層。 キーフレームがウィンドウから外れると、Basaltはそのマルコフブランケットの線形化を保存し、キーフレーム姿勢以外のすべてをマージナリゼーションし、元のガウス分布 $N(\boldsymbol{\mu}_{\text{o}}, \mathbf{H}_{\text{o}}^{-1})$ と因子分解近似との間のカルバック・ライブラー divergence を最小化することで、その密な事前分布を近似する非線形因子を回復する。回復される残差は相対姿勢とロールピッチである(位置・ヨーは観測不能として除外される):

$$\mathbf{r}_{\text{rel}}(\mathbf{s}, \mathbf{z}_{\text{rel}}) = \mathrm{Log}(\mathbf{z}_{\text{rel}}\, \mathbf{T}_{j}^{-1} \mathbf{T}_{i}), \qquad \mathbf{r}_{\text{rp}}(\mathbf{s}, \mathbf{z}_{\text{rp}}) = \lfloor \mathbf{z}_{\text{rp}}\, \mathbf{R}_{i}^{-1} (0,0,-1)^{\top} \rfloor_{xy}$$

擬似計測値 $\mathbf{z}$ は現在の推定値から読み取られる(そのため平均は保存される)ため、情報行列は閉形式で得られる: $\mathbf{H}_{i} = (\{\mathbf{J}_{\text{r}} \boldsymbol{\Sigma}_{\text{o}} \mathbf{J}_{\text{r}}^{\top}\}_{i})^{-1}$。次に大域マッピングはORB特徴を検出・マッチングし(VIOのKLT点とは統計的に独立であり、暗黙のループ閉じ込みを与える)、再投影誤差と回復された因子エネルギー $E_{\text{nfr}}$ を最小化する — これはキーフレーム姿勢と地図点のみに対するバンドル調整であり、速度もバイアスも含まず、すべてを自由に再線形化できる。ロールピッチ因子は大域地図を重力方向に整列させ続け、相対姿勢因子は特徴マッチングのない区間を橋渡しする。

実験結果

EuRoC上(RMS ATE、V2_03はフレーム欠落400超のため除外): VIO層はオドメトリ手法の中で10系列中8系列で最良 — 例えばMH_01–05で0.07 / 0.06 / 0.07 / 0.13 / 0.11 m、V1_01–V2_02で0.04 / 0.05 / 0.10 / 0.04 / 0.05 m — VI-DSO(5系列で最良)に対抗し、OKVISやVINS-Fusionを明確に上回る。フルマッピングシステムは0.02〜0.10 mを達成し、特に大きなキーフレーム間隔を持つmachine-hall系列でVI ORB-SLAMを上回る(MH_04: 0.10 対 0.22 m)。またV1_03(0.03 m)では、純粋なBAが失敗し、等重み因子を用いたBAも0.56 mまで劣化するのに対して追跡に成功しており — KLDで回復された重みが、単なる因子のトポロジー以上に重要であることを示している。処理時間(Intel E5-1620): VIOは平均7.83 ms/フレーム、フレームの約11.5%がキーフレームとなり、マッピングはキーフレームあたり52.8 ms; MH_05(2273ステレオフレーム、114秒)はVIO 19.2秒 + マッピング9.7秒で処理され、実時間の約4倍の速さであり、単純なIMU積分に比べて大域状態が2.5倍小さい。

SLAMにおける意義

Basaltは、VIOとマッピングを組み合わせたすべてのシステムが直面する問い — オドメトリの情報を、線形化点を凍結したり生の計測値を保持したりせずに大域最適化へ引き継ぐにはどうすればよいか — に対して、原理に基づいた解答を与えた。「オドメトリから何を保持すべきか」を分布近似問題(累積情報に最も適合する非線形因子は何か)として捉えたことで、このアイデアは応用範囲の広いものとなった — これはOKVIS2におけるマージナリゼーションされたランドマークのポーズグラフエッジとしての扱いなど、後続システムにも影響を与えた — そして、その高品質なオープンソース実装は、EuRoCやTUM-VI上でよく使われる高精度ベースラインとなっている。

ハンズオン

• Basalt-VIOを実行する

関連ノート

• OKVIS — Basaltが標的とするマージナリゼーションの弱点を持つ、スライディングウィンドウアーキテクチャ。
• Marginalization — その基盤となるメカニズムと線形化の落とし穴。
• DM-VIO — 同じ不整合問題に対する別の対処法(遅延マージナリゼーション)。
• OKVIS2 — マージナリゼーションされた情報を再活性化できる次世代システム。
• VI-DSO — 論文の評価における最も近いVIO競合手法。
• Camera models beyond pinhole — Basaltのオープンソース実装で用いられる魚眼モデルの背景。