Robust pose-graph optimization

標準的な姿勢グラフ最適化には危険な特性がある。すべてのエッジを信頼してしまうことである。オドメトリエッジは通常信頼できるが、ループ閉じ込みエッジは場所認識から得られるものであり、それは時に嘘をつく — 知覚的アリアシング(見た目が同じ2つの通路)が偽のループ閉じ込みを生じさせる。1本の誤ったループエッジを最小二乗法に投入すると、二次コストが1つの悪い制約に全体を支配させてしまうため、最適化器はマップを自身に折り畳んでしまう。Robust pose-graph optimizationは、バックエンドが外れ値制約を乗り越えて生存できるようにする手法群である。

ロードマップで名前が挙げられている3つの古典的アプローチは以下の通り。

• Switchable Constraints(Sunderhauf & Protzel, 2012)。各ループ閉じ込みエッジ $(i,j)$ に、そのエッジの情報行列をスケールするスイッチ $s_{ij} \in [0, 1]$ という追加の最適化変数が与えられる。最適化器は、他のグラフと矛盾するエッジを「オフにする」ことができるが、エッジを不必要に無効化することを抑制するペナルティ項を支払う。

  $$\min_{\{T_i\}, \{s_{ij}\}} \sum_{\text{odom}} \|\mathbf{e}_{ij}\|^2_{\Sigma^{-1}} + \sum_{\text{loops}} s_{ij}^2\, \|\mathbf{e}_{ij}\|^2_{\Sigma^{-1}} + \lambda (1 - s_{ij})^2$$

• Dynamic Covariance Scaling (DCS)(Agarwal et al., 2013)。最適なスイッチ値は、現在の残差から閉形式で計算できることを観察し、追加変数を排除する。各ループエッジの共分散はその場でスケールされる — 残差が小さければ完全に信頼し、残差が大きければ共分散を膨張させる。DCSはロバストなM推定量カーネルのように振る舞い、switchable constraintsよりも高速に収束する。

• Pairwise Consistency Maximization (PCM)(Mangelson et al., 2018)。最適化器の内部でエッジを軟化させるのではなく、PCMは最適化の前にループ閉じ込みをフィルタリングする。2つのループ閉じ込みは、それらが示唆する相対運動(その間のオドメトリと合成した上で)がそれぞれの共分散の範囲内で一致する場合に、pairwise consistent(対整合)であるとされる。整合性グラフを構築し、その最大クリークを見つけることで、相互に整合した最大のループ閉じ込み集合が得られ、残りは棄却される。PCMは、ロボット間のループ閉じ込みが特に信頼できないマルチロボットSLAM(DOOR-SLAM、Kimera-Multi)における標準的なゲートキーパーとなった。

これらのアイデアは、より広いロバスト推定のツールボックスと結びついている。M推定量カーネル(Huber、Cauchy)は中程度の外れ値の重みを下げ、Graduated Non-Convexity (GNC)は初期化を必要とせずに強く非凸なロバストコストを解く。実際のシステムでは防御を複数層に重ねる。フロントエンドでの幾何学的検証、PCM式の整合性チェック、そして最後の防衛線として最適化器内のロバストカーネルまたはDCSである。

M推定量の視点

すべての「軟らかい」手法は、1つの操作の変種である。二次コストをロバストカーネル $\rho$ に置き換えることである。

$$\min_{\{T_i\}} \sum_{(i,j)} \rho\left(\|\mathbf{e}_{ij}\|_{\Sigma_{ij}^{-1}}\right),$$

これは、大きな残差に対してコストの影響力(その導関数)が成長を止める — あるいは縮小する — ように選ばれる。二次関数の下では、残差 $10\sigma$ のエッジは $1\sigma$ のエッジよりも100倍強く引く。Huberの下では線形にしか強く引かない。CauchyやGeman-McClureの下では、残差が大きくなるにつれてその引く力は薄れていく。実装の観点では、これは**反復再重み付け最小二乗法(iteratively reweighted least squares)**になる。各Gauss-Newton反復で、現在の残差から計算される重み $w(\|\mathbf{e}\|)$ をすべてのエッジの情報行列に乗じる — これはまさにDCSが特定の閉形式の重みで行うことであり、switchable constraintsが明示的なスイッチ変数で達成することでもある。トレードオフとして、redescendingカーネル(Cauchyおよびそれより強いもの)は大きな外れ値を完全に排除できるが、コストを非凸にしてしまうため、初期化が悪いと良いエッジまでロックアウトされてしまう可能性がある — これはGNCが凸なサロゲートから始めて徐々に鋭くしていくことで対処する問題である。

フロントエンド側の補完的なハードチェックはカイ二乗ゲーティングである。エッジが受理される前に、現在の推定値の下でその残差が統計的にあり得るかどうかを検定する。$\mathbf{e}^T \Sigma^{-1} \mathbf{e} < \chi^2_{d,\,0.95}$($SE(3)$エッジでは6自由度)。ゲーティングは明白な外れ値を安価に捕捉できるが、これは現在の推定値を信頼している — ドリフトが大きい場合、正しいループ閉じ込みもあり得ないように見えてしまう。これはまさにPCMの対整合性ロジックがより良く処理するケースである。

よくある落とし穴

• 間違ったエッジをロバスト化する — オドメトリエッジが外れ値であることは稀である。これを積極的なカーネルでラップすると、最適化器が本物のドリフトを「言い訳」できるようになり、軌跡を分断された島に分裂させてしまう可能性がある。ループ閉じ込みをロバスト化し、オドメトリは(ほぼ)二次のままにすること。
• ロバスト性が検証の代わりになると思い込む — カーネルとスイッチは外れ値の一部しか生き延びられない。フロントエンドが40%の偽ループを出力すれば、それでもマップは破壊される。防御は複数層に重ねること。
• 調整の盲目性 — switchable constraintsの $\lambda$、DCSの $\Phi$ パラメータ、カーネルの幅は、すべて「エッジを疑う前に残差がどれだけ大きくなれるか」を符号化している。これらは現実的なエッジ共分散に対して相対的に設定する必要があるが、その共分散自体もしばしば推測されたものである。
• redescendingカーネル+悪い初期化 — 大きなドリフトがある場合、正しいループ閉じ込みは巨大な残差から始まり、すぐにオフにされてしまう。GNC式の継続手法やPCMによる事前フィルタリングが、この失敗を回避する。
• 知覚的アリアシングは構造化された環境において整合的な嘘を生み出す — 見た目が同一の通路区間からの複数の偽マッチが互いに一致してしまうことがある。整合性最大化は基準を上げるが、証明にはならない。

SLAMにおける意義

1本の偽ループ閉じ込みが、1時間かけて構築したマップを破壊しうるため、デプロイされたシステムにおいてバックエンドでのロバスト性は必須である。このトピックはまた、マルチロボットSLAMへの入り口でもある。そこではロボットたちは、他のロボットが生成した制約を信頼すべきかどうかを判断しなければならず、これはPCMが設計された状況そのものである。

ハンズオン

• Kimera-RPGO ハンズオン

関連ノート

• Pose graph optimization
• GNC
• DOOR-SLAM
• Inter-robot loop closure
• Visual Place Recognition (VPR)