StitchVideo/stitch/src/Arith_GeoSolver.h

/*********版权所有（C）2025，武汉高德红外股份有限公司***************************************
* 文件名称： Arith_GeoStitcher.h
* 文件标识：吊舱视频帧外参拼接
* 内容摘要：基于机载下视外部参数的几何校正,支持独立工作,也可以作为特征拼接的外参初始值
* 其它说明：【注意：为了便于推导，局部地理坐标系采用的是东-北-地坐标系，使用大地坐标转换工具时注意转换。】
* 当前版本：V0.5
* 创建作者：04046wcw
* 创建日期：2025/01/15
* 包含关系：
*****************************************************************************************/
#pragma once
#include "StitchStruct.h"



// 像方-物方转换关系
struct TForm
{
    cv::Mat R;
    cv::Mat T;
};


// 投影关系,描述反算过程
struct Proj
{
    TForm tf_p2g;//从帧到地理坐标系的Rt矩阵
    TForm tf_g2p;//从地理坐标系到帧的Rt矩阵
};




class GeoSolver 
{
public:
    // 构造函数
    GeoSolver();
	~GeoSolver();

    // 建立全景图与归一化地理系的H矩阵，解析出H
    cv::Mat findHomography(FrameInfo info);

    // 测试
    cv::Mat findHomography2(FrameInfo info);

    // 设置起始拼接点外参，返回当前光轴大地指向
    ANGLE32F SetOriginPoint(FrameInfo info);

    // 根据H计算原始像方的经纬度
    PointBLH getBLHFromFrame(cv::Mat H, cv::Point2f ptInFrame);

	// 计算局部地理系下目标的经纬度
    PointBLH getBLHFromGeo(cv::Point2f ptInGeo);

	// 经纬度转换为局部地理系坐标
    cv::Point2f getGeoFromBLH(PointBLH ptPos);


private:


    // 计算当前帧像方-地理坐标系R t（反投影关系）
    Proj AnlayseTform(FrameInfo info);

    // 帧像方-地理坐标
    cv::Point2f Trans_uv2Geo(cv::Point2f pos_frame, TForm form);

    // 地理坐标-帧像方
    cv::Point2f Trans_Geo2uv(cv::Point2f pos_geo, TForm form);


    // 平移矩阵，以初始化点为基准，计算当前位置在初始点的地理坐标，那么当前帧所有点的坐标做此平移
    cv::Mat Mat_TransENGMove(FrameInfo info);

    // 机体ENG(东北地)到像方的 旋转矩阵
    cv::Mat Mat_TransENG2uv(FrameInfo info);


private:

    PointXYZ originPoint;//成图初始点的地心地固坐标，作为拼接参考

    FrameInfo origininfo;
};

// 多边形面积
double polygonArea(const vector<cv::Point2f>& points);

// 计算两个四边形的 IOU
double computeQuadrilateralIOU(const vector<cv::Point2f>& quad1, const vector<cv::Point2f>& quad2);

// H映射点
cv::Point2f warpPointWithH(cv::Mat H,cv::Point2f srcPt);

// H映射多边形，按照顺时针
vector<cv::Point2f> warpRectWithH(cv::Mat H,cv::Size size);

// 坐标系统转换
PointXYZ getNUEFromENG(PointXYZ p);
PointXYZ getENGFromNUE(PointXYZ p);