OpenCV相机标定与3D重建(53)解决 Perspective-3-Point (P3P) 问题函数solveP3P()的使用

操作系统：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 输入的相机内参矩阵 $\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 一起将模型坐标系中的点变换到相机坐标系中。一个 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]