OpenCV相机标定与3D重建(54)解决透视 n 点问题（Perspective-n-Point, PnP）函数solvePnP()的使用

操作系统：ubuntu22.04
OpenCV版本：OpenCV4.9
IDE:Visual Studio Code
编程语言：C++11

算法描述

根据3D-2D点对应关系找到物体的姿态。
cv::solvePnP 是 OpenCV 库中的一个函数，用于解决透视 n 点问题（Perspective-n-Point, PnP），即通过已知的 3D 点及其对应的 2D 图像点来估计物体的姿态（旋转和平移）。这个函数可以处理任意数量的点对，并且提供了多种算法来求解姿态。

此函数返回旋转和平移向量，这些向量将用物体坐标系表示的3D点变换到相机坐标系中，使用不同的方法：

P3P 方法（SOLVEPNP_P3P, SOLVEPNP_AP3P）：需要4个输入点来返回一个唯一解。
SOLVEPNP_IPPE：输入点必须 >= 4 且物体点必须共面。
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，且物体点可以是任意配置。

函数原型

bool cv::solvePnP
(
	InputArray 	objectPoints,
	InputArray 	imagePoints,
	InputArray 	cameraMatrix,
	InputArray 	distCoeffs,
	OutputArray 	rvec,
	OutputArray 	tvec,
	bool 	useExtrinsicGuess = false,
	int 	flags = SOLVEPNP_ITERATIVE 
)

参数

参数objectPoints：物体坐标空间中的物体点数组，格式为 Nx3 的单通道或 1xN/Nx1 的三通道，其中 N 是点的数量。也可以传递 vector。
参数imagePoints：对应的图像点数组，格式为 Nx2 的单通道或 1xN/Nx1 的双通道，其中 N 是点的数量。也可以传递 vector。
参数cameraMatrix：输入的相机内参矩阵 $\begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix}$
。
参数distCoeffs：输入的畸变系数向量 (k1, k2, p1, p2[, k3[, k4, k5, k6[, s1, s2, s3, s4[, τx, τy]]]])，包含 4、5、8、12 或 14 个元素。如果该向量为空，则假设畸变为零。
参数rvec：输出的旋转向量（见 Rodrigues），与 tvec 一起使用，将模型坐标系中的点变换到相机坐标系中。
参数tvec：输出的平移向量。
参数useExtrinsicGuess：仅用于 SOLVEPNP_ITERATIVE 方法。如果为 true（1），函数会使用提供的 rvec 和 tvec 值作为旋转和平移向量的初始近似值，并进一步优化它们。
参数flags：解决 PnP 问题的方法，详见 calib3d_solvePnP_flags。

注意

关于如何使用 solvePnP 进行平面增强现实的一个示例可以在 opencv_source_code/samples/python/plane_ar.py 找到。
如果你使用的是 Python：
- Numpy 数组切片不能作为输入，因为 solvePnP 需要连续的数组（在版本 2.4.9 的 modules/calib3d/src/solvepnp.cpp 文件大约第 55 行通过 cv::Mat::checkVector() 断言强制要求）。
- P3P 算法要求图像点位于形状为 (N,1,2) 的数组中，因为它调用了 undistortPoints（在版本 2.4.9 的 modules/calib3d/src/solvepnp.cpp 文件大约第 75 行），这需要双通道信息。
- 因此，给定一些数据 D = np.array(…)，其中 D.shape = (N,M)，为了使用其子集作为例如 imagePoints，必须有效地将其复制到一个新数组中：imagePoints = np.ascontiguousarray(D[:,:2]).reshape((N,1,2))。
方法 SOLVEPNP_DLS 和 SOLVEPNP_UPNP 不能使用，因为当前实现不稳定，有时会给出完全错误的结果。如果你传递了这两个标志中的一个，则会使用 SOLVEPNP_EPNP 方法代替。
在一般情况下，最少需要 4 个点。
对于 SOLVEPNP_P3P 和 SOLVEPNP_AP3P 方法，必须使用恰好 4 个点（前 3 个点用于估计 P3P 问题的所有解，最后一个点用于保留最小化重投影误差的最佳解）。
使用 SOLVEPNP_ITERATIVE 方法且 useExtrinsicGuess=true 时，最少需要 3 个点（3 个点足以计算姿态，但最多有 4 个解）。初始解应接近全局解以收敛。
使用 SOLVEPNP_IPPE 时，输入点必须 >= 4 且物体点必须共面。
使用 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]

使用 SOLVEPNP_SQPNP 时，输入点必须 >= 3。

代码示例


#include <iostream>
#include <opencv2/opencv.hpp>
#include <vector>

using namespace cv;
using namespace std;

int main()
{
    // 假设我们有一个已知的 3D 点集 (例如一个正方形的四个角)
    std::vector< Point3f > objectPoints = { Point3f( -1.0f, -1.0f, 0.0f ), Point3f( 1.0f, -1.0f, 0.0f ), Point3f( 1.0f, 1.0f, 0.0f ), Point3f( -1.0f, 1.0f, 0.0f ) };

    // 对应的 2D 图像点 (这些点是从图像中检测到的特征点)
    std::vector< Point2f > imagePoints = { Point2f( 594.0f, 487.0f ), Point2f( 673.0f, 487.0f ), Point2f( 673.0f, 552.0f ), Point2f( 594.0f, 552.0f ) };

    // 相机内参矩阵 (假设已知)
    Mat cameraMatrix = ( Mat_< double >( 3, 3 ) << 718.856, 0, 607.1928, 0, 718.856, 185.2157, 0, 0, 1 );

    // 畸变系数 (假设已知)
    Mat distCoeffs = Mat::zeros( 5, 1, CV_64F );  // 如果没有畸变或忽略畸变，则可以是零矩阵

    // 初始化输出变量
    Mat rvec;  // 旋转向量
    Mat tvec;  // 平移向量

    // 调用 solvePnP 函数
    bool success = solvePnP( objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec, false, SOLVEPNP_ITERATIVE );

    if ( success )
    {
        cout << "Rotation Vector:\n" << rvec << "\nTranslation Vector:\n" << tvec << endl;

        // 可选：将旋转向量转换为旋转矩阵以更好地理解结果
        Mat rotationMatrix;
        Rodrigues( rvec, rotationMatrix );
        cout << "Rotation Matrix:\n" << rotationMatrix << endl;
    }
    else
    {
        cout << "solvePnP failed." << endl;
    }

    return 0;
}

运行结果

Rotation Vector:
[0.2895361443049176;
 0.01328548677652798;
 -0.008684530349597173]
Translation Vector:
[0.6665924885943908;
 8.493287223698232;
 18.23641869746051]
Rotation Matrix:
[0.999874917527441, 0.01047321277960457, 0.01185162915241468;
 -0.006653461772789516, 0.9583398410008748, -0.2855529383439369;
 -0.01434854508064377, 0.2854383663148514, 0.9582896526048779]