TEASER++

Yang 2020 · 論文

一行要約 — 打ち切り最小二乗、不変計測量の分離、最大クリークによる内点選択によって99%の外れ値対応にロバストな、初の高速かつ証明可能な3D点群レジストレーションアルゴリズム（T-RO/RSS 2020）。

問題

推定された対応関係$(\mathbf{a}_i, \mathbf{b}_i)$から2つの3D点群を位置合わせする問題は、生成モデル$\mathbf{b}_i = s^{\circ}\mathbf{R}^{\circ}\mathbf{a}_i + \mathbf{t}^{\circ} + \mathbf{o}_i + \boldsymbol{\epsilon}_i$に従う。ここで$\mathbf{o}_i$は内点ではゼロ、外れ値では任意の値を取る。外れ値がなければHornやArunの閉形式解で解けるが、1つの外れ値があるだけでこれらは破綻し、実際の記述子マッチングは大半が誤りである。ICPは良い初期推定を必要とし、RANSACは外れ値比率が上がると急激に性能が劣化し、それ以前の証明可能な解法はオンライン利用にはあまりに遅すぎた。TEASERは打ち切り最小二乗（TLS）formulationを採用し、外れ値モデルは仮定せず、内点ノイズの有界性$\|\boldsymbol{\epsilon}_i\| \le \beta_i$のみを仮定する。

$$\min_{s>0,\;\mathbf{R}\in SO(3),\;\mathbf{t}\in\mathbb{R}^3} \sum_{i=1}^{N} \min\!\left(\frac{1}{\beta_i^2}\big\lVert \mathbf{b}_i - s\mathbf{R}\mathbf{a}_i - \mathbf{t} \big\rVert^2,\; \bar{c}^2\right),$$

これにより、閾値を超える残差は定数の寄与しかせず、解を引きずることができない — しかしTLS最小化は凸領域上でもNP困難である。

手法とアーキテクチャ

• 不変計測量が問題をカスケードに分解する。 対応関係のペアを差し引くと並進が打ち消され、平行移動不変計測量（TIM、対応関係上のグラフの辺であり、ノイズ上界$\delta_{ij} = \beta_i + \beta_j$を持つ）$\bar{\mathbf{b}}_{ij} = s\mathbf{R}\bar{\mathbf{a}}_{ij} + \mathbf{o}_{ij} + \boldsymbol{\epsilon}_{ij}$が得られる。さらにTIMノルムの比を取ると回転も打ち消され、$s_{ij} = \lVert\bar{\mathbf{b}}_{ij}\rVert / \lVert\bar{\mathbf{a}}_{ij}\rVert$を満たすスカラーTRIM $s_{ij} = s + o^{s}_{ij} + \epsilon^{s}_{ij}$が得られる。TEASERはこれによりスケール、回転、並進を順にそれぞれTLSで解く。
• 正確な多項式時間のスケールと並進：適応的投票。 スカラーTLS（$\hat{s} = \arg\min_s \sum_k \min\big((s-s_k)^2/\alpha_k^2, \bar{c}^2\big)$）は多くとも$2K{-}1$個の異なる合意集合しか持たず、その境界は区間端点$s_k \pm \alpha_k\bar{c}$である — これらを列挙し、コスト最小の重み付き平均を取る（定理7）。並進も同じ方法で成分ごとに解かれる。
• 最大クリークによる内点の刈り込み（MCIS）。 $\hat{s}$と整合しないTRIM（つまり$|s_{ij} - \hat{s}| > \bar{c}\,\alpha_{ij}$を満たすもの）は刈り込まれる。定理6は、内点のTIMが残ったグラフのクリークを構成することを示し、最大クリークを計算することで、回転推定の前に相互整合した内点集合を分離できる。
• 緊密なSDPによる証明可能な回転推定。 TLS回転問題は単位クォータニオンと「バイナリクローニング」（$\mathbf{q}_k = \theta_k \mathbf{q}$、$\theta_k \in \{\pm 1\}$は内点・外れ値を表す）を用いてQCQP $\min_{\mathbf{x}} \mathbf{x}^{\mathsf{T}}\mathbf{Q}\mathbf{x}$に書き換えられ、その後$\mathbf{Z} = \mathbf{x}\mathbf{x}^{\mathsf{T}}$のランク1制約を外して緩和し、緩和を緊密にする冗長なブロック対称制約を追加する。定理13：SDP解のランクが1であれば、その因子は証明された大域最適解である。実験的には、緩和は外れ値率95%を超えても緊密性を保つ。
• TEASER++ = GNC＋高速な証明。 SDPの求解は遅い（$K{=}100$でMOSEKを用いると約1200秒）ため、TEASER++は回転部分問題をgraduated non-convexity（GNC、外れ値率が概ね80%未満であれば信頼できる — MCISでの刈り込み後であれば安全）で解き、その後ダグラス・ラシュフォード分割を双対問題に適用して推定値を証明する：アルゴリズム3は準最適性の限界$\eta$を返し、$(\hat{\mu} - \mu^{\star})/\hat{\mu} \le \eta$を満たす。これは双対証明書の最小固有値から計算される$\eta^{(t)} = |\lambda_1^{(t)}|(K+1)/\hat{\mu}$であり、推定が最適で緩和が緊密であればゼロに収束する。
• 推定の保証。 定理15〜17は推定誤差を上界で抑える — ロバストレジストレーションに対して初めてのこの種の限界である。ノイズのない内点とランダムな外れ値の場合、外れ値の数にかかわらず、厳密な復元にはわずか3個の内点しか必要としない。敵対的な外れ値の場合は内点が多数派である必要がある。

実験結果

• Bunnyベンチマーク（N=100、FGR、GORE、RANSACとの比較）: TEASER、TEASER++、GORE、60秒RANSACはいずれも90%の外れ値にロバストである（FGRは70%で破綻、RANSAC-1Kは90%で破綻）。95〜99%という極端な外れ値率（$N{=}1000$）では、TEASER/TEASER++/GOREは99%まで持ちこたえ、TEASER++はより正確で、GOREより1桁高速である。TEASER++はラップトップ上で10ms未満（スケール未知の場合は30ms未満）で高外れ値問題を解く。
• 証明: DRS証明器は正しいGNC解をすべて有効と判定し、誤ったGNC解をすべて棄却する。平均24回の反復（C++で反復あたり約50ms）を要し、MOSEKより桁違いに高速に問題を証明する。MOSEKは150 TIMを超えるとメモリ不足になる。
• 対応関係フリーのレジストレーション（全対全の仮説、約$10^4$個の候補ペア）: ICPはほぼ常に失敗し、Go-ICPはトリミングに敏感で平均16秒かかる；TEASER++はいかなる初期推定もなしに、約10%の重複率まで正しい姿勢を復元する。
• 物体姿勢推定（RGB-Dデータセット、FPFH対応関係、内点率は典型的に5%未満）: 8シーンにわたる平均回転誤差0.066 rad、平均並進誤差0.069 m。
• 3DMatchスキャンマッチング（3DSmoothNet記述子）: TEASER++はMIT Labを除くすべてのシーンでRANSAC-10Kと同等以上の成績を示す（例：Kitchenで98.6% 対 97.2%の成功率）、平均実行時間0.059秒；証明された推定のみに限定する（TEASER++ CERT）とさらに成功率が上がる（Kitchenで99.4%） — SLAMにおける不良なループクロージングに対する自然なフィルタとなる。

SLAMにおける意義

推定された対応関係を用いたレジストレーションはSLAMのあらゆる場所に現れる：LiDARループクロージング、グローバル再位置推定、複数ロボットによる地図統合、物体姿勢推定 — いずれも特徴マッチングがほとんど間違っている状況である。TEASER++は「対応関係の90%以上がゴミである」という状況を、最適性証明付きで解ける設定にした。証明済み・未証明の区別は、ループクロージングパイプラインに理にかなった棄却テストを与える。オープンソースのC++ライブラリはLiDAR SLAMや複数ロボットシステム（例：Kimera-Multi風の地図統合）に広く組み込まれており、SE-SyncやQUASARと並んで証明可能な知覚系の系譜の柱の一つである。

関連ノート

• SE-Sync — 証明可能なポーズグラフ最適化
• QUASAR — TEASER++が証明に用いる、証明可能なクォータニオンベースの回転部分求解器
• GNC — TEASER++が証明する高速なロバストヒューリスティック
• ICP — 外れ値に対してロバストにされた古典的な局所レジストレーション手法
• Kimera-Multi — 地図統合にロバストなレジストレーションを用いる複数ロボットシステム
• Inter-robot loop closure — 証明済みレジストレーションの重要な適用場面