Kimera-VIO

Rosinol 2020 · 論文

一行要約 — Kimeraライブラリの高速ステレオ・インエルシャルオドメトリモジュール:多様体上IMUプレインテグレーションとGTSAMのstructureless視覚ファクタをiSAM2固定ラグスムージングで解く——リアルタイム・CPUのみで動くメトリック・セマンティックSLAMパイプラインの状態推定コア。

問題

人の周りで動くロボットは、軌跡だけでなくメートル精度かつセマンティックにラベル付けされたシーンモデルを必要とする。既存のオープンライブラリ(ORB-SLAM、VINS-Mono、OKVIS、ROVIO)はポーズと疎な点で止まってしまい、一方リアルタイムのメトリック・セマンティックシステム(SLAM++、SemanticFusion、Voxblox++)はRGB-Dセンシングとほとんどの場合GPUに依存する。Kimeraは、ビジュアル・インエルシャルセンシングを用いて、CPU上でリアルタイムに「メッシュ再構成と3Dセマンティックラベリングを実現することで、既存のビジュアル・ビジュアルインエルシャルSLAMライブラリを超える」ことを目指す——それを支えるのは、全体のスタックを担えるほど高速かつ正確なVIOフロントエンドである。

手法とアーキテクチャ

Kimeraはステレオフレームと高レートIMUデータを取り込み、4つのモジュールをホストする4つのスレッドで並列化される。

• Kimera-VIOは、Forsterらのキーフレームベースの最大事後確率ビジュアル・インエルシャル推定器を固定ラグ(またはオプションで全)スムーザーとして実装する。フロントエンドはキーフレーム間で多様体上IMUプレインテグレーションを実行し、視覚側ではShi-Tomasiコーナーを検出してLucas-Kanadeトラッカーで追跡し、左右ステレオマッチを見つけ、幾何検証を実行する——単眼用の5点RANSACとステレオ用の3点RANSAC、IMU回転を用いる2点/1点変種はオプション。検出、ステレオマッチング、検証はキーフレームでのみ実行され、中間フレームは追跡のみされる。バックエンドはプレインテグレーションされたIMUファクタとstructureless視覚モデルをGTSAMファクタグラフに追加し、iSAM2で解く:各イテレーションで、観測された特徴は現在の姿勢推定からDLTで三角測量され、退化点(カメラ背後、低視差)と外れ値(大きな再投影誤差)を除去した後、3D点はVIO状態から解析的に消去される。スムージングホライズンを超えた状態は周辺化され、フロントエンドはIMUレートで状態推定を出力する。
• Kimera-RPGOはDBoW2バグオブワーズとモノ/ステレオ幾何検証でループクロージャを検出し、*インクリメンタルなペアワイズ一貫性測定集合最大化(PCM)*によるロバストPGOを適用する:各ループはそのサイクルに沿ったオドメトリと一貫していなければならず(カイ二乗検定)、以前のループともペアワイズ一貫性を持たなければならず、インクリメンタルに拡大する隣接行列$\boldsymbol{A}\in\mathbb{R}^{L\times L}$で追跡される。Gauss-Newton最適化の前に、高速な最大クリーク探索が最大の一貫集合を選択する。
• Kimera-Mesherは、バックエンドの深度で逆投影された追跡特徴に対する2D Delaunay三角測量によってフレームごとの3Dメッシュを5ms未満で構築し、VIOホライズンにわたるマルチフレームメッシュも構築する。検出された平面はレギュラリティファクタとしてVIOにフィードバックされる——メッシュ正則化と状態推定のタイトな結合である。
• Kimera-Semanticsは各キーフレームで密なステレオ(semi-global matching)を実行し、点群をバンドルレイキャスティングによってVoxblox TSDFに融合し、2Dセグメンテーションからのボクセルごとのセマンティックラベル確率をベイズ更新しながら、marching cubesでグローバルなメトリック・セマンティックメッシュを抽出する。

これらのモジュールは「単独でも組み合わせても実行できるため、Kimeraは最先端のVIOにも完全なSLAMシステムにも容易にフォールバックできる」。

実験結果

EuRoC(RMSE ATE、SE(3)アライメント)において、Kimeraは各カテゴリでトップの性能を達成する:固定ラグスムージングでは、Kimera-VIOは0.050.35 m(例:MH_1 0.11、V1_3 0.07)に達する一方、OKVISは0.090.47、MSCKFは0.101.13、ROVIOは0.100.52、VINS-Monoは0.080.32である。全スムージングではMH_1で0.04 mであり、SVO-GTSAMは3つのVシーケンスで失敗する。ループクロージャありでは、Kimera-RPGOはV2_3で0.19 mを達成し、VINS-LCは1.39 mを報告している。ロバスト性:PCMなしでは、DBoW2しきい値$\alpha$を緩めるとPGO誤差は1.74 mまで爆発するが、Kimera-RPGOはすべての$\alpha$で約0.05 mを維持する——パラメータ調整不要。幾何:グローバルセマンティックメッシュはEuRoCのグラウンドトゥルース点群に対して0.350.48 mの精度を持ち、高速なマルチフレームメッシュは最大24%ノイズが多いが2桁高速である。フォトリアリスティックなシミュレータでは、Kimera-Semanticsはグラウンドトゥルース深度とKimera-VIOのポーズ(ATE 0.04 m)で80.03% mIoUに達し、密なステレオでは57.23%に低下する。タイミング(CPU):IMUプレインテグレーションは約40 µs(200 Hz超の状態出力)、追跡は4.5 ms/フレーム、キーフレーム処理は45 ms、バックエンドは40 ms未満、RPGOは約55 ms、セマンティクスはキーフレームあたり約0.1秒。

SLAMにおける意義

Kimeraは、クリーンでモジュラーなオープンソースのスタックが、生のステレオ+IMUからセマンティックラベル付き3DメッシュへリアルタイムでCPU上で到達できることを示した——実用的なVIOベースラインであると同時に、3D Dynamic Scene Graphs、Hydra、Kimera-Multiといった一連の研究系譜の基盤にもなっている。GTSAMスマートファクタ+iSAM2という手法は、Ceresスタイルのスライディングウィンドウ最適化に対する標準的な代替として定着しており、Kimera-RPGOのロバストなポーズグラフ最適化はスタンドアロンライブラリとして使用されている。

ハンズオン

• Kimeraを実行する

関連ノート

• 多様体上のIMUプレインテグレーション — Kimera-VIOが用いるIMUファクタ。
• Kimera / 3D Dynamic Scene Graph — このVIOの上に構築されたシーン理解レイヤー。
• Kimera-Multi — マルチロボット拡張。
• インクリメンタルスムージング — 背後にあるiSAM2の機構。
• GNC — 後にKimera-RPGOに採用されたロバスト最適化。
• MSCKF — structureless測定アイデアのフィルタ系祖先。