#include "PnP.h"


class PnPInfo {

};


//Point2f reproject(Vector3d& xyz)
//{
//	Matrix3d mat_intrinsic;
//	cv2eigen(intrinsic, mat_intrinsic);
//	//(u,v,1)^T = (1/Z) * K * (X,Y,Z)^T
//	auto result = (1.f / xyz[2]) * mat_intrinsic * (xyz);//解算前进行单位转换
//	return Point2f(result[0], result[1]);
//}

//pnp位姿解算
void PNP(const cv::Mat & img, std::vector<cv::Point2f> points , cv::Mat cam, cv::Mat dis){

	//给PNP参考的世界坐标点
	std::vector<cv::Point3f> points_world;

	//顺时针的世界坐标
	points_world = {
	{-161.5, 52.0, 0.0},
	{161.5, 52.0, 0.0},
	{167.5, -45.0, 0.0},
	{-167.5, -45.0, 0.0},
	{0.0, 0.0, 0.0} };

	cv::Mat rVec = cv::Mat::zeros(3, 1, CV_64FC1);
	cv::Mat tVec = cv::Mat::zeros(3, 1, CV_64FC1);

	if(points.size() > 0)
	solvePnP(points_world, points, cam, dis, rVec, tVec, false, cv::SOLVEPNP_IPPE);


	// 提取旋转向量的分量
	double xr = rVec.at<double>(0);
	double yr = rVec.at<double>(1);
	double zr = rVec.at<double>(2);

	// 提取平移向量的分量
	double xt = tVec.at<double>(0);
	double yt = tVec.at<double>(1);
	double zt = tVec.at<double>(2);

	// 构造旋转向量和平移向量的字符串表示
	std::string rvec_str = "rotation_vector: (" + std::to_string(xr) + ", " + std::to_string(yr) + ", " + std::to_string(zr) + ")";
	std::string tvec_str = "translation_vector: (" + std::to_string(xt) + ", " + std::to_string(yt) + ", " + std::to_string(zt) + ")";

	putText(img, rvec_str, cv::Point2f(5, 25), cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(255,255,255), 1);
	putText(img, tvec_str, cv::Point2f(5, 50), cv::FONT_HERSHEY_COMPLEX, 0.5, cv::Scalar(255, 255, 255), 1);
	

}