S3315_RK3588/NeoTracker/src/Common/Arith_CoordModule.h

/*********版权所有（C）2022，武汉高德红外股份有限公司***************************************
* 文件名称： Arith_CoordModule.h
* 文件标识： 车载坐标转换模块标准化
* 内容摘要： 图像坐标系 <-> 平台地理坐标系，基于北-天-东 轴序
* 其它说明： 将带有像旋等特殊内参的设备进一步抽象适用。
		   经过此次升级，坐标转换程序适应性更强，出现新的内参关系时仅需要修改对应部分的转换函数而不必再对搜跟算法本身进行修改。
		   【注意】当前框架物方仍使用极坐标描述，距离真正的物方空间建航仍有一步之遥：距离信息。这种描述方式在高仰角时存在天然缺陷，会导致速度估计不正确，
		   唯一的出路是改用XYZ直角系建航，但并非所有系统都有测距信息，如何改进待后人研究。
		   ！！！！！方位角度范围 【-180,180】，俯仰角度范围 【-90，,90】！！！！！！！
* 当前版本：3.0
* 创建作者：04046wcw
* 创建日期：2022/12/03
* 包含关系：
* *********************************************************************************
* * 以前版本：2.0
* 作    者：04046wcw
* 完成日期：2020/12/03

* 以前版本：1.1
* 作    者：陈毅
* 完成日期：2014年9月23日

*****************************************************************************************/
#ifndef Arith_CoordModule_h__
#define Arith_CoordModule_h__

#pragma once


#include "Arith_SysStruct.h"
/*
// 坐标系示意图
y    x（前进方向）
▕
▕  ╱
▕ ╱
▕-——————————z



// 定义：
惯性系：建航坐标系，也是本地地理系（导航系）（本地 NUE）等
设备坐标系：原伺服系，随着伺服概念的弱化，定义为设备坐标系更为合理。//20220930  04046wcw

后续计划：将原伺服模块逐步从算法中移除  原先设计已经不满足需要了

*/





// 椭球体定义
const double a = 6378137;
const double f = 1 / 298.257222101;
const double b = ((1 - f)*a);
const double ee = (1 - (1 - f)*(1 - f));
const double epep = (a*a / b / b - 1);





////////////////////////////光电坐标转换统一接口（20220930）/////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////

// 从惯性系（建航坐标系）到  像坐标系
POINT32F getImagePosFromStablePole(Pole stablePole,CameraParam cam, ServoParam servoInfo, EulerRPY att, DeviceSetupError err);

// 从像坐标系 到 惯性系（建航坐标系）
Pole getStablePoleFromImagePos(POINT32F imagePos,CameraParam cam, ServoParam servoInfo, EulerRPY att, DeviceSetupError err);




// 在扫描区间判断
BBOOL bTargetInScanFov(ANGLE32F targetPos, Pole scanNueRange1, Pole scanNueRange2);

// 惯性系到设备系
Pole getDevicePoleFromStablePole(Pole stablePole, EulerRPY att,DeviceSetupError err);

// 设备系转像方坐标
POINT32F getImagePosFromDevicePole(Pole devicePole,CameraParam cam, ServoParam servoInfo);

// 像方转设备系
Pole getDevicePoleFromImagePos(POINT32F imagePos,CameraParam cam, ServoParam servoInfo);

// 设备系到惯性系
Pole getStablePoleFromDevicePole(Pole devicePole, EulerRPY att, DeviceSetupError err);

////////////////////////////光电坐标转换统一接口/////////////////////////////////





// // S726二维指向镜版本          by wcw04046 @ 2021/12/03
// Pole getPointMirrorServoPoleFromImageXY(int Col,int Row,int nWidth,int nHeight,
// 										float fServoA,float fServoB,float fLen,float pxSizeOfum);


// void getPointMirrorImageXYFromServoPole(int* Col,int* Row,int nWidth,int nHeight,float WAlaph,float WBeta,
// 										float fServoA,float fServoB,float fLen,float pxSizeOfum);


// S731成像模型


// s731_v2相对于v1 ，伺服方位正方向相反。因此在v1的模型上方位角加负号即可。
// 本质上v1时未理解到设备测试状态与装机状态上下颠倒，从而使用了错误了设备坐标系，v1伺服也没有理解到，正方向也与装机状态相反。v2的伺服改过来了@20240221
// 探测器方面v1 v2状态一致，可以说都装反了  设备测试状态下图像应该是倒立的，所以V1、V2中对二者都取了像方 Y=-Y
// 总体上v1 v2只有伺服方位正方向改了状态。


// 成像模型反射矩阵  【像方->物方】（含主反、快反)
void PMS_MultiRefMat(float fMainAz, float fMainAPt, float fFSMAz, float fFSMPt, double* refmat);
// 单点投影：像方到物方角度转换
Pole PMS_ProjectPixelToWorld(int x, int y, float fMainAz, float fMainAPt, float fFSMAz, float fFSMPt, float f_px, int cx, int cy);
Pole PMS_ProjectPixelToWorld(int x, int y, double* refmat, float f_px, int cxx, int cy);// 已知矩阵直接转
// 单点投影：像方到物方矢量
//PointXYZ PMS_ProjectPixelToWorldVec(int x, int y, double* refmat, float f_px, int cx, int cy);// 已知矩阵直接转
// 单点投影：从物方角度到像方
PointXYZ PMS_ProjectWorldToPixel(float fAz, float fPt, double* refmat, float f_px, int cx, int cy);// 已知矩阵直接转，大部分时候可以节约计算
// 单点投影：从物方矢量到像方
//PointXYZ PMS_ProjectWorldVecToPixel(PointXYZ worldVec, double* refmat, float f_px, int cx, int cy);







///////////////////////////////////////目标上报//////////////////////////////////////////////////////////
// 光电镜面极坐标 转 平台地理极坐标
Pole getCarNUEPoleFromEOCamPole(Pole targetCamPole,	double mYaw,double mPitch,double mRoll,double servoAlpha, double servoBeta);

// 光电镜面直角坐标 转 光电伺服直角坐标
PointXYZ getEOServoXYZFromEOCamXYZ(PointXYZ targetCamXYZ,double alaph, double beta);

// 光电伺服直角坐标 转 光电基准直角坐标
PointXYZ getEOBaseXYZFromEOServoXYZ(PointXYZ targetServoXYZ, double eYaw, double ePitch, double eRoll);

// 光电基准直角坐标 转 平台直角坐标
PointXYZ getCarXYZFromEOBaseXYZ(PointXYZ targetEOBaseXYZ, double deltaX, double deltaY, double deltaZ);

// 平台直角坐标 转 平台地理直角坐标
PointXYZ getCarNUEXYZFromCarXYZ(PointXYZ targetCarXYZ,double mYaw, double mPitch, double mRoll);
//////////////////////////////////////////////////////////////////////////////////////////////////////////////



///////////////////////////////////////目标导引//////////////////////////////////////////////////////////
Pole getEOServoPoleFromCarNUEPole(Pole targetCarNUEPole, Pole* targetEOCamPole,double mYaw, double mPitch, double mRoll, double servoAlpha, double servoBeta);

// 平台地理直角坐标 转 平台直角坐标 
PointXYZ getCarXYZFromCarNUEXYZ(PointXYZ targetCarNUEXYZ, double mYaw, double mPitch, double mRoll);

// 平台直角坐标 转 光电基准直角坐标
PointXYZ getEOBaseXYZFromCarXYZ(PointXYZ targetCarXYZ, double deltaX, double deltaY, double deltaZ);
 
// 光电基准直角坐标 转 光电伺服直角坐标 
PointXYZ getEOServoXYZFromEOBaseXYZ(PointXYZ targetEOBaseXYZ, double eYaw, double ePitch, double eRoll);

// 光电伺服直角坐标 转 光电镜面直角坐标
PointXYZ getEOCamXYZFromEOServoXYZ(PointXYZ targetEOServoXYZ, double alaph, double beta);



/////////////////////////////////////////大地坐标相关///////////////////////////////////////////////////////

//大地坐标转地球空间直角坐标
PointXYZ getXYZFromBLH(PointBLH BLH);

//地球空间直角坐标-->大地坐标
PointBLH getBLHFromXYZ(PointXYZ pointXYZ);

//由NUE直角坐标变换到CGCS直角坐标
PointXYZ getCGCSXYZFromNUEXYZ(PointXYZ targetPointXYZ, PointXYZ selfPointXYZ);

//由CGCS直角坐标变换到NUE直角坐标
PointXYZ getNUEXYZFromCGCSXYZ(PointXYZ targetPointXYZ, PointXYZ selfPointXYZ);


////////////////////////////////////////////////////////////////////////////////

// 直角坐标转极坐标（右手系）
Pole getPoleFromXYZ(PointXYZ targetXYZ);

// 极坐标转直角坐标（右手系）
PointXYZ getXYZFromPole(Pole targetPole);








#endif // Arith_VehicleCoordModule_h__