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