LVI-SAM

Shan 2021 · 論文

一行要約 — LVI-SAMは、VINS-Mono方式の視覚-慣性サブシステムとLIO-SAM方式のLiDAR-慣性サブシステムを共有のファクターグラフ上でタイトカップリングし、それぞれのサブシステムが互いを初期化し、救済し合う。

問題

LiDARベースの手法は長距離にわたって細かい環境の詳細を捉えられるが、長い廊下や広く開けた平坦な場所のような構造の少ない環境では失敗しがちである。視覚ベースの手法は場所認識やテクスチャの豊富なシーンで優れているが、照明変化、急速な動き、初期化に敏感である。それぞれをIMUと組み合わせることは有効だが、どちらのペア単独でも実際の運用全体を通した頑健性は得られない。LVI-SAMは、視覚またはLiDARの一方の系統が劣化しても機能し続けるように、この3つのセンサーすべてを1つのフレームワークに融合する。

手法とアーキテクチャ

2つのサブシステムが1つのファクターグラフを共有し、iSAM2で最適化される。

• 視覚-慣性システム(VIS): VINS-Monoを改良したもので、Shi-TomasiコーナーをKLTで追跡し、状態 $\mathbf{x} = [\,\mathbf{R},\ \mathbf{p},\ \mathbf{v},\ \mathbf{b}\,]$ に対してスライディングウィンドウのバンドル調整を行う。ここで $\mathbf{R} \in SO(3)$ は回転、$\mathbf{p} \in \mathbb{R}^3$ は位置、$\mathbf{v}$ は速度、$\mathbf{b} = [\,\mathbf{b}_a, \mathbf{b}_w\,]$ は加速度計/ジャイロのバイアスであり、機体座標系からワールド座標系への変換は $\mathbf{T} = [\mathbf{R}\,|\,\mathbf{p}] \in SE(3)$ である。
• LiDAR-慣性システム(LIS): LIO-SAMを改良したもので、IMUによるスキュー除去の後、エッジおよび平面特徴が、キーフレームのスライディングウィンドウで保持される特徴マップとマッチングされる。姿勢の変化が閾値を超えると新しいキーフレーム(グラフノード)が追加される。IMUプリインテグレーション、視覚オドメトリ、LiDARオドメトリ、ループ閉じ込みという4種類のファクターが同時に最適化される。

システム間の相互支援がこの設計の核心である。

• 初期化: VINS-Mono方式の初期化は、低速または一定速度(加速度による励起がないためスケールが観測不能)でしばしば失敗するため、深度が直接観測可能なLISが先に初期化を行い、推定した $\mathbf{x}$ と $\mathbf{b}$ を初期推定値としてVISに渡す。
• LiDARからの特徴深度: 複数のLiDARフレームを重ね合わせて密な深度マップを作成する。特徴点と深度点はカメラ周囲の単位球に投影され、極座標での2次元K-Dツリーによって最近傍の深度点3点が見つけられ、特徴の深度はカメラ中心からこの3点が張る平面との交点までの光線の長さとなる。この3点間の最大距離が2mを超える場合(重ね合わせによる深度の曖昧性)、深度は関連付けられない。
• スキャンマッチングの初期推定値: 視覚-慣性オドメトリがLiDARスキャンマッチングの種となる。LIS初期化前は、線速度10 m/s未満・角速度180°/s未満であれば、生のIMU積分で十分である。
• 失敗検出: 追跡特徴数が閾値を下回るか、推定IMUバイアスが閾値を超えるとVISは失敗を報告し、再初期化する。LISはスキャンマッチングを $\min_{\mathbf{T}} \|\mathbb{A}\mathbf{T} - \mathbf{b}\|^2$ の反復解法として扱い、$\mathbb{A}^{\mathsf{T}}\mathbb{A}$ の最小固有値が閾値未満のとき失敗を報告し、その場合LiDARファクターは追加されない。
• 2段階ループ閉じ込み: BRIEF記述子を用いたDBoW2がVIS内で候補を提案し、LISがそれをスキャンマッチングで精緻化してから制約がグラフに追加される。

実験結果

Velodyne VLP-16、FLIRカメラ、MicroStrain 3DM-GX5-25 IMU、RTK GPSを地上真値として使用し、自己収集の3つのデータセット(Urban、Jackal、Handheld)において、Intel i7-10710Uラップトップ上でVINS-Mono、LOAM、LIO-mapping、LINS、LIO-SAMと比較評価した。

• Urbanアブレーション(エンドツーエンド誤差): VISのみ239.19 m → LiDAR深度併用で142.12 m; LISのみ290.43 m(劣化箇所で発散); ループ閉じ込みなしの完全なLVIOで45.42 m → 深度併用で32.18 m(29%削減); 全モジュール有効時: 並進0.28 m、回転5.77°。
• Jackal(UGV): LVI-SAM(ループ閉じ込みあり)がGPSに対する最良のRMSE 0.67 m(LIO-SAMの1.52 m、LINSの0.77 m、VINS-Monoの4.49 mに対して)、および最良のエンドツーエンド回転誤差1.52°を達成。
• Handheld(開けた野球場を横断): LiDARベースの手法はすべて完全に失敗する; LVI-SAMはループ閉じ込みありで0.83 m RMSE、エンドツーエンド並進誤差0.27 mで完走する(VINS-Monoのループ閉じ込みありは73.07 m RMSE)。

SLAMにおける意義

LVI-SAMは、LiDAR-視覚-慣性融合をファクターグラフで実現した代表例であり、双方向のセンサー相互支援を最も明確に示す例である。深度はLiDARからカメラへ流れ、初期化と初期推定値は双方向に流れる。LIO-SAMの著者らによる自然な拡張として構築されたこの手法は、標準的なオープンソースLVIベースラインとなり、直接法・フィルタベースの競合手法(R3LIVE、FAST-LIVO)が自らを比較する基準となった。また、3センサー融合における劣化対応の代表的な事例研究にもなっている。

関連ノート

• LIO-SAM — LiDAR-慣性サブシステムとファクターグラフの基盤
• VINS-Mono — 視覚-慣性サブシステムの設計基盤
• LiDAR-Visual-Inertial (LVI) — それが具現化する融合コンセプト
• Degradation handling — サブシステム間のフォールバック挙動
• FAST-LIVO — 直接法・フィルタベースの代替手法
• Tightly-coupled LiDAR-camera — ファクターグラフで実現するアーキテクチャ上の原則
• IMU preintegration — そのグラフにおける4種のファクターの1つ