OpenCV相机标定与3D重建(55)通用解决 PnP 问题函数solvePnPGeneric()的使用

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

算法描述

根据3D-2D点对应关系找到物体的姿态。
cv::solvePnPGeneric 是 OpenCV 中一个更为通用的函数，用于解决 PnP 问题。它能够返回多个可能的姿态解（旋转和平移向量），并且支持多种不同的求解方法。这在某些情况下特别有用，例如当存在多解时，或者需要评估不同解的质量。

此函数返回所有可能的解的列表（一个解是一对<旋转向量, 平移向量>），具体取决于输入点的数量和选择的方法：
P3P 方法（SOLVEPNP_P3P, SOLVEPNP_AP3P）：需要 3 或 4 个输入点。如果有 3 个输入点，返回的解的数量可以在 0 到 4 之间。
-SOLVEPNP_IPPE：输入点必须 >= 4 且物体点必须共面。返回 2 个解。
SOLVEPNP_IPPE_SQUARE：适用于标记姿态估计的特殊情况。输入点的数量必须是 4，并且返回 2 个解。物体点必须按以下顺序定义：
- 点 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，且物体点可以是任意配置。仅返回 1 个解。

函数原型


int cv::solvePnPGeneric
(
	InputArray 	objectPoints,
	InputArray 	imagePoints,
	InputArray 	cameraMatrix,
	InputArray 	distCoeffs,
	OutputArrayOfArrays 	rvecs,
	OutputArrayOfArrays 	tvecs,
	bool 	useExtrinsicGuess = false,
	SolvePnPMethod 	flags = SOLVEPNP_ITERATIVE,
	InputArray 	rvec = noArray(),
	InputArray 	tvec = noArray(),
	OutputArray 	reprojectionError = noArray() 
)

参数

参数 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 个元素。如果该向量为空，则假设畸变为零。
-参数rvecs：输出的旋转向量数组（见 Rodrigues），与 tvecs 一起使用，将模型坐标系中的点变换到相机坐标系中。
-参数tvecs：输出的平移向量数组。
-参数useExtrinsicGuess：仅用于 SOLVEPNP_ITERATIVE 方法。如果为 true（1），函数会使用提供的 rvec 和 tvec 值作为旋转和平移向量的初始近似值，并进一步优化它们。
-参数flags：解决 PnP 问题的方法，详见 calib3d_solvePnP_flags。
-参数rvec：当标志为 SOLVEPNP_ITERATIVE 且 useExtrinsicGuess 设置为 true 时，用于初始化迭代 PnP 精化算法的旋转向量。
-参数tvec：当标志为 SOLVEPNP_ITERATIVE 且 useExtrinsicGuess 设置为 true 时，用于初始化迭代 PnP 精化算法的平移向量。
-参数reprojectionError：可选的重投影误差向量，即输入图像点和用估计的姿态投影的 3D 物体点之间的均方根误差 $\text{RMSE} = \sqrt{\frac{\sum_{i}^{N} \left ( \hat{y_i} - y_i \right )^2}{N}}$
关于如何使用 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]

代码示例


#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 );  // 如果没有畸变或忽略畸变，则可以是零矩阵

    // 初始化输出变量
    std::vector< Mat > rvecs, tvecs;
    Mat reprojectionError;

    // 调用 solvePnPGeneric 函数
    int solutionsFound = solvePnPGeneric( objectPoints, imagePoints, cameraMatrix, distCoeffs, rvecs, tvecs, false, SOLVEPNP_ITERATIVE, noArray(), noArray(), reprojectionError );

    if ( solutionsFound > 0 )
    {
        cout << "Number of solutions found: " << solutionsFound << endl;

        for ( int i = 0; i < solutionsFound; ++i )
        {
            cout << "\nSolution " << i + 1 << ":\n";
            cout << "Rotation Vector:\n" << rvecs[ i ] << "\nTranslation Vector:\n" << tvecs[ i ] << endl;

            // 可选：将旋转向量转换为旋转矩阵以更好地理解结果
            Mat rotationMatrix;
            Rodrigues( rvecs[ i ], rotationMatrix );
            cout << "Rotation Matrix:\n" << rotationMatrix << endl;
        }

        if ( !reprojectionError.empty() )
        {
            cout << "Reprojection Error: " << reprojectionError.at< double >( 0 ) << endl;
        }
    }
    else
    {
        cout << "solvePnPGeneric failed to find any solution." << endl;
    }

    return 0;
}

运行结果

Number of solutions found: 1

Solution 1:
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]
Reprojection Error: 5.21212e-315