OpenCV相机标定与3D重建(53)解决 Perspective-3-Point (P3P) 问题函数solve ...

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

算法形貌

根据 3 个 3D-2D 点对应关系找到物体的姿态。
cv::solveP3P 是 OpenCV 中的一个函数，用于解决 Perspective-3-Point (P3P) 问题。该问题的目标是根据给定的三个空间点（世界坐标系中的已知位置）及其对应的图像点（在图像平面上的位置），估计相机的姿态（旋转和平移）。这是计算机视觉和呆板人学中一个经典的问题，常用于单目视觉定位、增强现实等领域。
函数原型

2. int cv::solveP3P
3. (
4.         InputArray         objectPoints,
5.         InputArray         imagePoints,
6.         InputArray         cameraMatrix,
7.         InputArray         distCoeffs,
8.         OutputArrayOfArrays         rvecs,
9.         OutputArrayOfArrays         tvecs,
10.         int         flags
11. )       

复制代码

参数

• 参数objectPoints 物体坐标空间中的物体点数组，格式为 3x3 的单通道或 1x3/3x1 的三通道。也可以传递 vector。
  
• 参数imagePoints 对应的图像点数组，格式为 3x2 的单通道或 1x3/3x1 的双通道。也可以传递 vector。
  
• 参数cameraMatrix 输入的相机内参矩阵                                         A                            =                                       [                                                                                                     f                                              x                                                                                                            0                                                                                                             c                                              x                                                                                                                                  0                                                                                                             f                                              y                                                                                                                            c                                              y                                                                                                                                  0                                                                                             0                                                                                             1                                                                                 ]                                            A = \begin{bmatrix}f_x & 0 & c_x \\0 & f_y & c_y \\0 & 0 & 1\end{bmatrix}                     A=               ​fx​00​0fy​0​cx​cy​1​               ​。
  
• 参数distCoeffs 输入的畸变系数向量 (k1, k2, p1, p2 [,k3 [,k4, k5, k6 [,s1, s2, s3, s4 [,τx, τy]]]])，包罗 4、5、8、12 或 14 个元素。如果该向量为空，则假设畸变为零。
  
• 参数rvecs 输出的旋转向量（见 Rodrigues），与 tvecs 一起将模型坐标系中的点变更到相机坐标系中。一个 P3P 问题最多有 4 个解。
  
• 参数tvecs 输出的平移向量。
  
• 参数flags 解决 P3P 问题的方法：
  
  
  
  • SOLVEPNP_P3P 方法基于高小山、侯晓荣、唐建生、常华峰的论文 “Complete Solution Classification for the Perspective-Three-Point Problem” ([96])。
    
  • SOLVEPNP_AP3P 方法基于 T. Ke 和 S. Roumeliotis 的论文 “An Efficient Algebraic Solution to the Perspective-Three-Point Problem” ([141])。
    该函数根据 3 个物体点、它们对应的图像投影、相机内参矩阵和畸变系数估计物体的姿态。
    
  
  

留意
解按照重投影毛病从小到大排序。
代码示例

2. #include <iostream>
3. #include <opencv2/opencv.hpp>
5. using namespace cv;
6. using namespace std;
8. int main()
9. {
10.     // 定义世界坐标系中的3D点
11.     vector< Point3f > objectPoints = { Point3f( 0.0f, 0.0f, 0.0f ), Point3f( 1.0f, 0.0f, 0.0f ), Point3f( 0.0f, 1.0f, 0.0f ) };
13.     // 定义图像平面上的2D点
14.     vector< Point2f > imagePoints = { Point2f( 300.0f, 200.0f ), Point2f( 400.0f, 200.0f ), Point2f( 300.0f, 300.0f ) };
16.     // 定义相机内参矩阵
17.     Mat cameraMatrix = ( Mat_< double >( 3, 3 ) << 500, 0, 320, 0, 500, 240, 0, 0, 1 );
19.     // 定义畸变系数（假设无畸变）
20.     Mat distCoeffs = Mat::zeros( 5, 1, CV_64F );
22.     // 存储结果的旋转和平移向量
23.     vector< Vec3d > rvecs, tvecs;
25.     // 调用 solveP3P 函数
26.     int solutions = solveP3P( objectPoints, imagePoints, cameraMatrix, distCoeffs, rvecs, tvecs, SOLVEPNP_AP3P );
28.     // 打印结果
29.     cout << "Number of solutions: " << solutions << endl;
30.     for ( int i = 0; i < solutions; ++i )
31.     {
32.         cout << "Solution " << i + 1 << ":" << endl;
33.         cout << "Rotation Vector: " << rvecs[ i ] << endl;
34.         cout << "Translation Vector: " << tvecs[ i ] << endl;
35.     }
37.     return 0;
38. }

复制代码

运行效果

1. Number of solutions: 4
2. Solution 1:
3. Rotation Vector: [0.126348, -0.108248, -0.00179646]
4. Translation Vector: [-0.195386, -0.390773, 4.88466]
5. Solution 2:
6. Rotation Vector: [0.0710923, 0.375849, 0.0164948]
7. Translation Vector: [-0.197736, -0.395471, 4.94339]
8. Solution 3:
9. Rotation Vector: [0, 0, 0]
10. Translation Vector: [-0.2, -0.4, 5]
11. Solution 4:
12. Rotation Vector: [-0.277914, -0.0306778, -0.00682145]
13. Translation Vector: [-0.198903, -0.397806, 4.97257]

复制代码

免责声明：如果侵犯了您的权益，请联系站长，我们会及时删除侵权内容，谢谢合作！更多信息从访问主页：qidao123.com:ToB企服之家，中国第一个企服评测及商务社交产业平台。