OpenCV相机标定与3D重建(56)估计物体姿态（即旋转和平移）的函数solvePnPRansac()的使用

news2025/1/15 15:46:21

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

算法描述

使用RANSAC方案从3D-2D点对应关系中找到物体的姿态。
cv::solvePnPRansac 是 OpenCV 中用于估计物体姿态（即旋转和平移）的函数，它通过随机抽样一致性算法（RANSAC）来增强对异常值（outliers）的鲁棒性。

函数原型

bool cv::solvePnPRansac	
(
	InputArray 	objectPoints,
	InputArray 	imagePoints,
	InputArray 	cameraMatrix,
	InputArray 	distCoeffs,
	OutputArray 	rvec,
	OutputArray 	tvec,
	bool 	useExtrinsicGuess = false,
	int 	iterationsCount = 100,
	float 	reprojectionError = 8.0,
	double 	confidence = 0.99,
	OutputArray 	inliers = noArray(),
	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 方法。如果为真（1），函数会使用提供的 rvec 和 tvec 值作为旋转和平移向量的初始近似值，并进一步优化它们。
参数iterationsCount 迭代次数。
参数reprojectionError RANSAC过程使用的内点阈值。参数值是观察到的投影点和计算出的投影点之间的最大允许距离，以被认为是内点。
参数 confidence 算法产生有用结果的概率。
参数inliers 输出向量，包含 objectPoints 和 imagePoints 中内点的索引。
参数flags 解决PnP问题的方法（见 solvePnP）。

该函数根据一组物体点及其对应的图像投影，以及相机内参矩阵和畸变系数来估计物体的姿态。这个函数寻找一个使重投影误差最小的姿态，即观测到的投影点 imagePoints 和使用 projectPoints 投影的 objectPoints 之间的平方距离之和最小。RANSAC的使用使得函数对异常值具有鲁棒性。

注意

使用 solvePNPRansac 进行物体检测的一个示例可以在 opencv_source_code/samples/cpp/tutorial_code/calib3d/real_time_pose_estimation/ 找到。
最小样本集步骤中用于估计相机姿态的默认方法是 SOLVEPNP_EPNP。例外情况包括：
如果选择了 SOLVEPNP_P3P 或 SOLVEPNP_AP3P，则这些方法将被使用。
如果输入点的数量等于4，则使用 SOLVEPNP_P3P。
使用所有内点估计相机姿态的方法由 flags 参数定义，除非它等于 SOLVEPNP_P3P 或 SOLVEPNP_AP3P，在这种情况下，将使用 SOLVEPNP_EPNP 方法代替。

代码示例

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

using namespace cv;
using namespace std;

int main()
{
    // 确保至少有4个点对
    vector< Point3f > objectPoints = { Point3f( 0.0f, 0.0f, 0.0f ), Point3f( 1.0f, 0.0f, 0.0f ), Point3f( 0.0f, 1.0f, 0.0f ), Point3f( 1.0f, 1.0f, 0.0f ) };

    vector< Point2f > imagePoints = { Point2f( 300.0f, 300.0f ), Point2f( 400.0f, 300.0f ), Point2f( 300.0f, 400.0f ), Point2f( 400.0f, 400.0f ) };

    // 检查点的数量是否一致
    if ( objectPoints.size() != imagePoints.size() || objectPoints.size() < 4 )
    {
        cerr << "Error: Need at least 4 point pairs and the number of points must match." << endl;
        return -1;
    }

    // 相机内参矩阵
    Mat cameraMatrix = ( Mat_< double >( 3, 3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );

    // 畸变系数
    Mat distCoeffs = ( Mat_< double >( 5, 1 ) << 0.2624, -0.9531, -0.0054, 0.0026, 1.1633 );

    // 输出变量
    Vec3d rvec;  // 旋转向量
    Vec3d tvec;  // 平移向量

    // 调用 solvePnPRansac 函数
    bool success = solvePnPRansac( objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec, false, 100, 8.0, 0.99, noArray(), SOLVEPNP_ITERATIVE );

    if ( success )
    {
        cout << "成功找到解：" << endl;
        cout << "旋转向量: " << rvec << endl;
        cout << "平移向量: " << tvec << endl;

        // 可选：将旋转向量转换为旋转矩阵
        Mat R;
        Rodrigues( rvec, R );
        cout << "旋转矩阵: " << endl << R << endl;
    }
    else
    {
        cout << "未能找到有效的解" << endl;
    }

    return 0;
}

运行结果

成功找到解：
旋转向量: [0.0425377, -0.0162527, -0.000105512]
平移向量: [-0.251554, 0.504018, 5.22556]
旋转矩阵: 
[0.9998679414301481, -0.000240141627923739, -0.01624937021045788;
 -0.0004510929798306415, 0.999095423415709, -0.04252212864978416;
 0.01624488274385277, 0.04252384321511622, 0.9989633759767446]