位姿计算概述

位姿计算问题[182]在于求解旋转和平移，以最小化3D-2D点对应之间的重投影误差。

solvePnP及相关函数根据一组物体点、其对应的图像投影、相机内参矩阵和畸变系数来估计物体位姿，见下图（更准确地说，相机坐标系的X轴指向右侧，Y轴向下，Z轴向前）。

世界坐标系中表达的点\( \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的论文“透视三点问题的完全解分类”（[97]）。在这种情况下，该函数需要精确的四个物体点和图像点。
cv::SOLVEPNP_AP3P 方法基于T. Ke, S. Roumeliotis的论文“透视三点问题的一种高效代数解”（[144]）。在这种情况下，该函数需要精确的四个物体点和图像点。
cv::SOLVEPNP_EPNP 方法由F. Moreno-Noguer, V. Lepetit和P. Fua在论文“EPnP：高效透视n点相机位姿估计”（[157]）中提出。
cv::SOLVEPNP_DLS **实现已损坏。使用此标志将回退到EPnP。**
方法基于J. Hesch和S. Roumeliotis的论文。“PnP的一种直接最小二乘（DLS）方法”（[124]）。
cv::SOLVEPNP_UPNP **实现已损坏。使用此标志将回退到EPnP。**
方法基于A. Penate-Sanchez, J. Andrade-Cetto, F. Moreno-Noguer的论文。“用于鲁棒相机位姿和焦距估计的穷举线性化”（[214]）。在这种情况下，该函数还估计参数\(f_x\)和\(f_y\)，假设两者具有相同的值。然后使用估计的焦距更新cameraMatrix。
cv::SOLVEPNP_IPPE 方法基于T. Collins和A. Bartoli的论文。“基于无穷小平面的位姿估计”（[61]）。此方法需要共面物体点。
cv::SOLVEPNP_IPPE_SQUARE 方法基于Toby Collins和Adrien Bartoli的论文。“基于无穷小平面的位姿估计”（[61]）。此方法适用于标记位姿估计。它需要按以下顺序定义的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点问题的一致快速全局最优解”（[268]）。它需要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 方案计算物体相对于相机坐标系的位姿，以处理离群值。

更多信息可在 [326] 中找到。

姿态细化

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

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

cv::solvePnPRefineVVS() 使用高斯-牛顿非线性最小化方案 [182]，并使用指数映射计算旋转部分的更新。

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