Frame-to-model tracking

フレーム対モデルトラッキングとは、入力される各フレームを直前のフレームに対してではなく、蓄積されたマップに対して位置合わせすることを意味する。フレーム対フレームトラッキングでは、各ペアワイズの位置合わせが小さな誤差を持ち、数百の相対ポーズを合成することでドリフトが急速に蓄積する。対照的に、蓄積されたモデルは多数の観測を平均化する。その表面は、単一のフレームよりも滑らかでノイズが少なく、それに対して位置合わせすることはより正確かつ安定である。

モデル予測・位置合わせ・融合ループ

古典的なレシピはKinectFusionに由来する。

1. 密なシーンモデル(TSDFボリュームまたはサーフェルマップ)を維持する。
2. 直前のカメラ姿勢から、モデルがどのように見えるかを予測する。すなわちTSDFをレイキャストする(あるいはサーフェルをレンダリングする)ことで頂点/法線マップを合成する。
3. 新しい深度フレームをこの予測に対してICP(典型的には点対平面のバリアント)で位置合わせする。

$$E(\mathbf{T}) = \sum_i \big( (\mathbf{T}\,\mathbf{p}_i - \mathbf{q}_i) \cdot \mathbf{n}_i \big)^2$$

ここで$\mathbf{p}_i$は新しいフレームからの点であり、$\mathbf{q}_i, \mathbf{n}_i$は対応する予測されたモデルの点と法線である。対応関係は、高価な最近傍探索ではなく投影的なデータアソシエーション(モデルの点を新しいフレームへ投影する)を用い、最適化はGPU上で粗から密へ実行される。

4. 新たに位置合わせされたフレームをモデルに融合し、次のフレームの予測を改善する。

位置合わせが実際にどう解かれるか

点対平面ICPは、運動更新をツイスト$\boldsymbol{\xi} = (\boldsymbol{\omega}, \mathbf{t}) \in \mathfrak{se}(3)$としてパラメータ化し、それが小さいと仮定した瞬間に、微小な線形問題になる。現在の推定値$\hat{\mathbf{p}}_i = \hat{\mathbf{T}}\,\mathbf{p}_i$に対して、残差は次のように線形化される。

$$r_i(\boldsymbol{\xi}) \approx \mathbf{n}_i^\top\big( \hat{\mathbf{p}}_i + \boldsymbol{\omega} \times \hat{\mathbf{p}}_i + \mathbf{t} - \mathbf{q}_i \big),$$

したがって、有効な各ピクセルは、わずか6個の未知数を持つ最小二乗問題に1行を提供する。$6\times 6$の正規方程式$\mathbf{J}^\top\mathbf{J}\,\boldsymbol{\xi} = -\mathbf{J}^\top\mathbf{r}$は、GPU上で並列に(数十万ピクセルにわたる縮約として)累積され、CPU上でマイクロ秒単位で解かれる。ピラミッドの各レベルで数回の反復で十分である。これが、密なトラッキングがフレームレートで動作できる理由である。すなわちデータは密であるが、状態はただ1つのポーズに過ぎない。

ハイブリッドシステムは測光項を追加する。ElasticFusionは、以下の重み付き結合コストを最小化することで、サーフェルマップのレンダリングされた色+深度予測に対してトラッキングする。

$$E = \sum_i \Big[ w_{\mathrm{icp}} \big(\mathbf{n}_i^\top(\mathbf{T}\mathbf{v}_i - \mathbf{u}_i)\big)^2 + w_{\mathrm{rgb}} \big(I(\pi(\mathbf{T}\mathbf{v}_i)) - \hat{I}(\mathbf{u}_i)\big)^2 \Big],$$

これは、幾何のみでは退化してしまう場合でも(下記参照)トラッキングを制約された状態に保つ。DVO方式の直接法は、疎な特徴点ではなく全ピクセルにわたるロバストな密な残差という、補完的な要素を提供する。

よくある落とし穴

• モデルの破損はフィードバックする: 悪い位置合わせがモデルに融合されると、破損したモデルがその後のトラッキングを誤誘導する。誤差は平均化されるのではなく複合していく可能性がある。融合の重みと外れ値を考慮した統合が通常の防御策である。
• 収束範囲が狭い: ICPは良い初期推定を必要とする。速い回転や大きな変位はその範囲を超え、回復不能なトラッキング失敗を引き起こす可能性がある。粗から密へのピラミッドは範囲を広げるが、その限界を取り除くわけではない。
• 幾何的な退化: 単一の平らな壁に対する点対平面ICPは、3つの運動方向(面内の平行移動と法線周りの回転)を制約しないまま残す。$\mathbf{J}^\top\mathbf{J}$はランク落ちし、ポーズが滑ってしまう。測光項や特徴点を追加することで、欠けている制約が回復する。
• それでもオドメトリである: フレーム対モデルトラッキングはドリフトを減らすが排除しない。モデル自体が軌道とともにゆっくりとドリフトする。完全なシステムはその上にループ閉じ込みを追加する: ポーズグラフ、マップ変形(ElasticFusion)、TSDF再統合(BundleFusion)などである。

SLAMにおける意義

フレーム対モデルトラッキングは、密なRGB-D SLAMを特徴づける決定的な技巧である。これこそが、2011年にKinectFusionがバンドル調整を全く使わずにドリフトのないように見えるデスクスケールの再構成を生成できた理由である。それ以来のほぼすべての密なシステム、Kintinuous、ElasticFusion、InfiniTAM、BundleFusionは、何らかの形でこれを中心に構築されている。モデル予測・位置合わせ・融合ループを理解することが、それらの論文のいずれを読む際にも鍵となる。

関連ノート

• ICP — このループの中核にある位置合わせアルゴリズム
• KinectFusion — 標準的なTSDFフレーム対モデルシステム
• ElasticFusion — サーフェルベースのフレーム対モデルトラッキング
• TSDF vs Surfel maps — トラッキング対象となる2つのモデル表現
• DVO — ロバストな直接RGB-D位置合わせ