2D-3D correspondence

既知のマップにある $n$ 個の3D点 $\{\mathbf{X}_i\}$ と、カメラ画像上のそれらの2D投影 $\{\mathbf{u}_i\}$ が与えられているとき、Perspective-n-Point (PnP) 問題はカメラ姿勢 $[R|\mathbf{t}]$ を推定する。これは、マップが既に存在する場合にSLAMシステムが新しいフレームごとに自己位置を推定する標準的な方法である。

P3P

Perspective-3-Point問題は、まさに3組の対応点を使う ―― 姿勢の6自由度に対応する最小ケースである(各2D点は2つの制約を与える)。この幾何は古典的な「余弦定理」系に帰着する: 視線方向間の3つの角度は画像から既知であり、3点間の距離はマップから既知であるため、最大4つの実数解を持つ多項式系が得られる。4つ目の対応点によって曖昧性が解消される。必要な点数が非常に少ないため、P3PはRANSAC内部で選ばれる最小解法である ―― 必要な反復回数はサンプルサイズに対して指数的に増加するため、 $s = 3$ はより大きな解法に比べて大きな優位性を持つ。

EPnP

EPnP(Lepetit et al., 2009)は $n$ 個の3D点を4つの仮想制御点の重み付き和として表現し、PnPを( $n$ に関わらず)12個の未知数(カメラ座標系における制御点の座標)を推定する問題に帰着させる。その $O(n)$ の計算量は大きな対応集合に対して効率的であり、多くのパイプラインでデフォルトとなっている(cv::solvePnP は直接サポートしている)。

DLTとSVDの役割

直接線形変換(Direct Linear Transform)は、 $n \geq 6$ 組の対応から $3 \times 4$ の射影行列 $P$ 全体を、同次線形系としてSVDを用いて解く: 解は最小特異値を持つ右特異ベクトルである。姿勢はその後 $P = \mathbf{K}[R|\mathbf{t}]$ の分解(内部パラメータも未知の場合はRQ分解)によって抽出される。DLTはEPnPより単純だが精度は劣る。実際には、どちらも再投影誤差を最小化することで洗練される初期化として役立つ。

非線形refinement(motion-only bundle adjustment)

どの解法が初期姿勢を与えたとしても、精度の高い答えはマップ点を固定したまま姿勢のみに関する再投影誤差を最小化することから得られる:

$T^* = \arg\min_{T \in SE(3)} \sum_{i} \rho\!\left(\big\|\mathbf{u}_i - \pi(T\,\mathbf{X}_i)\big\|^2\right)$

ここで $\pi$ はカメラ投影であり、 $\rho$ は残存する外れ値の影響を制限するロバストカーネル(例: Huber)である。これはGauss-NewtonまたはLevenberg-Marquardtによって数回の反復で解かれる ―― これは特徴点ベースSLAMにおいて毎フレームで実行される「motion-only BA」ステップである。

典型的なパイプライン

現在フレームのキーポイントをマップ点とマッチングする(記述子マッチング、あるいは投影ガイド付き探索)。
P3Pを用いたRANSACで外れ値のマッチを除去し、初期姿勢を得る。
すべての内点に対して非線形最小二乗法で姿勢を洗練する(再投影誤差にロバストカーネルを適用)。

よくある落とし穴

退化した点配置: 3D点が同一平面上にある(あるいはさらに悪く、同一直線上にある)場合、DLTは失敗する。EPnPとP3Pは平面をより良く扱うが、使用する解法の前提を確認すること。
点数が少ない場合の曖昧な解: P3Pの最大4つの解は曖昧性を解消する必要がある ―― 間違った根をrefinementに与えると、誤っているが低い残差を持つ姿勢に収束してしまう。
マップ点の質が姿勢の質の上限を決める: PnPは3D点を完璧なものとして扱う。三角測量の質が悪いランドマーク(視差が小さい)は姿勢を系統的に偏らせる。これが、点も同時に動かすフルバンドル調整が最終的にフレームごとのPnPを上回る理由である。
未校正の歪み: 歪みを除去せずに歪んだピクセル座標に対してPnPを実行すると、一見一貫しているが偏った姿勢が得られる。

SLAMにおける意義

PnPは特徴点ベースSLAMのトラッキングの中核である: ORB-SLAMはローカルマップ点とのマッチングとPnPの求解によって毎フレームをトラッキングし、トラッキング失敗後の再局在化(relocalization)は場所認識の候補に対するPnPである。これはまた、視覚的自己位置推定サービス(クエリ画像と既存のマップ)がカメラ姿勢を計算する方法でもある。2D-2D(初期化)と3D-3D(点群の位置合わせ)とともに、SLAMエンジニアが知っておくべき対応関係問題の三本柱を完成させる。

ハンズオン

Perspective-n-pointsハンズオン