- 操作系统：ubuntu22.04
- OpenCV版本：OpenCV4.9
- IDE:Visual Studio Code
- 编程语言：C++11
算法描述
根据 3 个 3D-2D 点对应关系找到物体的姿态。
cv::solveP3P 是 OpenCV 中的一个函数，用于解决 Perspective-3-Point (P3P) 问题。该问题的目标是根据给定的三个空间点（世界坐标系中的已知位置）及其对应的图像点（在图像平面上的位置），估计相机的姿态（旋转和平移）。这是计算机视觉和机器人学中一个经典的问题，常用于单目视觉定位、增强现实等领域。
函数原型
int cv::solveP3P
(
InputArray objectPoints,
InputArray imagePoints,
InputArray cameraMatrix,
InputArray distCoeffs,
OutputArrayOfArrays rvecs,
OutputArrayOfArrays tvecs,
int flags
)
参数
- 参数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= fx000fy0cxcy1 。
- 参数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 个物体点、它们对应的图像投影、相机内参矩阵和畸变系数估计物体的姿态。
注意
解按照重投影误差从小到大排序。
代码示例
#include <iostream>
#include <opencv2/opencv.hpp>
using namespace cv;
using namespace std;
int main()
{
// 定义世界坐标系中的3D点
vector< Point3f > objectPoints = { Point3f( 0.0f, 0.0f, 0.0f ), Point3f( 1.0f, 0.0f, 0.0f ), Point3f( 0.0f, 1.0f, 0.0f ) };
// 定义图像平面上的2D点
vector< Point2f > imagePoints = { Point2f( 300.0f, 200.0f ), Point2f( 400.0f, 200.0f ), Point2f( 300.0f, 300.0f ) };
// 定义相机内参矩阵
Mat cameraMatrix = ( Mat_< double >( 3, 3 ) << 500, 0, 320, 0, 500, 240, 0, 0, 1 );
// 定义畸变系数（假设无畸变）
Mat distCoeffs = Mat::zeros( 5, 1, CV_64F );
// 存储结果的旋转和平移向量
vector< Vec3d > rvecs, tvecs;
// 调用 solveP3P 函数
int solutions = solveP3P( objectPoints, imagePoints, cameraMatrix, distCoeffs, rvecs, tvecs, SOLVEPNP_AP3P );
// 打印结果
cout << "Number of solutions: " << solutions << endl;
for ( int i = 0; i < solutions; ++i )
{
cout << "Solution " << i + 1 << ":" << endl;
cout << "Rotation Vector: " << rvecs[ i ] << endl;
cout << "Translation Vector: " << tvecs[ i ] << endl;
}
return 0;
}
运行结果
Number of solutions: 4
Solution 1:
Rotation Vector: [0.126348, -0.108248, -0.00179646]
Translation Vector: [-0.195386, -0.390773, 4.88466]
Solution 2:
Rotation Vector: [0.0710923, 0.375849, 0.0164948]
Translation Vector: [-0.197736, -0.395471, 4.94339]
Solution 3:
Rotation Vector: [0, 0, 0]
Translation Vector: [-0.2, -0.4, 5]
Solution 4:
Rotation Vector: [-0.277914, -0.0306778, -0.00682145]
Translation Vector: [-0.198903, -0.397806, 4.97257]