//文件保护符 #ifndef mainPNP #define mainPNP #include<iostream> #include<opencv.hpp> #include"struct_create.h" using namespace std; using namespace cv; //因为写在一个工程里，所以写成了一个函数 //传入的是四个点的世界坐标和图像坐标，它们的顺序必须是一一对应的 Point3f mainOfPNP(vector<Point3f> p4p_3, vector<Point2f> p4p_2) { Mat camera_matrix; Mat distortion_coefficients; Mat rvec; Mat tvec; //相机的内参矩阵 camera_matrix = cv::Mat(3, 3, CV_64FC1, cv::Scalar::all(0)); camera_matrix.ptr<double>(0)[0] = 645.127684593878; camera_matrix.ptr<double>(0)[2] = 330.465249136366; camera_matrix.ptr<double>(1)[1] = 645.594637768319; camera_matrix.ptr<double>(1)[2] = 275.770884058043; camera_matrix.ptr<double>(2)[2] = 1.0f; //相机的畸变参数矩阵 distortion_coefficients = cv::Mat(5, 1, CV_64FC1, cv::Scalar::all(0)); distortion_coefficients.ptr<double>(0)[0] = 0.0169980568389292; distortion_coefficients.ptr<double>(1)[0] = 0.0219922707832352; distortion_coefficients.ptr<double>(2)[0] = -0.0411838784394819; distortion_coefficients.ptr<double>(3)[0] = -0.000111144332938787; distortion_coefficients.ptr<double>(4)[0] = -0.0411838784394819; //最重要的一个函数！ //rvec为旋转矩阵，tvec为平移矩阵 solvePnP(p4p_3, p4p_2, camera_matrix, distortion_coefficients, rvec, tvec, false, SOLVEPNP_ITERATIVE); double tx = tvec.ptr<double>(0)[0]; double ty = tvec.ptr<double>(0)[1]; double tz = tvec.ptr<double>(0)[2]; return Point3f(tx, ty, tz); } #endif // !mainOfPNP

分析：

整个函数想实现的功能是 ***获得我所设的世界坐标中的原点的坐标*** 比如在这个工程中，我设装甲板中心点为原点坐标，通过测量装甲板大小获得灯条上下四个点的坐标来与图像中坐标一一对应，通过solvepnp我就可以获得在相机坐标系下原点的坐标。