FAST-LIVO

Zheng 2022 · 論文

一行要約 — FAST-LIVOは、LiDARマップ点に画像パッチを付加することで直接LiDAR-慣性オドメトリと直接視覚オドメトリを単一のESIKFに統合し、両モダリティから特徴抽出を取り除いた。

問題

既存のLVIシステム（R2LIVE、LVI-SAM）は特徴ベースのフロントエンド——視覚側ではコーナー抽出とスライディングウィンドウ最適化、LiDAR側ではエッジ／平面抽出——を実行しており、これは相当な計算コストを要し、顕著な特徴が乏しい場所では失敗する。FAST-LIVOは、FAST-LIO2がLiDARにもたらした直接的な思想がカメラにも拡張できるか、そして両センサが別々の視覚・LiDAR表現ではなく1つのマップを共有できるかを問う。

手法とアーキテクチャ

18次元多様体状態 $\mathbf{x} = [{^G}\mathbf{R}_I^T\ {^G}\mathbf{p}_I^T\ {^G}\mathbf{v}^T\ \mathbf{b_g}^T\ \mathbf{b_a}^T\ {^G}\mathbf{g}^T]^T \in SO(3)\times\mathbb{R}^{15}$ に対する単一の誤差状態反復カルマンフィルタ（ESIKF）で、測定値間はIMUによる前方伝播、各LiDARスキャンのスキュー除去には後方伝播を用いる。

• LIOサブシステム（FAST-LIO2から適応）：生のスキャン点——エッジ／平面特徴なし——を、点対平面残差でフレーム対マップにレジストレーションする。

$$\mathbf{r}_l(\mathbf{x}_k, {^L}\mathbf{p}_j) = \mathbf{u}_j^T\big({^G}\mathbf{T}_{I_k}\,{^I}\mathbf{T}_L\,{^L}\mathbf{p}_j - \mathbf{q}_j\big),$$

ここで $\mathbf{u}_j, \mathbf{q}_j$ は、インクリメンタルk-d木（ikd-Tree）内で見つかった最近傍5マップ点にフィットした平面の法線と中心である。

• VIOサブシステム（疎な直接方式、フレーム対マップ）：マップ点 ${^G}\mathbf{p}_i$ は過去の観測画像からのパッチピラミッドを保持し、参照パッチ $\mathbf{Q}_i$ は観測角度が最も近いものが選ばれ、アライメントは測光残差

$$\mathbf{r}_c(\mathbf{x}_k, {^G}\mathbf{p}_i) = \mathbf{I}_k\big(\boldsymbol{\pi}({^I}\mathbf{T}_C^{-1}\,{^G}\mathbf{T}_{I_k}^{-1}\,{^G}\mathbf{p}_i)\big) - \mathbf{A}_i\mathbf{Q}_i$$

（$\mathbf{A}_i$ はアフィンワープ、$\boldsymbol{\pi}$ はピンホール投影）を最小化することで行われ、3段のピラミッドレベルにわたって粗から密へ最適化される。ORB/FASTコーナーなし、三角測量なし、深度フィルタなしである。

• 測定ごとの単一MAP問題：時刻 $t_k$ にどちらのセンサのデータが到着しても、$\min_{\mathbf{x}_k} \big( \|\mathbf{x}_k \boxminus \hat{\mathbf{x}}_k\|^2_{\hat{\mathbf{P}}_k} + \sum_j \|\mathbf{r}_l\|^2_{\boldsymbol{\Sigma}_l} + \sum_i \|\mathbf{r}_c\|^2_{\boldsymbol{\Sigma}_c} \big)$ の反復更新が起動する——LiDARスキャンは $\mathbf{r}_l$ のみを、画像は $\mathbf{r}_c$ のみを融合し、両者は同じ状態と共分散に対して作用する（ガウス-ニュートン法と等価）。
• 共有マップ：LiDARグローバルマップは全点のikd-Treeであり、視覚グローバルマップはLiDAR点とそのパッチピラミッドをハッシュインデックス付きボクセルに保持する。視覚サブマップは、最新スキャンがヒットしたボクセルを問い合わせることで取得される。
• 外れ値除去：サブマップの点は予測されたポーズで投影され、40×40画素グリッドごとに最も低い深度の点のみが保持され、9×9近傍内の現在スキャン点によって遮蔽される点は除外される——測光アライメントを損なうエッジ点と遮蔽点を取り除く。
• マップ更新：アライメント後、最後のパッチ取得から20フレームまたは40画素以上経過している場合、測光誤差の大きい点には新しい8×8パッチが割り当てられる；新しいマップ点は40×40グリッドごとに勾配最大の投影LiDAR点であり、高曲率のエッジ点はスキップされる。

実験結果

• NTU-VIRALベンチマーク（UAV系列9本、Ouster OS1-16＋カメラ＋IMU）：FAST-LIVOは9系列中8で最良の絶対並進RMSEを達成した——例えばeee_01：0.28 m対0.54 m（FAST-LIO2）、0.45 m（R2LIVE）、2.88 m（DVL-SLAM、ループ閉じ込み除去版）、SVO2.0は失敗；nya_01：0.19 m。sbs_01（0.29 m対FAST-LIO2の0.25 m）のみ、激しいモーションブラーにより画像が有用でなかった。
• LiDAR退化壁（約30 mの壁に正面から向かう）：FAST-LIO2は制約されない方向にドリフトし、SVO2.0は繰り返しテクスチャ上でドリフトするが、FAST-LIVOは最小の端点ドリフト0.05 mを達成した。
• 視覚チャレンジ系列（屋内-屋外の遷移、2回の激しい運動、無地の白壁）：79.52 mの経路上で0.04 mの端点誤差で走破した。
• 実行時間：LiDAR＋画像1フレームあたり合計36.75 ms対R2LIVEの45.16 msフロントエンド＋59.27 msスライディングウィンドウバックエンド；VIOサブシステムはIntel i7で10.23 ms、ARM（Qualcomm RB5）で13.82 ms——両方でリアルタイム動作であり、ハード同期されたLivox Avia＋カメラのハードウェアもオープンソース化されている。

SLAMにおける意義

FAST-LIVOは、「直接」的な思想がLiDARから視覚モダリティへきれいに拡張できることの証明である——1つの共有マップ、1つのフィルタ、特徴フロントエンドなし。LiDARがすべての視覚アンカーに正確な深度を提供するため、視覚はほとんど追加コストなしにポーズ制約を与えることができる。このアーキテクチャ上の経済性（LVI-SAMの2本の特徴パイプラインや、R3LIVEの色付けのみのVIOと比較して）は、小型オンボードコンピュータ上での高レートLVIオドメトリのテンプレートとなり、これがFAST-LIVO2へ直接つながる。

関連ノート

• FAST-LIO2 — 直接LIOの基盤とikd-Treeマップ
• FAST-LIVO2 — 逐次ESIKF更新による改良版
• R3LIVE — マップ着色型VIOの思想を持つ姉妹システム
• SVO — 基盤となる疎な直接視覚アライメントのアイデア
• Direct LiDAR-camera alignment — このシステムが定義する概念
• LVI-SAM — 特徴ベース、ファクタグラフ方式の代替案