使用PnP算法计算装甲板旋转向量和平移向量

主要分为以下步骤

1 加载相机矩阵和畸变参数

2加载图片并进行预处理

3定义装甲板四个顶点的坐标

4定义装甲板四个顶点在图像中的坐标

5使用PnP算法计算旋转向量和平移向量

主要使用solvePnP函数

6将旋转向量转化为旋转矩阵

7输出旋转矩阵和平移矩阵

#include <opencv2/opencv.hpp>

using namespace std;
using namespace cv;

int main()
{
    // 加载相机矩阵和畸变参数
    Mat camera_matrix = (Mat_<double>(3, 3) << 2.4533055557326181e+03, 0., 9.4925681914912386e+02, 0.,
                         2.4551723094234911e+03, 5.5454977746938223e+02, 0., 0., 1.);
    Mat distortion_coeffs = (Mat_<double>(1, 5) << -2.7582931901279197e-01, 1.3973322381045875e-01,
                             2.8099747483245295e-03, -1.8185597815801518e-04, 0.);

    // 加载图片并进行预处理
    Mat src = imread("11.jpg");
    Mat gray;
    cvtColor(src, gray, COLOR_BGR2GRAY);

    // 定义装甲板四个顶点的坐标
    vector<Point3f> object_points;
    object_points.push_back(Point3f(-12.5f, -5.5f, 0.f)); // 左下角
    object_points.push_back(Point3f(-12.5f, 5.5f, 0.f));  // 左上角
    object_points.push_back(Point3f(12.5f, 5.5f, 0.f));   // 右上角
    object_points.push_back(Point3f(12.5f, -5.5f, 0.f));  // 右下角

    // 定义装甲板四个顶点在图像中的坐标
    vector<Point2f> image_points;
    image_points.push_back(Point2f(227, 197)); // 左下角
    image_points.push_back(Point2f(214, 89));  // 左上角
    image_points.push_back(Point2f(360, 71));  // 右上角
    image_points.push_back(Point2f(364, 179)); // 右下角

    // 使用PnP算法计算旋转向量和平移向量
    Mat rvec, tvec;
    solvePnP(object_points, image_points, camera_matrix, distortion_coeffs, rvec, tvec);

    // 将旋转向量转化为旋转矩阵
    Mat rotation_matrix;
    Rodrigues(rvec, rotation_matrix);

    // 输出旋转矩阵和平移矩阵
    cout << "Rotation Matrix: " << endl << rotation_matrix << endl;
    cout << "Translation Vector: " << endl << tvec << endl;

    return 0;
}