Pose Graph Optimization

• 別名: PGO, pose-graph SLAM, pose graph optimisation
• カテゴリ: State estimation & navigation › Localisation
• タグ: slam, state-estimation, optimization, localization, mapping, factor-graphs

姿勢からなるグラフを調整して、姿勢間のオドメトリ制約とループクロージャ制約ができるだけ一致するようにし、ロボットの軌跡を精密化する SLAM のバックエンド技術。

定義

ポーズグラフ最適化（Pose graph optimization）は、グラフ上の最小二乗問題を解くことでロボットの軌跡を推定する処理である。グラフの各ノードはロボットの姿勢（ある時刻における位置と向き）であり、各エッジは二つの姿勢間で計測された相対変換とその不確かさで、不確かさは通常、情報行列として表される。エッジは、連続する姿勢間のオドメトリと、ロボットが以前に訪れた場所を認識するループクロージャから得られる。

すべての計測にはノイズがあるため、エッジ同士が完全に一致することはない。ループに沿ってオドメトリのエッジをつなげても、ループクロージャのエッジが示す位置にロボットがちょうど戻ることはない。ポーズグラフ最適化は、全エッジにわたる重み付き二乗誤差の総和を最小にする姿勢の集合を求め、各計測の確からしさに比例して補正を軌跡全体に分散させる。

回転は加法的に合成できないためこの問題は非線形であり、Gauss-Newton 法や Levenberg-Marquardt 法などで、各ステップで現在の推定値のまわりで線形化しながら反復的に解く。各姿勢は少数のほかの姿勢としかつながらないため、得られる線形方程式は疎であり、疎行列ソルバーによって何千もの姿勢を持つグラフも扱えるようになる。この段階には g2o、GTSAM、Ceres Solver などのオープンソースライブラリがよく使われる。

ポーズグラフ最適化は、ほとんどのグラフベース SLAM システムのバックエンドである。バンドル調整との違いは、ランドマークを変数として保持しない点にある。最適化するのは姿勢だけであり、より高速だが一部の情報を捨てることになる。既知の弱点は誤ったループクロージャに敏感なことであり、そのためロバストなコスト関数と外れ値の除去が標準的に追加される。

重要な理由

オドメトリやビジュアルオドメトリは時間とともに際限なくドリフトし、ポーズグラフ最適化は、ループクロージャが見つかった時点でその蓄積したドリフトを取り除く標準的な方法である。局所的には正確だが大域的には整合しない計測の集合を、建物や敷地全体で整合した軌跡と地図に変える。

主な用途

• LiDAR やカメラによる地図作成のためのグラフベース SLAM のバックエンド
• 車輪、ビジュアル、ビジュアル慣性オドメトリから蓄積したドリフトの除去
• 異なる時期に記録した地図を統合するマルチセッションの地図作成
• 複数のロボットの軌跡を一つの座標系にまとめるマルチロボットの地図作成

実際の使われ方

典型的な LiDAR SLAM やビジュアル SLAM のスタックでは、フロントエンドが、ロボットが設定した距離や角度だけ動くたびに新しいノードを追加し、オドメトリのエッジで直前のノードとつなぐ。場所認識モジュールがループクロージャの候補を提案し、それらはエッジとして追加される前に幾何学的に検証される（たとえば ICP や特徴マッチングによって）。その後、バックエンドが g2o や GTSAM などの最適化器を、ループクロージャのたびにバッチで、または（iSAM2 のように）逐次的に実行し、補正された姿勢を使って地図を作り直す。調整の労力の多くは、各エッジに割り当てる共分散と、Huber や Cauchy などのロバストカーネルに費やされる。信頼度の高い誤ったループクロージャが一つあるだけで、地図全体が曲がってしまうことがあるからである。

よくある誤解

• ポーズグラフ最適化が地図を作る — 補正するのは姿勢だけであり、地図はその後、補正された姿勢にセンサデータを配置して作り直す
• ループクロージャは多いほどよい — 誤ったループクロージャが一つあるだけで軌跡全体が歪みうるため、クロージャは検証し、ロバストなコストで重みを下げなければならない
• バンドル調整と同じである — バンドル調整はランドマークの位置も最適化するが、ポーズグラフは姿勢と相対制約だけを保持する

知っておくべき人

• SLAM と自己位置推定のエンジニア
• 移動ロボットや自動運転車の開発者
• ビジュアルオドメトリと地図作成に取り組むコンピュータビジョンのエンジニア
• 確率ロボティクスと状態推定を学ぶ学生

関連技術

• g2o
• GTSAM
• Ceres Solver
• ファクターグラフ（Factor Graphs）
• バンドル調整（Bundle Adjustment）
• Levenberg-Marquardt 法（Levenberg-Marquardt Algorithm）

関連用語

• SLAM
• Odometry
• Visual Odometry
• Loop Closure

関連プロジェクト

• 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
• OpenDroneMap: Open-source photogrammetry toolkit that turns drone images into maps and 3D models
• Nav2: ROS 2 navigation stack with path planning and obstacle avoidance for mobile robots
• FAST-LIVO2: Fast, direct LiDAR-inertial-visual odometry from HKU-MARS, published in IEEE T-RO

動画

• Understanding SLAM Using Pose Graph Optimization | Autonomous Navigation, Part 3: MATLAB