|
|
|
|
|
/*********版权所有（C）2020，武汉高德红外股份有限公司***************************************
|
|
|
* 文件名称： Arith_VehicleCoordModule.h
|
|
|
* 文件标识： 车载坐标转换模块标准化
|
|
|
* 内容摘要： 图像坐标系 <-> 平台地理坐标系，基于北-天-东 轴序
|
|
|
* 其它说明：
|
|
|
* 当前版本：2.0
|
|
|
* 创建作者：04046wcw
|
|
|
* 创建日期：2020/12/03
|
|
|
* 包含关系：
|
|
|
* *********************************************************************************
|
|
|
* 以前版本：1.1
|
|
|
* 作 者：陈毅
|
|
|
* 完成日期：2014年9月23日
|
|
|
*****************************************************************************************/
|
|
|
#ifndef Arith_VehicleCoordModule_h__
|
|
|
#define Arith_VehicleCoordModule_h__
|
|
|
|
|
|
|
|
|
/*
|
|
|
// 坐标系示意图
|
|
|
y x（车体前进方向）
|
|
|
▕
|
|
|
▕ ╱
|
|
|
▕ ╱
|
|
|
▕-——————————z
|
|
|
|
|
|
*/
|
|
|
|
|
|
|
|
|
// 标定参数：安装误差
|
|
|
extern double g_YawBias;
|
|
|
extern double g_PitchBias;
|
|
|
extern double g_RollBias;
|
|
|
extern double g_XBias;
|
|
|
extern double g_YBias;
|
|
|
extern double g_ZBias;
|
|
|
|
|
|
|
|
|
// 椭球体定义
|
|
|
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);
|
|
|
|
|
|
|
|
|
#ifndef _ARITH_GLOBAL_
|
|
|
#define ANGLE(x) (x / 3.141592653589793 * 180)
|
|
|
#define RADIAN(x) (x / 180 * 3.141592653589793)
|
|
|
#define ABS(x) ((x) < 0 ? -(x) : (x)) //取x的绝对值
|
|
|
#define MIN(a, b) ((a) < (b) ? (a) : (b)) //取a、b两者中的最小值
|
|
|
#define MAX(a, b) ((a) > (b) ? (a) : (b)) //取a、b两者中的最大值
|
|
|
#endif
|
|
|
|
|
|
#ifndef _POINTXYZ_
|
|
|
#define _POINTXYZ_
|
|
|
// 空间直角坐标系,也可以看做向量
|
|
|
typedef struct tgPointXYZ {
|
|
|
double X;
|
|
|
double Y;
|
|
|
double Z;
|
|
|
}PointXYZ;//单位：米
|
|
|
#endif
|
|
|
|
|
|
|
|
|
#ifndef _POLE_
|
|
|
#define _POLE_
|
|
|
//极坐标系
|
|
|
typedef struct tgPointPole {
|
|
|
double alpha; // 俯仰角
|
|
|
double beta; // 方位角
|
|
|
double distance; // 极径
|
|
|
}Pole;//单位：°
|
|
|
#endif
|
|
|
|
|
|
#ifndef _RPY_
|
|
|
#define _RPY_
|
|
|
//RPY姿态角
|
|
|
typedef struct tgEulerRPY {
|
|
|
double fRoll;//横滚角
|
|
|
double fPitch;//俯仰角
|
|
|
double fYaw;//方位角
|
|
|
}EulerRPY;//单位：°
|
|
|
#endif
|
|
|
|
|
|
|
|
|
#ifndef _POINTBLH_
|
|
|
#define _POINTBLH_
|
|
|
typedef struct tgPointBLH
|
|
|
{
|
|
|
double B; // 纬度
|
|
|
double L; // 经度
|
|
|
double H; // 高程
|
|
|
}PointBLH; // 地理坐标系 //单位：°
|
|
|
#endif
|
|
|
|
|
|
|
|
|
void Coord_Test();
|
|
|
|
|
|
|
|
|
// 设置标定参数
|
|
|
void setBiasPara(double YawBias, double PitchBias, double RollBias, double deltaX, double deltaY, double deltaZ);
|
|
|
|
|
|
// 获取当前标定参数
|
|
|
void getBiasPara(double *YawBias, double *PitchBias, double *RollBias, double *deltaX, double *deltaY, double *deltaZ);
|
|
|
|
|
|
|
|
|
|
|
|
// 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);
|
|
|
|
|
|
|
|
|
|
|
|
// IR370A-H补充接口
|
|
|
|
|
|
//光电镜面直角坐标 转 平台地理直角坐标 的 旋转矩阵。如果考虑平移需引入齐次坐标，不方便使用，这里简化处理
|
|
|
void getCarNUEXYZFromEOCamXYZ_Mat(double mYaw,double mPitch,double mRoll,double servoAlpha, double servoBeta,double* Mat);
|
|
|
|
|
|
// 相机坐标Z轴正方向(平行于图像X方向)在大地系下的偏转角度
|
|
|
Pole getCamZaxisNUERota(double mYaw,double mPitch,double mRoll,double servoAlpha, double servoBeta);
|
|
|
|
|
|
///////////////////////////////////////目标上报//////////////////////////////////////////////////////////
|
|
|
// 光电镜面极坐标 转 平台地理极坐标
|
|
|
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);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// 矩阵乘法
|
|
|
void MultiMat(double *A, double *B, double *AB, int m, int n, int p);
|
|
|
// M1(3*3) * M2(3*3)
|
|
|
void MultiMat33(double *A, double *B, double *AB);
|
|
|
// M1(3*3) * M2(3*3) * M3(3*3)
|
|
|
void MultiMat333(double *A, double *B, double *C, double *ABC);
|
|
|
|
|
|
// 坐标(矢量)旋转: M1(3*3) * M2(3,1)
|
|
|
PointXYZ RtPoint(double *M, PointXYZ Point);
|
|
|
|
|
|
// 坐标(矢量)归一化
|
|
|
PointXYZ NormPoint(PointXYZ Point);
|
|
|
|
|
|
|
|
|
// 绕X轴旋转矩阵
|
|
|
void Rx(double omega, double *Opp);
|
|
|
|
|
|
// 绕Y轴旋转矩阵
|
|
|
void Ry(double omega, double *Opp);
|
|
|
|
|
|
// 绕Z轴旋转矩阵
|
|
|
void Rz(double omega, double *Opp);
|
|
|
|
|
|
// 镜面反射矩阵
|
|
|
void RMirror(PointXYZ normalVec, double *Opp);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#endif // Arith_VehicleCoordModule_h__
|
|
|
|
|
|
|
