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点だけで外れ値検出を保証できるわけではなく、多数の対応とRANSACによる検証を使う。
• EPnPは点を4個の仮想制御点の線形結合で表し、多数点を高速に解く。LM法などの非線形最適化で最後に精密化する。
• 対応に外れ値が混ざると姿勢全体が崩れるため、RANSAC-PnP、深度正値、前後フレーム整合性で検証する。
• 対応点が画像の狭い範囲に集中する場合や、内部パラメータの誤り、ローリングシャッター、動的物体がある場合は推定が不安定になる。平面にはIPPEなどの専用解法もある。

1. 投影モデル

図1 · ボタンで表示を切り替え

図1 — 同じIDを持つ3D–2D対応から候補姿勢を作り、再投影残差で外れ値を除去してR,tを精密化する。ここでR,tは世界座標からカメラ座標への変換である。

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

で、画素座標は内部行列

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

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

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

座標系と返り値を取り違えない

OpenCVのsolvePnPが返すrvec, tvecは、物体・世界座標の点をカメラ座標へ移す変換である。カメラ中心を世界座標で使うなら\mathbf C_w=-R^Tt、カメラ姿勢ならT_{wc}=T_{cw}^{-1}へ反転する。tvecをそのまま「世界座標でのカメラ位置」と解釈するのは典型的な誤りである。また、入力画像点を事前に歪み補正した場合は、同じ点へ歪み係数を二重適用してはいけない。

2. P3P・AP3P・EPnP

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

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

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

3. RANSAC-PnP

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

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

4. 退化を見抜く

平面点群

平面点でも姿勢推定は可能であり、OpenCVには平面用のIPPEがある。ただし正面に近い視点や遠距離では、ノイズ下で複数の候補姿勢を区別しにくい。平面上の対応はホモグラフィでも説明できる。チェッカーボード校正では平面を意図的に使うが、姿勢の自由度を十分に拘束する視点と点配置を選ぶ必要がある。ARマーカー1枚の正面視で奥行きが不安定になるのも同じ理由である。

画像上で狭い点配置・遠距離

PnPの直接入力は1枚の画像と既知3D点であり、フレーム間視差そのものは必須ではない。ただし、対応点が画像の狭い範囲に集中する、物体が遠く小さく写る、3D点の奥行き分布が乏しい、といった配置では画素ノイズが並進・奥行きへ強く増幅される。インライア数だけで成功とせず、画像全体への分散、再投影RMSE、姿勢共分散または摂動感度を確認し、必要ならIMUや深度センサを併用する。

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

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

5. Visual SLAMでの役割

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

6. 実装チェックリスト

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

最小実装の順序

OpenCVでは、まずsolvePnPRansacでrvec, tvec, inliersを得て、インライアだけをsolvePnPRefineLMへ渡す。最後にprojectPointsで全対応を再投影し、インライアRMSEと誤差分布を自分で計算する。APIの成功フラグだけでは、誤対応の多さ、点の偏り、物理的に不可能な姿勢を検出できない。

ok, rvec, tvec, inliers = cv2.solvePnPRansac(
    object_points, image_points, K, dist,
    flags=cv2.SOLVEPNP_EPNP,
    reprojectionError=3.0, confidence=0.999, iterationsCount=200,
)
if not ok or inliers is None or len(inliers) < 6:
    raise RuntimeError("PnP failed or has too few inliers")

idx = inliers.ravel()
rvec, tvec = cv2.solvePnPRefineLM(
    object_points[idx], image_points[idx], K, dist, rvec, tvec
)
projected, _ = cv2.projectPoints(object_points[idx], rvec, tvec, K, dist)
rmse = np.sqrt(np.mean(np.sum(
    (projected.reshape(-1, 2) - image_points[idx].reshape(-1, 2)) ** 2,
    axis=1,
)))
R, _ = cv2.Rodrigues(rvec)
camera_center_world = -R.T @ tvec.reshape(3, 1)

3.0 pxやインライア6点は普遍的な合格値ではなく、この例の初期設定にすぎない。画像解像度、特徴点精度、用途の許容姿勢誤差から閾値を決め、インライアが画像の一角へ偏っていないことと、全点のカメラ座標Z_c>0も検査する。

再現できる合成データ：外れ値と焦点距離の誤りを分ける

以下はDuskcoilで実行した合成データの計算例で、実機の精度測定ではありません。48個の非平面3D点（単位m）を1280×720画素の画像へ投影し、各画素成分に標準偏差0.5 pxの独立ガウスノイズを加えます。外れ値のケースでは同じ点のうち12点を画像内のランダムな座標へ置換します。乱数seedは20261001です。

真値は f_x=f_y=800 px、主点 (640,360)、歪みなし。世界→カメラのRodrigues回転ベクトルは (0.15,-0.10,0.05) rad、並進は (0.20,-0.10,5.0) mです。焦点距離誤りのケースだけ推定に使う f_x,f_y を920 pxへ変更し、観測点は変えません。EPnPで初期化し、RANSAC使用時は3 px・200反復・信頼度0.999、選ばれた点をLMで精密化します。RANSACなしのケースは全48点を精密化に使います。

RMSEは精密化に使った対応点に対する再投影誤差です。カメラ中心誤差は \|\hat C_w-C_w\| で、既知の真値と比較しています。各行で使用点集合が異なるため、RMSEだけを横並びで順位付けしないでください。

外れ値があるケースでは、RANSACなしでもAPIは成功を返し、推定した深度もすべて正でしたが、カメラ中心は約1.95 mずれました。RANSACを使うと、この入力では36点が残り、中心誤差は約0.0077 mでした。一方、焦点距離を15%誤らせたケースでは48点が残り、RMSEが約1.24 pxでも中心誤差は約0.75 mあります。低い再投影誤差や正の深度だけでは、内部パラメータの正しさを保証できません。

これは固定seed・1配置の例です。点配置、距離、ノイズ、外れ値率を変えれば結果も変わります。単眼の実画像では通常、真のカメラ中心は未知なので、この誤差を直接計算できません。実データでは独立した校正・基準位置や複数条件で検証してください。

自分で再実行する

実行コード reproduce.py · 入力点CSV · 全結果JSON

記録した環境はPython 3.12.3 / OpenCV 4.6.0 / NumPy 1.26.4です。reproduce.pyを保存し、NumPyとOpenCVを導入した環境で次を実行すると、入力点と結果を生成します。入力CSVは生成結果の確認用で、スクリプトは固定seedから入力を作ります。OpenCVの版・ビルドや乱数実装により末尾桁・インライア集合が変わる場合があるため、出力JSONの環境情報も保存してください。

python -m venv .venv
# Windows: .venv\Scripts\activate
# macOS / Linux: source .venv/bin/activate
python -m pip install numpy==1.26.4 opencv-python-headless==4.6.0.66
python reproduce.py --output results.json --inputs inputs.csv

7. まとめ

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

参考資料

• OpenCV — Camera calibration and 3D reconstruction
• Lepetit et al., EPnP
• Gao et al., Complete solution classification for the perspective-three-point problem

• Hartley and Zisserman, Multiple View Geometry

• OpenCV — PnP methods and pose refinement