姿勢計算の概要

姿勢計算問題 [188] は、3D-2D点対応からの再投影誤差を最小化する回転と並進を求めることからなる。

solvePnP および関連関数は、一連のオブジェクト点とそれらに対応する画像投影、ならびにカメラの内部行列と歪み係数が与えられたときにオブジェクトの姿勢を推定する。下図を参照のこと(より正確には、コンピュータビジョン分野の慣例では、カメラフレームのX軸を右向き、Y軸を下向き、Z軸を前向きとする)。

Top: the 6 dof pose computed from a list of 3D-2D point correspondences. Bottom: the considered perspective projection model.

ワールドフレーム \( \bf{X}_w \) で表された点は、透視投影モデル \( \Pi \) とカメラ内部パラメータ行列 \( \bf{A} \)(文献では \( \bf{K} \) とも表記される)を用いて、画像平面 \( \left[ u, v \right] \) に投影される:

\[ \begin{align*} \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} &= \bf{A} \hspace{0.1em} \Pi \hspace{0.2em} ^{c}\bf{T}_w \begin{bmatrix} X_{w} \\ Y_{w} \\ Z_{w} \\ 1 \end{bmatrix} \\ \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} &= \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \end{bmatrix} \begin{bmatrix} r_{11} & r_{12} & r_{13} & t_x \\ r_{21} & r_{22} & r_{23} & t_y \\ r_{31} & r_{32} & r_{33} & t_z \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} X_{w} \\ Y_{w} \\ Z_{w} \\ 1 \end{bmatrix} \end{align*} \]

したがって推定される姿勢は、ワールドフレームで表された3D点をカメラフレームに変換できる回転 (rvec) と並進 (tvec) のベクトルである:

\[ \begin{align*} \begin{bmatrix} X_c \\ Y_c \\ Z_c \\ 1 \end{bmatrix} &= \hspace{0.2em} ^{c}\bf{T}_w \begin{bmatrix} X_{w} \\ Y_{w} \\ Z_{w} \\ 1 \end{bmatrix} \\ \begin{bmatrix} X_c \\ Y_c \\ Z_c \\ 1 \end{bmatrix} &= \begin{bmatrix} r_{11} & r_{12} & r_{13} & t_x \\ r_{21} & r_{22} & r_{23} & t_y \\ r_{31} & r_{32} & r_{33} & t_z \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} X_{w} \\ Y_{w} \\ Z_{w} \\ 1 \end{bmatrix} \end{align*} \]

姿勢計算の手法

取り得る値の一覧についてはcv::SolvePnPMethod enumのドキュメントを参照のこと。各手法の詳細を以下に示す:

cv::SOLVEPNP_ITERATIVE 反復法はLevenberg-Marquardt最適化に基づく。この場合、関数は再投影誤差、すなわち観測された投影 "imagePoints" と (cv::projectPoints を用いて)投影された "objectPoints" との二乗距離の和を最小化する姿勢を求める。非平面の "objectPoints" に対する初期解は少なくとも6点を必要とし、DLTアルゴリズムを用いる。平面の "objectPoints" に対する初期解は少なくとも4点を必要とし、ホモグラフィ分解から得られる姿勢を用いる。
cv::SOLVEPNP_P3P 本手法はDing, Y., Yang, J., Larsson, V., Olsson, C., & Åstrom, K. の論文 "Revisiting the P3P Problem" ([73]) に基づく。この場合、関数はちょうど4組のオブジェクト点と画像点を必要とする。
cv::SOLVEPNP_AP3P 本手法はT. Ke, S. Roumeliotis の論文 "An Efficient Algebraic Solution to the Perspective-Three-Point Problem" ([150]) に基づく。この場合、関数はちょうど4組のオブジェクト点と画像点を必要とする。
cv::SOLVEPNP_EPNP 本手法はF. Moreno-Noguer, V. Lepetit, P. Fua によって論文 "EPnP: Efficient Perspective-n-Point Camera Pose Estimation" ([163]) で導入された。
cv::SOLVEPNP_DLS 実装が壊れている。このフラグを使用するとEPnPにフォールバックする。
本手法はJ. Hesch, S. Roumeliotis の論文 "A Direct Least-Squares (DLS) Method for PnP" ([128]) に基づく。
cv::SOLVEPNP_UPNP 実装が壊れている。このフラグを使用するとEPnPにフォールバックする。
本手法はA. Penate-Sanchez, J. Andrade-Cetto, F. Moreno-Noguer の論文 "Exhaustive Linearization for Robust Camera Pose and Focal Length Estimation" ([220]) に基づく。この場合、関数は \(f_x\) と \(f_y\) が同じ値を持つと仮定して、これらのパラメータも推定する。そして、推定された焦点距離でcameraMatrixが更新される。
cv::SOLVEPNP_IPPE 本手法はT. Collins, A. Bartoli の論文 "Infinitesimal Plane-Based Pose Estimation" ([64]) に基づく。この手法は同一平面上のオブジェクト点を必要とする。
cv::SOLVEPNP_IPPE_SQUARE Method is based on the paper of Toby Collins and Adrien Bartoli. "Infinitesimal Plane-Based Pose Estimation" ([64]). This method is suitable for marker pose estimation. It requires 4 coplanar object points defined in the following order:
- 点 0: [-squareLength / 2, squareLength / 2, 0]
- 点 1: [ squareLength / 2, squareLength / 2, 0]
- 点 2: [ squareLength / 2, -squareLength / 2, 0]
- 点 3: [-squareLength / 2, -squareLength / 2, 0]
cv::SOLVEPNP_SQPNP 本手法はG. Terzakis, M.Lourakis の論文 "A Consistently Fast and Globally Optimal Solution to the Perspective-n-Point Problem" ([274]) に基づく。3点以上を必要とする。

P3P

cv::solveP3P() はちょうど3組の3D-2D点対応からオブジェクトの姿勢を計算する。P3P問題は最大4個の解を持つ。

覚え書き: 解は再投影誤差の小さい順(低い順から高い順)にソートされる。

PnP

cv::solvePnP() は、オブジェクト座標フレームで表された3D点をカメラ座標フレームに変換する回転ベクトルと並進ベクトルを、さまざまな手法を用いて返す:

P3P手法 (cv::SOLVEPNP_P3P, cv::SOLVEPNP_AP3P): 一意な解を返すには4個の入力点が必要。
cv::SOLVEPNP_IPPE 入力点は >= 4 でなければならず、オブジェクト点は同一平面上になければならない。
cv::SOLVEPNP_IPPE_SQUARE Special case suitable for marker pose estimation. Number of input points must be 4. Object points must be defined in the following order:
- 点 0: [-squareLength / 2, squareLength / 2, 0]
- 点 1: [ squareLength / 2, squareLength / 2, 0]
- 点 2: [ squareLength / 2, -squareLength / 2, 0]
- 点 3: [-squareLength / 2, -squareLength / 2, 0]
それ以外のすべてのフラグでは、入力点の数は >= 4 でなければならず、オブジェクト点は任意の配置でよい。

汎用PnP

cv::solvePnPGeneric() は、取り得るすべての解を取得できる。

現在、cv::SOLVEPNP_P3P, cv::SOLVEPNP_AP3P, cv::SOLVEPNP_IPPE, cv::SOLVEPNP_IPPE_SQUARE, cv::SOLVEPNP_SQPNP のみが複数の解を返すことができる。

RANSAC PnP

cv::solvePnPRansac() は、外れ値を扱うためにRANSACスキームを用いてカメラフレームに対する物体の姿勢を計算する。

詳細は [331] を参照

姿勢の精緻化

姿勢の精緻化とは、非線形最小化手法を用い、解の初期推定値から開始して、再投影誤差を最小化する回転と並進を推定することである。OpenCVはこの問題に対して cv::solvePnPRefineLM() と cv::solvePnPRefineVVS() を提供する。

cv::solvePnPRefineLM() は非線形なLevenbergMarquardt最小化スキーム [184] [81] を用いており、現在の実装では回転の更新をSO(3)上ではなく摂動として計算する。

cv::solvePnPRefineVVS() はGaussNewton非線形最小化スキーム [188] を用い、回転部分の更新は指数写像を用いて計算する。

覚え書き: 少なくとも3組の3D2D点対応が必要である。