Pose graph optimization

Pose graph optimization (PGO) は、SLAMバックエンドを縮小した形式であり、変数はロボット/カメラの姿勢 $\{T_i\} \subset SE(3)$ のみ — 3Dマップ点は含まれない。グラフの構成は次の通り。

ノード: 姿勢 $T_i$ (通常はキーフレーム姿勢)。
エッジ: 不確かさ $\Sigma_{ij}$ を伴う相対姿勢制約 $T_{ij}$ 。これはオドメトリ(連続する姿勢間)およびループ閉じ込み(同一の場所と認識された非連続な姿勢間)から得られる。

各エッジは「ノード $i$ から見て、ノード $j$ は相対姿勢 $T_{ij}$ に現れるはずだ」と述べている。最適化は、すべての制約を同時に最もよく満たす姿勢の配置を見つける。

$\min_{\{T_i\}} \sum_{(i,j) \in \mathcal{E}} \left\| \log\!\left(T_{ij}^{-1}\, T_i^{-1} T_j\right) \right\|^2_{\Sigma_{ij}^{-1}}$

残差は $SE(3)$ の対数写像を用いて計算されるため、誤差は6自由度の接空間に存在する。問題は多様体上のGauss-NewtonまたはLevenberg-Marquardt法で解かれ、バンドル調整とまったく同様だが、変数の数は遥かに少ない。

視覚SLAMシステムにおける典型的な使用パターンは以下の通り。

フロントエンドがカメラを追跡し、オドメトリエッジを構築する。ドリフトが蓄積していく。
場所認識がループ閉じ込みを検出し、幾何学的検証により、時間的に離れた2つのキーフレーム間の相対姿勢エッジが生成される。
PGOは蓄積されたドリフトを軌跡全体に再分配し、ループを「パチンと閉じる」。
任意選択で、その後に完全なバンドル調整で姿勢と点を精密化する。

(多数の)ランドマーク変数を除外しているため、PGOは完全なバンドル調整よりもはるかに高速であり、ループ閉じ込み後のグローバルな軌跡補正における標準的なツールである — これはORB-SLAMのessential-graph最適化や、大半のLiDAR SLAMバックエンド(例えばGTSAMやg2o経由)が実行している処理である。単眼SLAMでは、スケールドリフトも補正できるように、 $SE(3)$ の代わりに $\mathrm{Sim}(3)$ 上でグラフを最適化することが多い。

一つ注意点がある。PGOはそのエッジを信頼する。1つの偽ループ閉じ込みエッジが、マップ全体を自身に折り畳んでしまう可能性があり、これがrobust pose-graph optimization手法を必要とする理由である。

最適化はどのように実行されるか

PGOは多様体上の非線形最小二乗問題である。回転は単純な加算で更新できないため、各Gauss-Newton/LM反復は接空間で作業する。

各エッジについて、残差 $\mathbf{e}_{ij} = \log\!\left(T_{ij}^{-1} T_i^{-1} T_j\right) \in \mathbb{R}^6$ と、2つの姿勢の微小な摂動 $\boldsymbol{\delta}_i, \boldsymbol{\delta}_j$ に関するそのヤコビアンを計算する。
正規方程式 $H \boldsymbol{\delta} = -\mathbf{b}$ を構築して解く。ここで $H = \sum J_{ij}^T \Sigma_{ij}^{-1} J_{ij}$ はスパースである。各エッジは2つの姿勢にのみ関わるため、 $H$ のスパース性パターンはグラフの隣接構造そのものになる。
リトラクション $T_i \leftarrow T_i \cdot \mathrm{Exp}(\boldsymbol{\delta}_i)$ で各姿勢を更新し、収束するまで反復する。

構造上、重要な点が2つある。まずゲージの自由度: コストは、すべての姿勢を一斉に剛体変換しても不変であるため、 $H$ はゲージを固定するまで特異である — 最初の姿勢を固定する(または事前分布因子を追加する)必要がある。次に情報行列 $\Sigma_{ij}^{-1}$ : これは補正がどのように分配されるかを決めるノブである。信頼できるオドメトリエッジはわずかにしか曲がらず、不確かなエッジがループ誤差の大半を吸収する。すべてのエッジの情報を単位行列に設定することも「動作する」が、ドリフトを非現実的に分配してしまう。

直感的な理解

大きな正方形を走行するロボットを想像してみよう。オドメトリは各コーナーで少しずつ方位誤差を蓄積し、推定された終端姿勢は開始点から明らかに離れた位置にある。場所認識が最終視点と最初の視点を照合し、「この2つの姿勢は一致する」と述べる1本のループエッジを追加する。PGOを実行する前は、そのエッジは巨大な残差を持ち、オドメトリエッジには残差がない。最適化器は誤差が共有される配置を見つける。すべての姿勢がわずかに回転・移動し(そのエッジの共分散によって重み付けされる)、各オドメトリエッジは小さな残差を持つようになり、ループエッジの残差は同じ統計的スケールまで縮小する — 軌跡は目に見えて閉じた正方形に「パチンと収まる」。何も再度計測されたわけではない。同じ計測値が単に一貫性を持って再説明されただけである。この誤差再分配の描像は、すべてのグラフベースSLAMバックエンドに対する正しいメンタルモデルである。

よくある落とし穴

ゲージの固定を忘れる — 特異な $H$ は、ソルバーの失敗や、反復間でマップ全体が自由に漂う現象として現れる。
未検証のループエッジを与える — エッジがグラフに入る前には常に幾何学的検証(インライア数、整合性チェック)を行うこと。1本の外れ値エッジが致命的になりうる(robust PGOを参照)。
大きなループでの初期化不良 — Gauss-Newton法は局所的に収束する。方位が90°ドリフトした軌跡には、ロバストカーネル、より良い初期推定値(例えば回転平均化/スパニングツリー初期化)、または悪い局所最小値を回避するための減衰付きLM法が必要になる場合がある。
2Dにおける角度の扱い — $\theta$ の残差は $[-\pi, \pi]$ にラップすること。ラップされていない角度は、2D姿勢グラフが「発散」する典型的な原因である。
$\mathrm{Sim}(3)$ が必要な場面で $SE(3)$ を使う — 単眼のループ閉じ込みはスケールドリフトも補正する必要がある。 $SE(3)$ 上で最適化すると、ループ部分にスケールの継ぎ目が残る。

SLAMにおける意義

PGOはループ閉じ込みの主力であり、ドリフトしたオドメトリ軌跡を大域的に整合したマップに変換するステップである。グラフベースSLAMの最も単純な例であるため、完全なバンドル調整やファクターグラフに取り組む前に、多様体上の非線形最小二乗、スパース性、情報行列を最初に理解するのに最適な場所でもある。

ハンズオン

Kimera-RPGO ハンズオン