PnP入門 — 3次元点と画像だけでカメラ姿勢を求める

カメラで撮影した画像に、地図上の3次元点がいくつか対応しているとする。カメラがどこに置かれ、どちらを向いているかを求める問題がPnP（Perspective-n-Point）である。Visual SLAMの地図追跡、ARで仮想物体を重ねる処理、ロボットのハンドアイ校正、測量カメラの位置推定で共通して使われる。

0. 30秒要約

入力はカメラ内部行列 $K$ 、既知の3D点 $\mathbf X_i$ 、対応する画像点 $\mathbf u_i$ 。出力は回転 $R$ と並進 $t$ である。
投影式 $\mathbf u_i\sim K(R\mathbf X_i+t)$ の再投影誤差を最小化する。点が3個ならP3Pの候補解、4個以上なら冗長性で外れ値を検出できる。
EPnPは点を4個の仮想制御点の線形結合で表し、多数点を高速に解く。LM法などの非線形最適化で最後に精密化する。
対応に外れ値が混ざると姿勢全体が崩れるため、RANSAC-PnP、深度正値、前後フレーム整合性で検証する。
点がほぼ同一平面、視差が小さい、内部パラメータが誤る、ローリングシャッターや動的物体がある場合は退化・発散しやすい。

1. 投影モデル

カメラ座標系の点を $\mathbf X_c=R\mathbf X_w+t$ とする。ピンホールモデルでは正規化画像座標は

x=\frac{X_c}{Z_c},\qquad y=\frac{Y_c}{Z_c}

で、画素座標は内部行列

K=\begin{bmatrix}f_x&0&c_x\\0&f_y&c_y\\0&0&1\end{bmatrix}

により $\mathbf u\sim K\mathbf X_c$ へ変換する。 $R\in SO(3)$ は回転、 $t$ は並進。レンズ歪みがあると投影前後の歪み補正が必要である。

未知数は回転3自由度と並進3自由度の6つ。3D点 $\mathbf X_i$ と観測 $\mathbf u_i$ の対応が $n$ 個あれば、再投影誤差

E(R,t)=\sum_{i=1}^{n}\rho\left(\left\|\mathbf u_i-\pi(K(R\mathbf X_i+t))\right\|^2\right)

を最小化する。 $\pi$ は透視除算、 $\rho$ はHuberなどのロバスト損失である。

2. P3P・AP3P・EPnP

3点の画像角度と3D点間距離からカメラ中心までの距離を求めるP3P（Perspective-3-Point）は、最大4つの解を持つ。4点目や地図の姿勢と照合して正しい解を選ぶ。AP3Pは代数的に解を整理した高速な変種である。

点数が多い場合、EPnP（Efficient PnP）はすべての3D点を4個の仮想制御点の重み付き和で表す。

\mathbf X_i=\sum_{j=1}^{4}\alpha_{ij}\mathbf C_j,\qquad \sum_j\alpha_{ij}=1

制御点のカメラ座標を線形方程式で求め、回転・並進を復元する。計算量が点数に対してほぼ線形になるため、SLAMの多数ランドマークから初期姿勢を作るのに向く。初期解の後は、再投影誤差をLevenberg–Marquardtで反復最適化する。

3. RANSAC-PnP

特徴点対応には、似た模様、動く物体、誤った地図IDが混ざる。最小点集合で仮の姿勢を作り、全対応を再投影して閾値以内のインライア数を数えるRANSACが定石である。反復回数 $N$ は、外れ値率 $\epsilon$ 、最小サンプル数 $s$ 、成功確率 $p$ に対し

N\ge\frac{\log(1-p)}{\log(1-(1-\epsilon)^s)}

で決まる。外れ値率が高いほど必要回数が急増するので、特徴点の比率テスト、グリッド分散、動的物体マスクで事前に $\epsilon$ を下げる。OpenCVのsolvePnPRansacは、点数、フラグ（EPNP、P3P、SQPNP等）、再投影閾値、信頼度を明示して使う。

4. 退化を見抜く

平面点群

すべての3D点が同一平面にあると、PnPの奥行きと姿勢が曖昧になり、ホモグラフィでも説明できる。チェッカーボード校正では平面を意図的に使うが、姿勢の自由度を十分に拘束する視点と点配置を選ぶ必要がある。ARマーカー1枚の正面視で奥行きが不安定になるのも同じ理由である。

小さな視差・遠距離

カメラがほとんど動かず、点が画像上で数画素しか変わらない場合、画素ノイズが奥行き・並進へ大きく増幅される。カメラ移動、解像度、焦点距離、点の奥行き分布を考え、姿勢だけを更新するか、IMUや深度センサを併用する。

キャリブレーションと時刻

焦点距離・主点・歪みの誤差は、すべての点の系統的な再投影誤差になる。ズーム、温度、フォーカスで内部行列が変わるレンズは再校正が必要だ。車載・ドローンではローリングシャッターの行時刻とIMUを同期しないと、PnPが「曲がったカメラ姿勢」を返す。

5. Visual SLAMでの役割

Visual SLAMでは、過去に三角測量した地図点が増えると、フレームごとにPnPでカメラ姿勢を追跡できる。姿勢を固定して新しい点を三角測量し、キーフレームが増えたらBundle Adjustmentで姿勢と地図をまとめて最適化する。PnPは軽量なフロントエンド、Bundle Adjustmentは大域的な整合を担う、と分担すると理解しやすい。

6. 実装チェックリスト

チェッカーボード等で $K$ と歪みを校正し、再投影誤差を記録する。
3D点の単位（m/mm）と座標系、画像点の歪み補正状態をそろえる。
対応点をratio test・相互最近傍・時系列追跡で絞る。
RANSAC-PnPで外れ値を除き、インライア分布と再投影誤差を保存する。
深度が正、姿勢変化が物理的、前フレームからの差が妥当かを検査する。
条件が悪ければIMU、深度、ホモグラフィ、再初期化へ切り替える。

7. まとめ

PnPは、3D地図と2D画像の対応を、カメラ姿勢という6自由度へ変換する橋である。P3P/EPnPで初期解を作り、RANSACで外れ値を除き、非線形最適化で精密化する。点の配置、キャリブレーション、視差、時間同期を同時に管理して初めて、Visual SLAMやARの安定した姿勢推定になる。

参考資料

#PnP #Perspective-n-Point #カメラ姿勢 #3D復元 #RANSAC #OpenCV