Open Source Robotics

高品質なオープンソースロボティクスプロジェクト、ツール、ソフトウェアの厳選コレクション。

Factor Graph

  • カテゴリ: State estimation & navigation › Localisation
  • タグ: state-estimation, slam, probabilistic-inference, optimization, sensor-fusion

確率分布をファクターの積として表す二部グラフで、姿勢やランドマークといった未知変数を、それらを制約する計測や事前分布と結び付ける。

定義

ファクターグラフ（Factor graph）には 2 種類のノードがある。変数ノードは推定すべき未知量、たとえばロボットの姿勢、速度、センサーのバイアス、ランドマークの位置を表す。ファクターノードはそれらについての情報、たとえば事前分布、2 つの姿勢間のオドメトリ計測、カメラによるランドマークの観測を表し、各ファクターは自身が関わる変数にのみ接続される。グラフはすべてのファクターの積に等しい同時分布を記述するため、問題の構造、すなわちどの未知量が結び付いており、どの計測が疎で局所的な制約であるかが明示される。

ファクターグラフは Kschischang、Frey、Loeliger（2001）によって推論の汎用的な枠組みとして示され、主に Dellaert と Kaess による平滑化とマッピングの研究を通じてロボティクスで広く採用された。ガウスノイズモデルのもとでは、すべてのファクターが与えられたときにすべての変数の最も確からしい値を求めることは、非線形最小二乗問題になる。各ファクターは情報行列で重み付けされた残差を 1 つずつ寄与する。ソルバは残差を線形化し、グラフの疎性を利用して得られた線形システムを効率よく分解し、Gauss-Newton 法または Levenberg-Marquardt 法で反復する。iSAM2 のようなインクリメンタルソルバは、新しいファクターが届くたびに、ゼロから解き直すことなく解を更新する。GTSAM はファクターグラフを直接実装しており、g2o や Ceres のような汎用の最小二乗ライブラリも同種の問題に使われる。

実用上の主な魅力はモジュール性である。新しいセンサーは独自の残差とノイズモデルを持つ新しいファクタータイプになるため、IMU プリインテグレーション、GPS、ホイールオドメトリ、LiDAR のスキャンマッチング、ループクロージャ、学習済みの計測モデルをすべて 1 つの推定器に収められる。ポーズグラフ最適化は、すべての変数が姿勢で、すべてのファクターが相対姿勢の制約である特殊なケースである。一般のファクターグラフは、ランドマーク、速度、バイアス、キャリブレーションパラメータも保持できる。固定ラグ平滑化器は古い変数を周辺化して計算量を抑えるが、その代わりに一部の線形化点が固定される。誤ったループクロージャなどの外れ値には、ロバストなコスト関数や切り替え可能な制約が必要である。単純な最小二乗解は、たった 1 つの不良ファクターによって大きく引きずられることがあるからだ。

重要な理由

ファクターグラフは、現代の SLAM とマルチセンサー状態推定の共通言語である。推定器を設計し直すことなくセンサー、事前分布、制約を追加でき、各計測がどのようなノイズと独立性の仮定を持つかを明示できる。

主な用途

  • LiDAR、視覚、視覚慣性システムの SLAM バックエンド
  • IMU、GPS、ホイールオドメトリ、ビジョンのマルチセンサーフュージョン
  • カメラ、LiDAR、IMU の外部パラメータのキャリブレーション
  • Structure from Motion とバンドル調整
  • 確率的推論として定式化した軌道最適化
  • 共有制約を用いたマルチロボットマッピング

実際の使われ方

ある地上車両が、事前に記録した地下のマップに照らして地中レーダ（GPR）で自己位置推定を行う。各時間ステップで姿勢変数が 1 つ追加され、慣性またはホイールオドメトリが連続する姿勢の間にファクターを追加し、事前マップに対するレーダのマッチングが現在の姿勢にファクターを追加する。レーダ用のセンサーモデルを手書きする代わりに、GPR による自己位置推定のための学習済みファクターグラフモデルの研究のように、計測ファクターをデータから学習することもできる。チームはインクリメンタル平滑化器を動かして最新の姿勢をリアルタイムで得られるようにし、テスト走行のたびにファクターの残差を調べて、ノイズモデルが厳しすぎる、または緩すぎる計測タイプを見つける。

よくある誤解

  • ファクターグラフはソルバである — これは問題の表現であり、バッチ Gauss-Newton やインクリメンタル平滑化といったソルバは別の選択である
  • ファクターグラフはポーズグラフしか記述できない — ランドマーク、速度、バイアス、キャリブレーションパラメータ、任意の計測モデルを保持できる
  • ファクターを増やせば推定は常に改善する — ロバストコストを使わない限り、ノイズモデルが誤ったファクターや外れ値の計測が解全体を損なうことがある

知っておくべき人

  • SLAM と状態推定のエンジニア
  • センサーフュージョンとナビゲーションの開発者
  • ロボティクスにおける確率的推論の研究者
  • 自動運転車の自己位置推定チーム

関連技術

  • GTSAM
  • iSAM2
  • g2o
  • Ceres Solver
  • バンドル調整（Bundle Adjustment）
  • Levenberg-Marquardt 法

関連用語

  • Pose Graph Optimization
  • SLAM
  • IMU Preintegration
  • Loop Closure
  • Sensor Fusion
  • Tightly Coupled Sensor Fusion

関連プロジェクト

  • LingBot-Map: Streaming 3D reconstruction from one camera: pose and depth for every frame, live
  • oomwoo: Open-source DIY robot vacuum: 3D-printed, ROS2 + 2D LiDAR, fully local.
  • Cartographer: Google's real-time 2D and 3D SLAM system for multi-platform robotics
  • FAST-LIVO2: Fast, direct LiDAR-inertial-visual odometry from HKU-MARS, published in IEEE T-RO
  • DimOS: Python robotics runtime with agents, SLAM and teleop, no ROS required
  • evo: De-facto standard tool for evaluating and plotting odometry/SLAM trajectories

動画

  • Factor Graph - 5 Minutes with Cyrill: Cyrill Stachniss
  • MIT Robotics - Frank Dellaert - Factor Graphs for Perception and Action: MIT Robotics

このウェブサイトにはJavaScriptが必要です。オープンソースロボティクスプロジェクトを閲覧するにはJavaScriptを有効にしてください。