姿勢計算の概要

姿勢計算問題 [189] は、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" ([149]) に基づく。この場合、関数はちょうど4個の物体点と画像点を必要とする。
cv::SOLVEPNP_EPNP この手法は F. Moreno-Noguer, V. Lepetit, P. Fua によって論文 "EPnP: Efficient Perspective-n-Point Camera Pose Estimation" ([162]) で導入された。
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" ([277]) に基づく。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手法を用いてカメラ座標系に対する物体の姿勢を計算する。

詳しくは [336] を参照のこと。

姿勢の精緻化

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

cv::solvePnPRefineLM() は非線形のLevenberg-Marquardt最小化法 [185] [81] を用いており、現在の実装では回転の更新を摂動として計算し、SO(3)上では計算しない。

cv::solvePnPRefineVVS() はGauss-Newton非線形最小化法 [189] を用いており、回転部分の更新は指数写像を使って計算する。

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