VINS-Fusion

Qin 2019 · 論文

一行要約 — VINS-Fusionは、VINS-Monoを一般化し、あらゆるセンサを一般的な因子として扱う最適化ベースのローカルオドメトリのフレームワークとした；同じ状態変数を共有する因子は一つのスライディングウィンドウ問題に足し合わされ、これはステレオのみ、単眼+IMU、ステレオ+IMUのスイートで単一のオープンソースコードベースにより実証されている。

問題

ロボットは多様化するセンサスイートを搭載する — 地上車両のステレオカメラ、スマートフォンの単眼カメラ+IMU、空中ロボットのステレオ+IMU — しかしほとんどの状態推定器は単一のセンサや特定のスイートのために設計されており、プラットフォーム間で移植できない。実用的なシステムには、センサの優雅な故障処理も必要である：非活性なセンサは取り除けるべきであり、代替のセンサを迅速に追加できるべきである。VINS-Fusionは、あらゆるセンサがポーズグラフの単なる残差因子であるような、一つの一般的な最適化ベースフレームワークを提案する。

手法とアーキテクチャ

状態. スライディングウィンドウは、機体のポーズに加えて、任意のセンサ固有の変数を推定する。

X=[p0,R0,…,pn,Rn,xcam,ximu],xcam=[λ0,…,λl],ximu=[v0,ba0,bg0,… ]\mathcal{X} = [\mathbf{p}_0, \mathbf{R}_0, \dots, \mathbf{p}_n, \mathbf{R}_n, \mathbf{x}_{cam}, \mathbf{x}_{imu}], \quad \mathbf{x}_{cam} = [\lambda_0, \dots, \lambda_l], \quad \mathbf{x}_{imu} = [\mathbf{v}_0, \mathbf{b}_{a_0}, \mathbf{b}_{g_0}, \dots]

ここでλ\lambdaは各特徴の最初に観測されたフレームにおける深度である；ximu\mathbf{x}_{imu}（速度とIMUバイアス）は、ステレオのみのスイートでは単純に省略される。状態推定は独立なガウス観測に対するMLE、すなわち非線形最小二乗である。

X∗=arg⁡min⁡X∑t=0n∑k∈S∥ztk−htk(X)∥Ωtk2\mathcal{X}^{*} = \arg\min_{\mathcal{X}} \sum_{t=0}^{n} \sum_{k\in\mathbf{S}} \left\lVert \mathbf{z}^{k}_{t} - h^{k}_{t}(\mathcal{X}) \right\rVert^{2}_{\mathbf{\Omega}^{k}_{t}}

カメラ因子. KLTで追跡されるShi-Tomasiコーナー（ステレオの場合は左右でマッチングされる）；この因子は、特徴llを最初の観測画像iiから画像ttへ再投影する。

ztl−htl(X)=[utlvtl]−πc((Tcb)−1 Tt−1 Ti Tcb πc−1(λl,[uilvil]))\mathbf{z}^{l}_{t} - h^{l}_{t}(\mathcal{X}) = \begin{bmatrix} u^{l}_{t} \\ v^{l}_{t} \end{bmatrix} - \pi_c\Big( (\mathbf{T}^{b}_{c})^{-1}\, \mathbf{T}_{t}^{-1}\, \mathbf{T}_{i}\, \mathbf{T}^{b}_{c}\, \pi_c^{-1}\big(\lambda_l, \begin{bmatrix} u^{l}_{i} \\ v^{l}_{i} \end{bmatrix}\big) \Big)

ここでπc\pi_cはカメラモデルの投影、Tcb\mathbf{T}^b_cは機体-カメラ外部パラメータである。同じ因子が時間的な観測（左-左）と空間的な観測（左-右）の両方に用いられる — 空間的な観測はキャリブレーションされたベースラインを通じてメトリックスケールを制約し、IMUの励起は不要である。

IMU因子. 連続するフレーム間のオンマニフォルドのプレインテグレーションにより、擬似観測αtt−1,βtt−1,γtt−1\boldsymbol{\alpha}^{t-1}_{t}, \boldsymbol{\beta}^{t-1}_{t}, \boldsymbol{\gamma}^{t-1}_{t}（相対位置、速度、回転）と伝播された共分散が得られる；残差はこれらを状態から予測される運動と比較する、例えば位置についてはαtt−1⊖Rt−1−1(pt−pt−1+12g dt2−vt−1dt)\boldsymbol{\alpha}^{t-1}_{t} \ominus \mathbf{R}_{t-1}^{-1}(\mathbf{p}_t - \mathbf{p}_{t-1} + \tfrac{1}{2}\mathbf{g}\,dt^2 - \mathbf{v}_{t-1} dt)であり、バイアスはランダムウォークとしてモデル化される。

最適化とマージナライズ. 合計コストはCeresにおいてガウス・ニュートン法/レーベンバーグ・マルカート法で解かれる。ウィンドウは10個の空間カメラフレームを保持し、新しいキーフレームが到着すると、最古のフレームの視覚および慣性因子はシュール補元によってマージナライズされる。

Hp=Hrr−HrmHmm−1Hmr,bp=br−HrmHmm−1bm\mathbf{H}_p = \mathbf{H}_{rr} - \mathbf{H}_{rm}\mathbf{H}_{mm}^{-1}\mathbf{H}_{mr}, \qquad \mathbf{b}_p = \mathbf{b}_r - \mathbf{H}_{rm}\mathbf{H}_{mm}^{-1}\mathbf{b}_m

これにより問題は情報損失なしに事前項を持つMAPへと変わる。残差因子に還元できるセンサであれば何でも組み込めるため（車輪オドメトリ、LiDAR、レーダーが挙げられている）、センサ故障は非活性なセンサの因子を取り除き別のセンサの因子を追加することで処理される。

実験結果

EuRoC（RMSE ATE、Horn整列）において、3つのスイートはOKVISと比較される：単眼+IMUはMH_02で0.09 m（OKVISは0.22）、V1_02で0.09 m（OKVISは0.20）、V2_01で0.06 m（OKVISは0.13）を達成し、ほとんどのシーケンスでOKVISを上回る。ステレオのみはV1_03とV2_03で失敗する（動きが視覚追跡には激しすぎる）、他の場所でも最もドリフトする；IMU支援のあらゆる構成は全11シーケンスで生存する — IMUは照明変化、テクスチャレス領域、動きブラーを橋渡しし、重力を観測することでロール/ピッチのドリフトを抑制する。ステレオ+IMUは常に最良ではなく、キャリブレーション誤差に対してより敏感である。屋外のハンドヘルド実験（20 HzのmvBlueFOXステレオ + 200 HzのDJI A3 IMU、GPSを正解として使用）では、約224–232 mのループにおいて、RMSEはステレオのみの1.85–2.59 mからIMU融合で0.43–0.75 mまで下がる。GPS融合はこの論文では将来の課題として言及されているが、オープンソース版のVINS-Fusionでは、ローカルオドメトリとグローバルな位置観測を融合する付随のグローバルポーズグラフモジュールとして同梱されている。

SLAMにおける意義

VINS-Fusionは、ロボティクスで最も広く展開されているオープンソースオドメトリスタックの一つである：学術的に成功したVINS-Monoを取り上げ、ステレオによるスケール観測可能性とセンサ非依存の因子定式化を加えることで実際の車両に対して実用的なものとし、後にGPSドリフト補正で拡張された。その「あらゆるものが因子である」という設計は、自動運転やドローンの自律性におけるローカル+グローバル融合のテンプレートとなり、ステレオ慣性推定の標準的なベースラインとして残っている。

関連ノート