StitchVideo/stitch/src/Arith_GeoSolver.cpp

#include "Arith_GeoSolver.h"
#include "Arith_UnderStitch.h"
#include "Arith_Utils.h"
#include "Arith_CoordModule.h"
#include "Arith_SysStruct.h"
//#define GLOG_USE_GLOG_EXPORT

using namespace cv;

GeoSolver::GeoSolver()
{
}

GeoSolver::~GeoSolver()
{
}



//cv::Point2f GeoSolver::project(cv::Point2f pos_pan, Proj m_Proj, PanInfo pan)
//{
//	cv::Point2f pos_geo = Trans_pan2Geo(pos_pan, pan);
//	cv::Point2f pos_frame = Trans_Geo2uv(pos_geo, m_Proj.tf_g2p);
//	return pos_frame;
//}
//
//cv::Point2f GeoSolver::back_project(cv::Point2f pos_frame, Proj m_Proj, PanInfo pan)
//{
//	cv::Point2f pos_geo = Trans_uv2Geo(pos_frame, m_Proj.tf_p2g);
//
//	cv::Point2f pos_pan = Trans_Geo2pan(pos_geo, pan);
//
//	return pos_pan;
//}

void GeoSolver::SetOriginPoint(FrameInfo info)
{
	originCGCSPoint = getXYZFromBLH(info.craft.stPos);
	origininfo = info;
	return;
}

PointXYZ GeoSolver::GetOriginPoint()
{
	return originCGCSPoint;
}

int GeoSolver::GetOriginDHeight()
{
	return origininfo.nEvHeight;
}


PointBLH GeoSolver::getBLHFromFrame(cv::Mat H, cv::Point2f pt)
{
	cv::Point2f pt_Geo = warpPointWithH(H, pt);
	return getBLHFromGeo(pt_Geo);
}


PointBLH GeoSolver::getBLHFromGeo(cv::Point2f ptInGeo)
{
	PointXYZ ptNUE = { 0 };
	// 本模块使用的地理系是东(x)-北(y)-地(z),需要转换一下
	ptNUE.X = ptInGeo.y;
	ptNUE.Y = -origininfo.nEvHeight;
	ptNUE.Z = ptInGeo.x;
	PointBLH res = getBLHFromXYZ(getCGCSXYZFromNUEXYZ(ptNUE, originCGCSPoint));
	return res;
}

cv::Point2f GeoSolver::getGeoFromBLH(PointBLH ptPos)
{
	cv::Point2f ptInGeo = { 0 };
	PointXYZ ptNUE = { 0 }; 
	ptNUE = getNUEXYZFromCGCSXYZ(getXYZFromBLH(ptPos), originCGCSPoint);
	// 本模块使用的地理系是东(x)-北(y)-地(z),需要转换一下
	ptInGeo.x = ptNUE.Z;
	ptInGeo.y = ptNUE.X;

	return ptInGeo;
}

// cv::Point2f GeoSolver::Trans_uv2Geo(cv::Point2f pos_frame, TForm form)
// {
// 	Mat point = (Mat_<double>(3, 1) << pos_frame.x, pos_frame.y, 1);
// 	Mat result = form.R * point;
// 	// 转局部地理系
// 	double warpedX = result.at<double>(0, 0) / result.at<double>(2, 0);
// 	double warpedY = result.at<double>(1, 0) / result.at<double>(2, 0);
// 	// 平移到原点地理系
// 	warpedX += form.T.at<double>(0, 0);
// 	warpedY += form.T.at<double>(1, 0);

// 	return cv::Point2f(warpedX, warpedY);
// }

// cv::Point2f GeoSolver::Trans_Geo2uv(cv::Point2f pos_geo, TForm form_inv)
// {
// 	// 先平移到当前相机位置
// 	cv::Point2f pos_cam = pos_geo;
// 	pos_cam.x = pos_geo.x + form_inv.T.at<double>(0, 0);
// 	pos_cam.y = pos_geo.y + form_inv.T.at<double>(1, 0);

// 	Mat point = (Mat_<double>(3, 1) << pos_cam.x, pos_cam.y, 1);
// 	Mat result = form_inv.R * point;

// 	// 转像方
// 	double warpedX = result.at<double>(0, 0) / result.at<double>(2, 0);
// 	double warpedY = result.at<double>(1, 0) / result.at<double>(2, 0);

// 	return cv::Point2f(warpedX, warpedY);
// }


Mat GeoSolver::Mat_TransENGMove(FrameInfo info)
{
	PointXYZ ptCurr = getXYZFromBLH(info.craft.stPos);
	PointXYZ diff = getNUEXYZFromCGCSXYZ(ptCurr, originCGCSPoint);

	Mat move = Mat::zeros(3, 1, CV_64F);

	move.at<double>(0, 0) = diff.Z;
	move.at<double>(1, 0) = diff.X;
	move.at<double>(2, 0) = -diff.Y;
	return move;
}

Mat GeoSolver::Mat_TransENG2uv(FrameInfo info)
{
	//从地理坐标系转像素坐标
	//[u,v,1]'=Z*M*[X,Y,DH]' = Z*M*[1,0,0;0,1,0;0,0,DH]'*[X,Y,1]'
	//[u,v,1]'=Z*M(内参)*M(alaph)*M(beta)*M(roll)*M(pitch)*M(yaw)*[X,Y,DH]

	// 深度矩阵
	Mat M_het = (Mat_<double>(3, 3) << 1, 0, 0,
		0, 1, 0,
		0, 0, info.nEvHeight
		);

	Mat M = Mat_GetCamK(info) * Mat_TransENG2Cam(info) * M_het;

	return M;
}

using namespace std;
cv::Mat GeoSolver::Mat_TransENG2Cam(FrameInfo info)
{

	float yaw = info.craft.stAtt.fYaw;
	Mat M_yaw = (Mat_<double>(3, 3) << cosd(yaw), -sind(yaw), 0,
		sind(yaw), cosd(yaw), 0,
		0, 0, 1
		);

	float pit = info.craft.stAtt.fPitch;
	Mat M_pitch = (Mat_<double>(3, 3) << 1, 0, 0,
		0, cosd(pit), -sind(pit),
		0, sind(pit), cosd(pit)
		);

	/* 1    0      0
	0    cos    sin
	0   -sin    cos
	*/

	float roll = info.craft.stAtt.fRoll;
	Mat M_roll = (Mat_<double>(3, 3) << cosd(roll), 0, sind(roll),
		0, 1, 0,
		-sind(roll), 0, cosd(roll)
		);


	float beta = info.servoInfo.fServoAz;

	Mat M_beta = (Mat_<double>(3, 3) << cosd(beta), -sind(beta), 0,
		sind(beta), cosd(beta), 0,
		0, 0, 1
		);
	
	
	

	float alaph = info.servoInfo.fServoPt;

	Mat M_alaph = (Mat_<double>(3, 3) << 1, 0, 0,
		0, cosd(alaph), -sind(alaph),
		0, sind(alaph), cosd(alaph)

		);

	Mat M =M_alaph * M_beta * M_roll * M_pitch * M_yaw;

	return M;
}

cv::Mat GeoSolver::Mat_GetCamK(FrameInfo info)
{
	// 内参
	FLOAT32 fd = info.camInfo.nFocus / info.camInfo.fPixelSize * 1000;

	Mat M_cam = (Mat_<double>(3, 3) << fd, 0, info.nWidth / 2,
		0, -fd, info.nHeight / 2,
		0, 0, 1
		);

	return M_cam;
}

RtK GeoSolver::AnlayseRtK(FrameInfo info)
{
	RtK rtk;
	rtk.R = Mat_TransENG2Cam(info);
	rtk.T = Mat_TransENGMove(info);
	rtk.K = Mat_GetCamK(info);
	return rtk;
}

cv::Mat TransRtKToH(RtK rtk, double Depth)
{
	// 深度矩阵
	Mat M_het = (Mat_<double>(3, 3) << 1, 0, 0,
		0, 1, 0,
		0, 0, Depth
		);

	// cv::Mat R_p2g = (rtk.K * rtk.R * M_het).inv();
	// //cv::Mat R_p2g = R_g2p.inv();
	// cv::Mat t_MAT = rtk.T;

	// double R[9] = { 0 };
	// double T[3] = { 0 };

	// memcpy(R, R_p2g.data, sizeof(double) * 9);
	// memcpy(T, t_MAT.data, sizeof(double) * 3);

	// Mat H = (Mat_<double>(3, 3) << (R[0] + T[0] * R[6]), (R[1] + T[0] * R[7]), (R[2] + T[0] * R[8]),
	// 	(R[3] + T[1] * R[6]), (R[4] + T[1] * R[7]), (R[5] + T[1] * R[8]),
	// 	R[6], R[7], R[8]);

	// // 归一化
	// H = H / H.at<double>(2, 2);

	// 地面法向量 n_w（向上方向，Z轴正方向，在地理坐标系中）
	cv::Mat n_w = (cv::Mat_<double>(3, 1) << 0, 0, 1);

	// 平面到相机的距离（有效高度）
	double d = Depth;

	cv::Mat I = cv::Mat::eye(3, 3, CV_64F);
	
	// 计算 (T_geo * n_w^T) / d
	cv::Mat TnT = (rtk.R * rtk.T *n_w.t()) / d;


	
	
	// 计算从地理坐标到像素坐标的单应性矩阵
	cv::Mat H_geo_to_pixel = rtk.K * (rtk.R - TnT) * M_het;

	cv::Mat H = H_geo_to_pixel.inv();

	// 归一化（使 H[2,2] = 1）
	H = H / H.at<double>(2, 2);

	return H.clone();
}

cv::Mat Transe3ToH(double* se3, cv::Mat K, double Depth)
{
	RtK rtk;
	cv::Rodrigues(cv::Mat(3, 1, CV_64FC1, se3), rtk.R);


	//cv::Mat w, u, vt;
	//cv::SVD::compute(rtk.R, w, u, vt);

	//// 核心：确保 u * vt 是精确的正交矩阵
	//// 而不是直接对R进行尺度归一化
	//rtk.R = u * vt;

	rtk.T = cv::Mat(3, 1, CV_64FC1, se3 + 3);

	rtk.K = K;

	return TransRtKToH(rtk, Depth);
}



// 根据R和t，解析H
// cv::Mat GeoSolver::findHomography(FrameInfo info)
// {
// 	cv::Mat Rmat = Mat_TransENG2uv(info).inv();
// 	cv::Mat Tmat = Mat_TransENGMove(info);

// 	double R[9] = { 0 };
// 	double T[3] = { 0 };

// 	memcpy(R, Rmat.data, sizeof(double) * 9);
// 	memcpy(T, Tmat.data, sizeof(double) * 3);

// 	Mat H = (Mat_<double>(3, 3) << (R[0] + T[0] * R[6]), (R[1] + T[0] * R[7]), (R[2] + T[0] * R[8]),
// 		(R[3] + T[1] * R[6]), (R[4] + T[1] * R[7]), (R[5] + T[1] * R[8]),
// 		R[6], R[7], R[8]);


// 	// 归一化
// 	H = H / H.at<double>(2, 2);


// 	return H;
// }

cv::Mat GeoSolver::findHomography(FrameInfo info)
{
	// 获取相机内参矩阵 K
	cv::Mat Kmat = Mat_GetCamK(info);
	
	// 获取从地理坐标系到相机坐标系的旋转矩阵 R_geo_to_cam
	cv::Mat R_geo_to_cam = Mat_TransENG2Cam(info);
	
	// 获取平移向量 T（从原点地理坐标系到当前相机位置地理坐标系）
	cv::Mat T_geo = Mat_TransENGMove(info);
	
	// 深度矩阵
	Mat M_het = (Mat_<double>(3, 3) << 1, 0, 0,
		0, 1, 0,
		0, 0, info.nEvHeight
		);

	// 地面法向量 n_w（向上方向，Z轴正方向，在地理坐标系中）
	cv::Mat n_w = (cv::Mat_<double>(3, 1) << 0, 0, 1);
	
	// 平面到相机的距离（有效高度）
	double d = info.nEvHeight;

	cv::Mat I = cv::Mat::eye(3, 3, CV_64F);
	
	// 计算 (T_geo * n_w^T) / d
	cv::Mat TnT = (R_geo_to_cam * T_geo *n_w.t()) / d;
	
	
	// 计算从地理坐标到像素坐标的单应性矩阵
	cv::Mat H_geo_to_pixel = Kmat * (R_geo_to_cam - TnT) * M_het;

	cv::Mat H = H_geo_to_pixel.inv();

	// 归一化（使 H[2,2] = 1）
	H = H / H.at<double>(2, 2);
	
	return H.clone();
}


cv::Mat GeoSolver::findHomography(FrameInfo info, RtK& rtk)
{
	rtk = AnlayseRtK(info);

	cv::Mat H = TransRtKToH(rtk,info.nEvHeight);

	return H;
}


// 计算多边形的面积
double polygonArea(const vector<cv::Point2f>& points)
{
	double area = 0.0;
	int n = points.size();
	for (int i = 0; i < n; i++) {
		int j = (i + 1) % n;
		area += points[i].x * points[j].y - points[j].x * points[i].y;
	}
	return abs(area) / 2.0;
}

// 计算两个四边形的 IOU
double computeQuadrilateralIOU(const vector<cv::Point2f>& quad1, const vector<cv::Point2f>& quad2)
{
	// 将四边形转换为多边形
	vector<Point2f> poly1 = quad1;
	vector<Point2f> poly2 = quad2;

	// 计算交集多边形
	vector<Point2f> intersectionPolygon;
	intersectConvexConvex(poly1, poly2, intersectionPolygon);

	// 计算面积
	double area1 = polygonArea(poly1);
	double area2 = polygonArea(poly2);
	double intersectionArea = polygonArea(intersectionPolygon);
	double unionArea = area1 + area2 - intersectionArea;

	// 计算 IOU
	if (unionArea == 0) return 0.0; // 避免除零错误
	return intersectionArea / unionArea;
}

cv::Point2f warpPointWithH(cv::Mat H, cv::Point2f srcPt)
{
	Mat point = (Mat_<double>(3, 1) << srcPt.x, srcPt.y, 1);
	Mat result = H * point;
	// 转局部地理系
	double warpedX = result.at<double>(0, 0) / result.at<double>(2, 0);
	double warpedY = result.at<double>(1, 0) / result.at<double>(2, 0);

	return cv::Point2f(warpedX,warpedY);
}



vector<cv::Point2f> warpRectWithH(cv::Mat H,cv::Size size)
{
	vector<cv::Point2f> _res;
	_res.push_back(warpPointWithH(H, cv::Point2f(0,0)));
	_res.push_back(warpPointWithH(H, cv::Point2f(size.width,0)));
	_res.push_back(warpPointWithH(H, cv::Point2f(size.width,size.height)));
	_res.push_back(warpPointWithH(H, cv::Point2f(0,size.height)));
	return _res;
}

cv::Rect2f warpRectWithH_2Rect(cv::Mat H, cv::Size size)
{
	vector<cv::Point2f> dstCorners = warpRectWithH(H, size);

	// 找到变换后图像的边界
	float minX = MIN(MIN(dstCorners[0].x, dstCorners[1].x), MIN(dstCorners[2].x, dstCorners[3].x));
	float minY = MIN(MIN(dstCorners[0].y, dstCorners[1].y), MIN(dstCorners[2].y, dstCorners[3].y));
	float maxX = MAX(MAX(dstCorners[0].x, dstCorners[1].x), MAX(dstCorners[2].x, dstCorners[3].x));
	float maxY = MAX(MAX(dstCorners[0].y, dstCorners[1].y), MAX(dstCorners[2].y, dstCorners[3].y));


	// 变换后结果的rect
	cv::Rect2f roi(minX, minY, maxX - minX, maxY - minY);

	return roi;
}


cv::Point2f warpPointWithse3(const double* se3, double depth, cv::Mat K, cv::Point2f srcPt)
{
    Mat point = (Mat_<double>(3, 1) << srcPt.x, srcPt.y, 1.0);

	// 轴角转R
	cv::Mat rvec = (Mat_<double>(3, 1) << -se3[0], -se3[1], -se3[2]);
	cv::Mat tvec = (Mat_<double>(3, 1) << se3[3], se3[4], se3[5]);
	cv::Mat R;
	cv::Rodrigues(rvec, R);


	// 深度矩阵
	Mat M_het_inv = (Mat_<double>(3, 3) << 1, 0, 0,
		0, 1, 0,
		0, 0, 1.0/depth
		);

	// 转地理系
	Mat result = M_het_inv * R * K.inv() * point;


	// 说明ceres::AngleAxisRotatePoint有效，结果等效于cv::Rodrigues
	//double resP[3] = { 0 };
	//cv::Mat pt = (K.inv() * point);
	//ceres::AngleAxisRotatePoint((double*)rvec.data, (double*)pt.data, (double*)resP);
	//result = M_het_inv * cv::Mat(3, 1, CV_64FC1, resP);


	// 转局部地理系
	double warpedX = result.at<double>(0, 0) / result.at<double>(2, 0);
	double warpedY = result.at<double>(1, 0) / result.at<double>(2, 0);

	warpedX += tvec.at<double>(0, 0);
	warpedY += tvec.at<double>(1, 0);


	return cv::Point2f(warpedX,warpedY);
}