OKVIS

Leutenegger 2015 · 論文

一行要約 — OKVIS(Open Keyframe-based Visual-Inertial SLAM)は、VIOにおけるタイトカップリング型スライディングウィンドウ最適化のパラダイムを確立した:キーフレームと最近のフレームの有限なウィンドウにわたって再投影誤差とIMU誤差を同時に最小化し、Schur補元による周辺化がそれより古いものをすべて事前分布に圧縮する。

問題

フィルタベースのVIO(MSCKF系)は各測定を更新時に一度だけ線形化するため、蓄積する線形化誤差が精度を損なう。完全なバンドル調整はすべてを各イテレーションで再線形化し、ドリフトははるかに小さくなるが——過去のすべてのフレームにわたるBAはリアルタイムでは実行できず、数百Hzで到来する慣性測定値は連続する状態間に密な時間的制約を作り出す。OKVISはこの緊張を有限なキーフレームウィンドウで解決する:キーフレームは時間的に任意に離れて配置できる(そのため、静止していても推定値はドリフトフリーのままである)し、古い状態は周辺化によってガウス事前分布に折り込まれる。

手法とアーキテクチャ

• 状態と結合コスト。 各ロボット状態は姿勢、速度、IMUバイアスを保持する: $\mathbf{x}_R = \begin{bmatrix} {}_W\mathbf{r}_S^\top & \mathbf{q}_{WS}^\top & {}_S\mathbf{v}^\top & \mathbf{b}_g^\top & \mathbf{b}_a^\top \end{bmatrix}^\top$、加えて3Dランドマーク${}_W\mathbf{l}^j$と(オプションでオンラインキャリブレーションされる)カメラの外部パラメータ。推定器は、重み付き再投影誤差$\mathbf{e}_r$とIMU誤差項$\mathbf{e}_s$を結合した1つのコストを最小化する(式7): $$J(\mathbf{x}) := \sum_{i=1}^{I}\sum_{k=1}^{K}\sum_{j \in \mathcal{J}(i,k)} \mathbf{e}_r^{i,j,k\,\top}\,\mathbf{W}_r^{i,j,k}\,\mathbf{e}_r^{i,j,k} \;+\; \sum_{k=1}^{K-1} \mathbf{e}_s^{k\,\top}\,\mathbf{W}_s^{k}\,\mathbf{e}_s^{k},$$ カメラ番号$i$、フレーム番号$k$、ランドマーク番号$j$、情報行列$\mathbf{W}$を用いて——(フィルタとは異なり)各イテレーションで再線形化されるGoogle Ceresで解かれる。
• 再投影誤差。 $\mathbf{e}_r^{i,j,k} = \mathbf{z}^{i,j,k} - \mathbf{h}_i\big(\mathbf{T}_{C_iS}\,\mathbf{T}_{SW}\,{}_W\mathbf{l}^j\big)$、ここで$\mathbf{h}_i$はカメラ$i$の(歪み込みの)投影である;解析的ヤコビアンは周辺化ステップの要素としても兼用される。
• IMU誤差項。 フレーム$k$と$k{+}1$の間の生のIMU測定値は古典的なルンゲ・クッタ法(この論文は多様体上プレインテグレーション以前のもの)で積分され、予測$\hat{\mathbf{x}}^{k+1}$を得る。15次元の残差は、予測と推定値との差である——位置、最小クォータニオン誤差$2\big[\hat{\mathbf{q}}_{WS}^{k+1} \otimes \mathbf{q}_{WS}^{k+1\,-1}\big]_{1:3}$、速度、バイアス——情報行列$\mathbf{W}_s^k$は共分散$\mathbf{P}\big(\delta\hat{\boldsymbol{\chi}}_R^{k+1}\,\vert\,\mathbf{x}_R^k, \mathbf{z}_s^k\big)$を残差ヤコビアンを通じて伝播させて得られる。
• キーフレームウィンドウ。 最適化は最も新しい$S$個のフレーム(時間的/IMUウィンドウ)と、過去のずっと遠くにありうる$M$個のキーフレームにわたって行われる。投影・マッチングされたランドマークの凸包が画像の約50%未満をカバーするか、検出されたキーポイントの約20%未満しかマッチしない場合、フレームはキーフレームになる——こうして保持されるキーフレームは多様な視点をカバーする。
• 周辺化。 状態$\mathbf{x}_\mu$を落とすことは、Gauss-Newton系$\mathbf{H}\delta\boldsymbol{\chi} = \mathbf{b}$にSchur補元を適用する: $$\mathbf{H}^{*}_{\lambda\lambda} = \mathbf{H}_{\lambda\lambda} - \mathbf{H}_{\lambda\mu}\mathbf{H}_{\mu\mu}^{-1}\mathbf{H}_{\mu\lambda}, \qquad \mathbf{b}^{*}_{\lambda} = \mathbf{b}_{\lambda} - \mathbf{H}_{\lambda\mu}\mathbf{H}_{\mu\mu}^{-1}\mathbf{b}_{\mu},$$ 線形化点は周辺化時点での推定値に固定される(first-estimateの扱い)。非キーフレームはその測定値を捨てて状態が周辺化され、古いキーフレームはそこでのみ観測されるランドマークとともに周辺化され、問題の疎性を保つ。
• フロントエンド。 マルチスケールでSSE最適化されたHarrisコーナーとBRISKディスクリプタ(均一なキーポイント分布を強制)、マハラノビス検定とアブソリュートポーズRANSACによってゲートされたブルートフォース3D-2Dマッチング、続いて最新キーフレームに対する2D-2Dマッチング、三角測量、相対RANSAC。ステレオ版とモノラル版はパイプラインを共有する。

実験結果

カスタムFPGA同期ステレオ慣性センサ(ADIS16448 IMU、800 Hz、2台のWVGAグローバルシャッターカメラ、20 Hz、11 cm基線)によるデータセットを用いて、同じキーポイントとIMUデータを与えられたMSCKFスタイルの確率的クローニングスライディングウィンドウフィルタ(参照)と比較し、$M{=}7$個のキーフレームと$S{=}3$個の最近フレームで評価。1200 mのVicon Loopsシーケンス(14分、Vicon真値)では、すべての手法が走行距離に対する位置誤差の中央値0.1%未満に留まるが、OKVISはフィルタよりヨードリフトが少ない。7.9 kmのBicycle Trajectory(23分、速度最大13.1 m/s、DGPS真値)と620 mのETH Main Buildingハンドヘルドデータセットでは、ステレオ版(aslam)と単眼版(aslam-mono)の両方がmsckf-monoを一貫して上回る。粗いCAD推定からのオンライン外部パラメータキャリブレーションは、誤キャリブレーションが引き起こすスケール誤差を取り除く。キーフレームを7から12に増やしても有意な向上はなく、1画像あたりのキーポイントを240から45に減らしても精度の低下は軽微である。

SLAMにおける意義

OKVISが定義したスライディングウィンドウBA+周辺化のアーキテクチャは、VINS-Mono、Basalt、ORB-SLAM3のVIモード、DM-VIO、OKVIS2すべてが従うテンプレートであり、タイトカップリングされた非線形最適化がフィルタリングを精度において許容可能なコストで上回るという最初の強力な証拠を提供した。そのキーフレーム選択ロジックと周辺化戦略は、「VIOの計算量を情報を捨てずに有限に抑えるにはどうすればよいか」という問いに対する今なお標準的な答えである。OKVISは長い系譜も生んだ:OKVIS2は再活性化可能なランドマークによるループクロージャを追加し、OKVIS2-XはフレームワークをLiDAR、深度、GNSSへ拡張した。

関連ノート

• VINS-Mono — このアーキテクチャの最も広く展開された後継。
• OKVIS2 — スケーラブルなループクロージャを追加した直接の後継。
• Basalt — OKVISスタイルの周辺化事前分布の線形化の弱点に対処する。
• 多様体上のIMUプレインテグレーション — 後に標準となったIMUファクタの定式化。
• 周辺化 — スライディングウィンドウの背後にある核心的な機構。
• Schur補元 / 疎性 — その圧縮を行う線形代数のツール。