视角-点（PnP）姿态计算 OpenCV v4.8.0

HIRSHDZ327

已于 2023-10-02 19:52:44 修改

阅读量401

点赞数

分类专栏： OpenCV 4.8.0 说明文档文章标签： opencv

于 2023-10-02 19:50:57 首次发布

OpenCV 4.8.0 说明文档专栏收录该内容

140 篇文章

订阅专栏

姿态计算概述

姿态计算问题 [170] 包括求解旋转和平移，使三维-二维点对应的重投影误差最小。

solvePnP 和相关函数会根据一组物体点、其对应的图像投影以及摄像机本征矩阵和畸变系数来估计物体姿态，见下图（更准确地说，摄像机帧的 X 轴指向右侧，Y 轴向下，Z 轴向前）。
在这里插入图片描述
Camera coordinate system：相机坐标系
World coordinate system:世界坐标系（基坐标系）

利用透视投影模型 Π 和摄像机固有参数矩阵 A（文献中也用 K 表示），将世界帧 Xw 中表示的点投影到图像平面 [u,v] 中：
$\left[ \begin{array}{c} u\\ v\\ 1\\ \end{array} \right] =A\varPi ^cT_W\left[ \begin{array}{c} X_{\omega}\\ Y_{\omega}\\ Z_{\omega}\\ 1\\ \end{array} \right]$
$\left[ \begin{array}{c} u\\ v\\ 1\\ \end{array} \right] =\left[ \begin{matrix} f_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\\ \end{matrix} \right] \left[ \begin{matrix} 1& 0& \begin{matrix} 0& 0\\ \end{matrix}\\ 0& 1& \begin{matrix} 0& 0\\ \end{matrix}\\ 0& 0& \begin{matrix} 1& 0\\ \end{matrix}\\ \end{matrix} \right] \left[ \begin{matrix} 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{matrix} \right] \left[ \begin{array}{c} X_{\omega}\\ Y_{\omega}\\ Z_{\omega}\\ 1\\ \end{array} \right]$
因此，估计姿势就是旋转（rvec）和平移（tvec）矢量，可以将世界帧中表示的三维点转换到摄像机帧中：
$\left[ \begin{array}{c} X_c\\ Y_c\\ Z_c\\ 1\\ \end{array} \right] =T_w\left[ \begin{array}{c} X_{\omega}\\ Y_{\omega}\\ Z_{\omega}\\ 1\\ \end{array} \right]$

$\left[ \begin{array}{c} X_c\\ Y_c\\ Z_c\\ 1\\ \end{array} \right] =\left[ \begin{matrix} 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{matrix} \right] \left[ \begin{array}{c} X_{\omega}\\ Y_{\omega}\\ Z_{\omega}\\ 1\\ \end{array} \right]$

姿势计算方法

有关可能值的列表，请参阅 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 “Complete Solution Classification for the Perspective-Three-Point Problem” ([88])的论文。在这种情况下，函数恰好需要四个对象点和图像点。
cv::SOLVEPNP_AP3P 方法基于 T. Ke 和 S. Roumeliotis 的论文 “透视-三点问题的高效代数解” ([133]) 。在这种情况下，函数恰好需要四个对象点和图像点。
cv::SOLVEPNP_EPNP 方法由 F. Moreno-Noguer、V. Lepetit 和 P. Fua 在论文 “EPnP: Efficient Perspective-n-Point Camera Pose Estimation”（[145]）中提出。
cv::SOLVEPNP_DLS 已损坏的实现。使用此标志将退回到 EPnP。
该方法基于 J. Hesch 和 S. Roumeliotis 的论文。“PnP 的直接最小二乘法 (DLS) 方法” ([114]).
cv::SOLVEPNP_UPNP 已损坏的实现。使用此标志将退回到 EPnP。
该方法基于 A. Penate-Sanchez、J. Andrade-Cetto、F. Moreno-Noguer 的论文。“Exhaustive Linearization for Robust Camera Pose and Focal Length Estimation” ([199])。在这种情况下，该函数还能估计参数 fx 和 fy，并假设两者具有相同的值。然后用估计的焦距更新 cameraMatrix。
cv::SOLVEPNP_IPPE 方法基于 T. Collins 和 A. Bartoli 的论文。“基于无限小平面的姿态估计”（[52]）。此方法需要共面物体点。
cv::SOLVEPNP_IPPE_SQUARE 方法基于 Toby Collins 和 Adrien Bartoli 的论文。“基于无限小平面的姿态估计”（[52]）。该方法适用于标记姿态估计。它需要按以下顺序定义 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 的论文 “A Consistently Fast and Globally Optimal Solution to the Perspective-n-Point Problem”（[252]）。它需要 3 个或更多点。