位姿计算概述

位姿计算问题 [185] 包括求解旋转和平移，以最小化来自 3D-2D 点对应的重投影误差。

solvePnP 和相关函数估计对象位姿，给定一组对象点、它们对应的图像投影，以及相机内参矩阵和畸变系数，如下图所示（更准确地说，计算机视觉领域的惯例是使相机坐标系的 X 轴指向右侧，Y 轴向下，Z 轴向前）。

上图：从 3D-2D 点对应列表计算出的 6 自由度位姿。下图：考虑的透视投影模型。

在世界坐标系 \( \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*} \]

因此，估计的位姿是旋转向量 (rvec) 和平移向量 (tvec)，它们允许将世界坐标系中表示的 3D 点转换到相机坐标系中。

\[ \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 枚举文档，了解可能值的列表。下面描述了每种方法的一些细节。

cv::SOLVEPNP_ITERATIVE 迭代方法基于 Levenberg-Marquardt 优化。在这种情况下，该函数找到一种位姿，使重投影误差最小化，即观察到的投影“imagePoints”和投影的（使用 cv::projectPoints ）“objectPoints”之间的平方距离之和。非平面“objectPoints”的初始解至少需要 6 个点，并使用 DLT 算法。平面“objectPoints”的初始解至少需要 4 个点，并使用单应性分解的位姿。
cv::SOLVEPNP_P3P 该方法基于 X.S. Gao、X.-R. Hou、J. Tang、H.-F. Chang 的论文“透视三点问题的完整解分类”([99])。在这种情况下，该函数恰好需要四个对象点和图像点。
cv::SOLVEPNP_AP3P 该方法基于 T. Ke、S. Roumeliotis 的论文“透视三点问题的有效代数解”([147])。在这种情况下，该函数恰好需要四个对象点和图像点。
cv::SOLVEPNP_EPNP 该方法由 F. Moreno-Noguer、V. Lepetit 和 P. Fua 在论文“EPnP：高效透视 n 点相机位姿估计”([160]) 中引入。
cv::SOLVEPNP_DLS 已损坏的实现。使用此标志将回退到 EPnP。
该方法基于 J. Hesch 和 S. Roumeliotis 的论文。“PnP 的直接最小二乘 (DLS) 方法”([127])。
cv::SOLVEPNP_UPNP 已损坏的实现。使用此标志将回退到 EPnP。
该方法基于 A. Penate-Sanchez、J. Andrade-Cetto、F. Moreno-Noguer 的论文。“鲁棒相机位姿和焦距估计的穷举线性化”([217])。在这种情况下，该函数还会估计参数 \(f_x\) 和 \(f_y\)，假设两者具有相同的值。然后，使用估计的焦距更新 cameraMatrix。
cv::SOLVEPNP_IPPE 该方法基于 T. Collins 和 A. Bartoli 的论文。“无穷小平面到位姿估计”([63])。此方法需要共面对象点。
cv::SOLVEPNP_IPPE_SQUARE 该方法基于 Toby Collins 和 Adrien Bartoli 的论文。“无穷小平面到位姿估计”([63])。此方法适用于标记位姿估计。它需要按以下顺序定义的 4 个共面对象点
- 点 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 的论文“透视 n 点问题的一致快速和全局最优解”([271])。它需要 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 适用于标记位姿估计的特殊情况。输入点的数量必须为 4。对象点必须按以下顺序定义
- 点 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 方案计算对象相对于相机坐标系的位姿，以处理异常值。

更多信息可以在 [328] 中找到

位姿细化

位姿细化包括估计旋转和平移，使用非线性最小化方法并从解的初始估计开始，最小化重投影误差。OpenCV 提出了 cv::solvePnPRefineLM() 和 cv::solvePnPRefineVVS() 来解决这个问题。

cv::solvePnPRefineLM() 使用非线性 Levenberg-Marquardt 最小化方案 [181] [79]，当前的实现将旋转更新计算为扰动，而不是在 SO(3) 上计算。

cv::solvePnPRefineVVS() 使用 Gauss-Newton 非线性最小化方案 [185]，并且使用指数映射计算的旋转部分的更新。

注意: 至少需要三个 3D-2D 点对应关系。