OpenCV解决PnP问题

#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include “aruco.h“
using namespace std;
using namespace cv;



const Mat  intrinsic_matrix = （Mat_（3 3）
                               << 868.2187  0         345.93152
                                  0         870.4765  252.03741
                                  0         0         1）;

//k1k2p1p2k3
const Mat  distCoeffs = （Mat_（5 1） << 0.17153 0.06634 0.00465 0.00868 0）;

//相机畸变参数
const Mat  arucodistCoeffs = （Mat_（1 5） << 0 0 0 0 0）;//

//判断solvePnPRansac是否完成
bool PnPRansac;

int main（int args char *argv[]）
{
	//图像物理坐标
	//vector > projectedPoints;
	vector imgpoint;

	Point3f pointimg0 pointimg1 pointimg2 pointimg3;

	pointimg0 = cv::Point3f（-0.046f -0.046f 0）;
	pointimg1 = cv::Point3f（+0.046f -0.046f 0）;
	pointimg2 = cv::Point3f（+0.046f +0.046f 0）;
	pointimg3 = cv::Point3f（-0.046f +0.046f 0）;


	imgpoint.push_back（cv::Point3f（-0.046f -0.046f 0））;
	imgpoint.push_back（cv::Point3f（+0.046f -0.046f 0））;
	imgpoint.push_back（cv::Point3f（+0.046f +0.046f 0））;
	imgpoint.push_back（cv::Point3f（-0.046f +0.046f 0））;


	//获取相机
	VideoCapture cap（1）;
        
	Mat frameframecopy;
	//设置字典
	cv::aruco::Dictionary dictionary = cv::aruco::getPredefinedDictionary（cv::aruco::DICT_6X6_250）;

	while （true）
	{
		cap>>frame;
		frame.copyTo（framecopy）;

		vector ids;
		vector > corners;
		vector > outArry;
		//检测markers（待检测marker的图像，字典对象，角点列表，marker的id列表）
		cv::aruco::detectMarkers（framecopy dictionary corners ids）;
		if （ids.size（） > 0）//检测到marker
		{
			cout<<“numberr of ids:  “<			for（int i=0;i			{
				//输出每一个id					
				cout<<“ids:  “<			}
			double x= （corners[0][0].x+corners[0][1].x+corner