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

操作系统：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]