SE-Sync

Rosen 2019 · 論文

一行要約 — ポーズグラフ最適化に対する初の証明可能に正しいアルゴリズム。SDP緩和をリーマン最適化で解き、大域的最適解を最適性の証明付きで回復する（arXiv 2016、IJRR 2019）。

問題

$SE(d)$同期——$n$個の未知の姿勢$x_1,\dots,x_n \in SE(d)$を、そのうち$m$個の対のノイズを含む相対変換の観測$x_{ij} = x_i^{-1}x_j$から推定する——は、標準的なSLAMバックエンド問題（ポーズグラフSLAM、カメラ姿勢推定）である。論文の生成モデル（精度$\tau_{ij}$を持つガウス並進ノイズ、集中度$\kappa_{ij}$を持つ等方的ランジュヴァン回転ノイズ）の下で、最大似度推定は次を最小化する。

$$p^{*}_{\mathrm{MLE}} = \min_{t_i \in \mathbb{R}^d,\; R_i \in SO(d)} \sum_{(i,j)\in\vec{\mathcal{E}}} \kappa_{ij}\big\lVert R_j - R_i\tilde{R}_{ij}\big\rVert_F^2 + \tau_{ij}\big\lVert t_j - t_i - R_i\tilde{t}_{ij}\big\rVert_2^2 .$$

これは高次元の非凸非線形プログラムであり、一般には計算的に困難である: 局所ソルバー（g2o、GTSAM、Ceres）は真の解から遠い局所最小値に暗黙的に収束することがあり、返された答えが大域的に最適であるかどうかを知る方法がない——安全性が重要な自律システムにとっては受け入れがたい。

手法とアーキテクチャ

• 並進を消去する。 回転を固定すると、問題は$t$に関する制約なし二次形式となり、一般化シュア補行列によって閉形式で解かれる。これによりMLEは純粋な回転同期問題に帰着する: $p^{*}_{\mathrm{MLE}} = \min_{R \in SO(d)^n} \operatorname{tr}(\tilde{Q} R^{\mathsf{T}} R)$。ここでデータ行列$\tilde{Q} = L(\tilde{G}^{\rho}) + \tilde{T}^{\mathsf{T}} \Omega^{1/2} \Pi\, \Omega^{1/2} \tilde{T}$は回転結合ラプラシアン$L(\tilde{G}^{\rho})$と並進データ項を組み合わせる; $\Pi$（直交射影）は、重み付き接続行列の薄いLQ分解を通じて疎な分解を持つため、$\tilde{Q}$との積は稠密な行列を形成することがない。最適な並進は事後的に$t^{*} = -\operatorname{vec}\big(R^{*}\tilde{V}^{\mathsf{T}} L(W^{\tau})^{\dagger}\big)$として回復される。
• 証明可能にタイトなSDP緩和。 $SO(d)$を$O(d)$に緩和すると、問題はQCQPとなり、そのラグランジュ双対は次の半正定値計画になる。

$$p^{*}_{\mathrm{SDP}} = \min_{Z \succeq 0}\; \operatorname{tr}(\tilde{Q} Z) \quad \text{s.t.} \quad \mathrm{BlockDiag}_{d\times d}(Z) = \mathrm{Diag}(I_d,\dots,I_d),$$

したがって$p^{*}_{\mathrm{SDP}} \le p^{*}_{\mathrm{MLE}}$。命題1: $\beta > 0$が存在し、$\lVert \tilde{Q} - \bar{Q} \rVert_2 < \beta$（$\bar{Q}$は真の潜在的変換のデータ行列——つまりノイズが臨界閾値未満）ならば、SDPは$R^{*} \in SO(d)^n$が正確なMLEである一意の解$Z^{*} = R^{*\mathsf{T}}R^{*}$を持つ。丸め処理された推定値がSDPの下界に達するときは常に、その等式は大域的最適性の計算的証明となる。

• リーマン階段法。 内点法によるSDPソルバー（数千個を超える変数では扱いにくい）の代わりに、SE-Syncはバーラー・モンテイロ分解$Z = Y^{\mathsf{T}}Y$（$Y \in \mathbb{R}^{r\times dn}$、$r \ll dn$）を用いる; ブロック制約は各$Y_i$が正規直交フレームであることを意味し、シュティーフェル多様体の積上の制約なし問題を与える。

$$p^{*}_{\mathrm{SDPLR}} = \min_{Y \in \mathrm{St}(d,r)^n} \operatorname{tr}(\tilde{Q}\, Y^{\mathsf{T}} Y).$$

命題2（Boumalらに基づく）: この問題のランク欠損な二次臨界点はすべて大域的最小化子であり、SDP解を与える——そのため、ランク欠損な臨界点が出現するまでランク階層（「リーマン階段」）を上る。

• 高速な2次局所探索。 多様体上で、$\nabla F(Y) = 2Y\tilde{Q}$であり、ヘッセ行列ベクトル積はアンビエント導関数の射影である（$\operatorname{grad} F(Y) = \operatorname{Proj}_Y \nabla F(Y)$）。すべて疎行列積と三角ソルバーで計算される; 打ち切り型ニュートン・リーマン信頼領域（RTR）法が高精度の臨界点を見つける。
• 丸め処理。 $Y^{*}$のランク$d$薄SVDによって$\hat{R} = \Xi_d V_d^{\mathsf{T}}$が得られる; ほとんどのブロックが負の行列式を持つ場合は向きを反転し、その後各ブロックを最も近い回転行列に射影する——緩和がタイトな場合は正確であり、そうでない場合は実行可能な近似となる。

実験結果

（Manopt上のMATLAB実装、階段は$r=5$に固定され、$\mathrm{St}(3,5)^n$上のランダムな点から初期化。ベースライン: オドメトリ初期化のガウス・ニュートン法、コーダル初期化によるGN、コーダル初期化GNに事後検証を加えたもの。）

• シミュレートされたキューブワールド（$s^3$格子、ループクロージャ確率$p_{LC}$、ノイズ$\sigma_T$、$\sigma_R$; 各設定30回の実行）: SE-Syncはランダムな初期化から証明可能に大域的最適な解に収束し、その時間は最先端のコーダル初期化を用いたGNに匹敵する——これらのテストでは多くの場合それよりも高速であり、GN+別途検証よりもはるかに高速である。例外は高回転ノイズ領域で、そこでは緩和のタイト性が崩れる。
• 大規模な実データ/標準3D SLAMデータセット——sphere（2500ノード/4949エッジ）、sphere-a（2200/8647）、torus（5000/9048）、cube（8000/22236）、garage（1661/6275）、cubicle（5750/16869）: SE-Syncはこれらすべてで証明された大域的最適解に到達する（例えばsphere-aで目的値$1.249\times 10^{6}$、オドメトリ初期化GNの$3.041\times 10^{6}$に対して）、3.6〜203秒で、緩和が困難な実世界のインスタンスでもタイトであり続けることを確認する。
• 論文の要旨によれば、大域的最適性は「ロボティクスの応用で典型的に遭遇するものより一桁大きい」ノイズの下でも回復され、そのコストは直接的なニュートン型局所探索と同程度にスケールする。

SLAMにおける意義

SE-Syncは基礎的な問いに答えた: PGOの非凸性にもかかわらず、実際のSLAMで生じるインスタンスは大域的に解くことができ、しかも大域的最適解を得たことを知ることができる。これは証明可能な知覚に関する研究プログラム（点群レジストレーションのためのTEASER++、回転探索のためのQUASAR）を立ち上げ、SLAMバックエンドに検証ツールを与えた——例えば、破損した可能性のあるループクロージャの後に局所ソルバーの答えが実際に最適であるかどうかを確認するために使われる。

関連ノート

• Pose graph optimization — 証明の対象となる問題
• MAP inference as sparse nonlinear least squares — 緩和されるMLEの定式化
• TEASER++ — 証明可能な点群レジストレーション
• QUASAR — 証明可能な回転探索
• GNC — 外れ値の多いグラフに対するロバスト推定の相棒