上传文件至 ''

Arith_VehicleCoordModule.cpp

@@ -0,0 +1,890 @@
+#include "Arith_VehicleCoordModule.h"
+#include <cmath>
+
+
+
+// 标定参数：安装误差
+double g_YawBias = 0;
+double g_PitchBias = 0;
+double g_RollBias = 0;
+double g_XBias = 0;
+double g_YBias = 0;
+double g_ZBias = 0;
+
+
+void setBiasPara(double YawBias, double PitchBias, double RollBias, double deltaX, double deltaY, double deltaZ)
+{
+	g_YawBias = YawBias;
+	g_PitchBias = PitchBias;
+	g_RollBias = RollBias;
+	g_XBias = deltaX;
+	g_YBias = deltaY;
+	g_ZBias = deltaZ;
+}
+
+void getBiasPara(double * YawBias, double * PitchBias, double * RollBias, double * deltaX, double * deltaY, double * deltaZ)
+{
+	*YawBias = g_YawBias;
+	*PitchBias =g_PitchBias;
+	*RollBias = g_RollBias;
+	*deltaX = g_XBias;
+	*deltaY=  g_YBias;
+	*deltaZ = g_ZBias;
+}
+
+
+// 像方到物方转换
+Pole getPointMirrorServoPoleFromImageXY(int Col,int Row,int nWidth,int nHeight,
+										float fServoA,float fServoB,float fLen,float pxSizeOfum)
+{
+	// by wcw04046 @ 2021/12/04
+	// 注意:在通常成像系统中，伺服读数基本等价于伺服系下角度（需要考虑正割补偿）
+	// 在指向镜成像系统中，伺服读数与伺服系下角度完全不等价 fServoAz fServoPt 与目标在伺服系位置非线性 
+	// 伺服坐标系角度
+	Pole ServoPole;
+
+	// 像素焦距
+	float f_0 = fLen/pxSizeOfum * 1000;
+
+	// 等效像方入射矢量
+	PointXYZ CamVec;
+	CamVec.X = f_0;
+	CamVec.Y = -nHeight/2 + Row;
+	CamVec.Z = (-nWidth/2 + Col);
+
+	// 反射镜能将正前方(x轴)的目标反射到成像系统光轴(y轴)上时，角编码器为零位
+
+	// 平面镜1零位的法向量
+	PointXYZ N_01;
+	N_01.X = 1/sqrt(2.0);
+	N_01.Y = 1/sqrt(2.0);
+	N_01.Z = 0;
+
+	double Sx[9] = {1,0,0,
+	0,cos(RADIAN(fServoA)),-sin(RADIAN(fServoA)),
+	0,sin(RADIAN(fServoA)),cos(RADIAN(fServoA))};
+
+	double Sy[9] = {cos(RADIAN(fServoB)),0,-sin(RADIAN(fServoB)),
+	0,1,0,
+	sin(RADIAN(fServoB)),0,cos(RADIAN(fServoB))};
+
+	PointXYZ N_1 = RtPoint(Sx,RtPoint(Sy,N_01));
+	// 镜面反射矩阵
+	//	M = eye(3)-(2*N*N');
+	double M1[9] = {0};
+	RMirror(N_1,M1);
+
+
+	// 平面镜2零位的法向量
+	PointXYZ N_02;
+	N_02.X = -1/sqrt(2.0);
+	N_02.Y = -1/sqrt(2.0);
+	N_02.Z = 0;
+	PointXYZ N_2 = RtPoint(Sx,N_02);
+	double M2[9] = {0};
+	RMirror(N_2,M2);
+
+	// 平面镜3零位的法向量
+	PointXYZ N_03;
+	N_03.X = 1/sqrt(2.0);
+	N_03.Y = -1/sqrt(2.0);
+	N_03.Z = 0;
+	PointXYZ N_3 = RtPoint(Sx,N_03);
+	double M3[9] = {0};
+	RMirror(N_3,M3);
+
+	// 平面镜4零位的法向量
+	PointXYZ N_04;
+	N_04.X = -1/sqrt(2.0);
+	N_04.Y = 1/sqrt(2.0);
+	N_04.Z = 0;
+	PointXYZ N_4 = RtPoint(Sx,N_04);
+	double M4[9] = {0};
+	RMirror(N_4,M4);
+
+	// 等效物方出射矢量
+	PointXYZ worldPosVec = RtPoint(M1,RtPoint(M2,RtPoint(M3,RtPoint(M4,CamVec))));
+	PointXYZ worldPosVec_1 = NormPoint(worldPosVec);
+
+    ServoPole.alpha = ANGLE(asin(worldPosVec_1.Y));
+	ServoPole.beta = ANGLE(atan2(worldPosVec_1.Z,worldPosVec_1.X));
+	ServoPole.distance = 1000;
+
+	return ServoPole;
+
+}
+
+// 物方到像方转换
+void getPointMirrorImageXYFromServoPole(int* Col,int* Row,int nWidth,int nHeight,float WAlaph,float WBeta,
+										float fServoA,float fServoB,float fLen,float pxSizeOfum)
+{
+	//物方
+	int dep = 1000;
+	PointXYZ WorldVec;
+	WorldVec.Y = -dep * sin(RADIAN(WAlaph));
+	WorldVec.X = -dep * cos(RADIAN(WAlaph)) * cos(RADIAN(WBeta));
+	WorldVec.Z = -dep * cos(RADIAN(WAlaph)) * sin(RADIAN(WBeta));
+
+	// 内参矩阵
+	float f_0 = fLen/pxSizeOfum * 1000;
+
+	double K[9] = 
+	{f_0,0,nWidth/2,
+	0,f_0,nHeight/2,
+	0,0,1};
+
+	// 平面镜1零位的法向量
+	PointXYZ N_01;
+	N_01.X = 1/sqrt(2.0);
+	N_01.Y = 1/sqrt(2.0);
+	N_01.Z = 0;
+
+	double Sx[9] = {1,0,0,
+		0,cos(RADIAN(fServoB)),-sin(RADIAN(fServoB)),
+		0,sin(RADIAN(fServoB)),cos(RADIAN(fServoB))};
+
+	double Sy[9] = {cos(RADIAN(fServoA)),0,-sin(RADIAN(fServoA)),
+		0,1,0,
+		sin(RADIAN(fServoA)),0,cos(RADIAN(fServoA))};
+
+	PointXYZ N_1 = RtPoint(Sx,RtPoint(Sy,N_01));
+	// 镜面反射矩阵
+	//	M = eye(3)-(2*N*N');
+	double M1[9] = {0};
+	RMirror(N_1,M1);
+
+
+	// 平面镜2零位的法向量
+	PointXYZ N_02;
+	N_02.X = -1/sqrt(2.0);
+	N_02.Y = -1/sqrt(2.0);
+	N_02.Z = 0;
+	PointXYZ N_2 = RtPoint(Sx,N_02);
+	double M2[9] = {0};
+	RMirror(N_2,M2);
+
+	// 平面镜3零位的法向量
+	PointXYZ N_03;
+	N_03.X = 1/sqrt(2.0);
+	N_03.Y = -1/sqrt(2.0);
+	N_03.Z = 0;
+	PointXYZ N_3 = RtPoint(Sx,N_03);
+	double M3[9] = {0};
+	RMirror(N_3,M3);
+
+	// 平面镜4零位的法向量
+	PointXYZ N_04;
+	N_04.X = -1/sqrt(2.0);
+	N_04.Y = 1/sqrt(2.0);
+	N_04.Z = 0;
+	PointXYZ N_4 = RtPoint(Sx,N_04);
+	double M4[9] = {0};
+	RMirror(N_4,M4);
+
+	PointXYZ camPosVec = RtPoint(M4,RtPoint(M3,RtPoint(M2,RtPoint(M1,WorldVec))));
+
+	double m_adjust[9] = {0,0,1,0,1,0,1,0,0};
+
+	PointXYZ pixPos = RtPoint(K,RtPoint(m_adjust,camPosVec));
+
+	*Col = pixPos.X / float(pixPos.Z);
+	*Row = pixPos.Y / float(pixPos.Z);
+
+
+
+}
+
+
+
+Pole getCamZaxisNUERota(double mYaw,double mPitch,double mRoll,double servoAlpha, double servoBeta)
+{
+	// 定义相机坐标系Z轴向量，长度不影响
+	PointXYZ pTest = {0};pTest.Z = 10000;
+
+	//计算旋转矩阵
+	double Mat[9] = {0};
+	getCarNUEXYZFromEOCamXYZ_Mat(mYaw,mPitch,mRoll,servoAlpha,servoBeta,Mat);
+
+	// 计算大地系向量
+	PointXYZ targetCarNUEXYZ = RtPoint(Mat,pTest);
+
+	// 计算大地系转动量
+	Pole targetCarNUEPole = getPoleFromXYZ(targetCarNUEXYZ);
+
+
+	return targetCarNUEPole;
+}
+
+// 信息上报函数
+Pole getCarNUEPoleFromEOCamPole(Pole targetCamPole, double mYaw, double mPitch, double mRoll, double servoAlpha, double servoBeta)
+{
+	//// 方位角限制在0-360
+	if (targetCamPole.beta > 360)
+	{
+		targetCamPole.beta -= 360;
+	}
+
+	if(targetCamPole.distance <= 100)
+	{
+		targetCamPole.distance = 10000;
+	}
+	//限制俯仰角度
+// 	targetCamPole.alpha = MIN(targetCamPole.alpha, 80);
+// 	targetCamPole.alpha = MAX(targetCamPole.alpha, -5);
+
+// 1:一般旋转矩阵方法
+	// 光电镜面极坐标 转 光电镜面直角坐标
+	PointXYZ targetCamXYZ = getXYZFromPole(targetCamPole);
+
+	// 光电镜面直角坐标 转 光电伺服直角坐标
+	PointXYZ targetServoXYZ = getEOServoXYZFromEOCamXYZ(targetCamXYZ, servoAlpha, servoBeta);
+
+// 2:正割补偿方法
+	// 光电镜面极坐标 转 光电伺服极坐标
+// 	Pole targetEOServoPOLE;
+// 	targetEOServoPOLE.alpha = alaph + targetCamPole.alpha;
+// 	targetEOServoPOLE.beta = beta + targetCamPole.beta/cos(RADIAN(alaph));
+// 	targetEOServoPOLE.distance = targetCamPole.distance;
+// 	// 光电伺服极坐标 转 光电伺服直角坐标
+// 	targetServoXYZ = getXYZFromPole(targetEOServoPOLE);
+
+
+//指向镜系统的成像模型
+#ifdef POINT_MIRROR_SYS
+	int nWidth = 640;
+	int nHeight = 512;
+	int Row = SERVO_CalcObjCoordinateX((FLOAT32)targetCamPole.beta, nWidth,
+		0, g_SERVO_stInput.fResolAz);
+	int Col = SERVO_CalcObjCoordinateY((FLOAT32)targetCamPole.alpha, nHeight,
+		0, g_SERVO_stInput.fResolPt);
+
+
+	// test
+	//servoBeta = 20;
+	//servoAlpha = 5;
+	//Col = 320;
+	//Row = 256;
+	//	Pole targetServoPole1 = getPointMirrorServoPoleFromImageXY( Col, Row, nWidth, nHeight,
+	//		servoBeta, servoAlpha,80,15);
+	//	targetServoXYZ = getXYZFromPole(targetServoPole1);
+	//
+	//	Col = 0;
+	//	Row = 0;
+	//
+	//	getPointMirrorImageXYFromServoPole(&Col,&Row,nWidth,nHeight,targetServoPole.alpha,targetServoPole.beta,
+	//		servoBeta, servoAlpha,80,15);
+
+
+	Pole targetServoPole = getPointMirrorServoPoleFromImageXY( Row, Col, nWidth, nHeight,
+			servoBeta, servoAlpha,80,15);
+		targetServoXYZ = getXYZFromPole(targetServoPole);
+
+#endif
+
+
+
+
+	// 光电伺服直角坐标 转 光电基准直角坐标
+	PointXYZ targetEOBaseXYZ = getEOBaseXYZFromEOServoXYZ(targetServoXYZ, g_YawBias, g_PitchBias, g_RollBias);
+
+	// 光电基准直角坐标 转 平台直角坐标
+	PointXYZ targetCarXYZ = getCarXYZFromEOBaseXYZ(targetEOBaseXYZ, g_XBias, g_YBias, g_ZBias);
+
+	// 平台直角坐标 转 平台地理直角坐标
+	PointXYZ targetCarNUEXYZ = getCarNUEXYZFromCarXYZ(targetCarXYZ, mYaw, mPitch, mRoll);
+
+	// 平台地理直角坐标 转 平台地理极坐标
+	Pole targetCarNUEPole = getPoleFromXYZ(targetCarNUEXYZ);
+
+#if 0
+	//double Mat[9] = {0};
+	//mYaw = 0;mPitch = 80;mRoll =5.1;
+	//servoAlpha = 0;servoBeta=0;
+	PointXYZ pTest = {0};pTest.Z = 10000;
+	getCarNUEXYZFromEOCamXYZ_Mat(mYaw,mPitch,mRoll,servoAlpha,servoBeta,Mat);
+	PointXYZ targetCarNUEXYZ2 = RtPoint(Mat,pTest);
+	Pole targetCarNUEPole2 = getPoleFromXYZ(targetCarNUEXYZ2);
+#endif
+
+
+	// 上报目标平台地理极坐标
+	return targetCarNUEPole;
+}
+
+PointXYZ getEOServoXYZFromEOCamXYZ(PointXYZ targetCamXYZ,double alaph,double beta)
+{
+	// 先绕Z轴俯仰角旋转-alaph，后绕Y轴方位角旋转-beta
+	double oppRz[9];
+	double oppRy[9];
+	double oppRyz[9];
+
+	// 计算旋转矩阵
+	Rz(-alaph, oppRz);
+	Ry(-beta, oppRy);
+	MultiMat33(oppRy,oppRz, oppRyz);
+
+	// 坐标转换
+	return RtPoint(oppRyz, targetCamXYZ);
+}
+
+PointXYZ getEOBaseXYZFromEOServoXYZ(PointXYZ targetServoXYZ, double eYaw, double ePitch, double eRoll)
+{
+	// 先绕x轴横滚旋转 -eRoll，再绕z轴俯仰旋转 -ePitch，再绕y轴方位旋转-eYaw
+
+	double oppRx[9];
+	double oppRy[9];
+	double oppRz[9];
+	double oppRyzx[9];
+
+	// 计算旋转矩阵
+	Rx(-eRoll, oppRx);
+	Rz(-ePitch, oppRz);
+	Ry(-eYaw, oppRy);
+	
+	MultiMat333(oppRy,oppRz, oppRx, oppRyzx);
+
+	// 坐标转换
+	return RtPoint(oppRyzx, targetServoXYZ);
+
+}
+
+PointXYZ getCarXYZFromEOBaseXYZ(PointXYZ targetEOBaseXYZ, double deltaX, double deltaY, double deltaZ)
+{
+	//平台坐标系下目标坐标
+	PointXYZ targetCarXYZ;
+
+
+	targetCarXYZ.X = targetEOBaseXYZ.X + deltaX;
+	targetCarXYZ.Y = targetEOBaseXYZ.Y + deltaY;
+	targetCarXYZ.Z = targetEOBaseXYZ.Z + deltaZ;
+
+	return targetCarXYZ;
+}
+
+PointXYZ getCarNUEXYZFromCarXYZ(PointXYZ targetCarXYZ, double mYaw, double mPitch, double mRoll)
+{
+	// 先绕x轴横滚旋转 -mRoll，再绕z轴俯仰旋转 -mPitch，再绕y轴方位旋转-mYaw
+
+	double oppRx[9];
+	double oppRy[9];
+	double oppRz[9];
+	double oppRyzx[9];
+
+	// 计算旋转矩阵
+	Rx(-mRoll, oppRx);
+	Rz(-mPitch, oppRz);
+	Ry(-mYaw, oppRy);
+
+	MultiMat333(oppRy,oppRz, oppRx, oppRyzx);
+
+	// 坐标转换
+	return RtPoint(oppRyzx, targetCarXYZ);
+
+}
+
+
+
+// 接受导引函数
+Pole getEOServoPoleFromCarNUEPole(Pole targetCarNUEPole, Pole* targetEOCamPole,double mYaw, double mPitch, double mRoll, double servoAlpha, double servoBeta)
+{
+	// 地理极坐标 转 地理直角坐标
+	PointXYZ targetCarNUEXYZ = getXYZFromPole(targetCarNUEPole);
+
+	// 地理直角坐标 转 平台直角坐标
+	PointXYZ targetCarXYZ = getCarXYZFromCarNUEXYZ(targetCarNUEXYZ, mYaw, mPitch, mRoll);
+
+	// 平台直角 转 光电基准系直角
+	PointXYZ targetEOBaseXYZ = getEOBaseXYZFromCarXYZ(targetCarXYZ, g_XBias, g_YBias, g_ZBias);
+
+	// 光电基准系直角 转 光电伺服直角
+	PointXYZ targetEOServoXYZ = getEOServoXYZFromEOBaseXYZ(targetEOBaseXYZ, g_YawBias, g_PitchBias, g_RollBias);
+
+	// 光电伺服直角 转 光电伺服极坐标
+	Pole targetEOServoPole = getPoleFromXYZ(targetEOServoXYZ);
+
+ 	// 光电伺服直角 转 光电镜面直角坐标
+ 	PointXYZ targetEOCamXYZ = getEOCamXYZFromEOServoXYZ(targetEOServoXYZ, servoAlpha, servoBeta);
+ 
+ 	// 光电镜面直角系 转 光电镜面极坐标
+ 	*targetEOCamPole = getPoleFromXYZ(targetEOCamXYZ);
+
+
+	//指向镜系统的成像模型
+#ifdef POINT_MIRROR_SYS
+	int nWidth = 640;
+	int nHeight = 512;
+
+	int Col = 0;
+	int Row = 0;
+
+	getPointMirrorImageXYFromServoPole(&Col,&Row,nWidth,nHeight,targetEOServoPole.alpha,targetEOServoPole.beta,servoAlpha,servoBeta,80,15);
+	// 计算目标的方位角、俯仰角
+	targetEOCamPole->beta = SERVO_CalcObjAzimuth(Col,
+		nWidth,	0, g_SERVO_stInput.fResolAz);
+	targetEOCamPole->alpha = SERVO_CalcObjPitching(Row,
+		nHeight,0, g_SERVO_stInput.fResolPt);
+
+#endif
+
+	return targetEOServoPole;
+}
+
+PointXYZ getCarXYZFromCarNUEXYZ(PointXYZ targetCarNUEXYZ, double mYaw, double mPitch, double mRoll)
+{
+	// 先绕y轴方位旋转mYaw -> 再绕z轴俯仰旋转 mPitch -> 绕x轴横滚旋转 mRoll
+	double oppRx[9];
+	double oppRy[9];
+	double oppRz[9];
+	double oppRxzy[9];
+
+	// 计算旋转矩阵
+	Ry(mYaw, oppRy);
+	Rz(mPitch, oppRz);
+	Rx(mRoll, oppRx);
+
+	MultiMat333(oppRx, oppRz, oppRy, oppRxzy);
+
+	// 坐标转换
+	return RtPoint(oppRxzy, targetCarNUEXYZ);
+
+}
+
+PointXYZ getEOBaseXYZFromCarXYZ(PointXYZ targetCarXYZ, double deltaX, double deltaY, double deltaZ)
+{
+	//平台坐标系下目标坐标
+	PointXYZ targetEOBaseXYZ;
+
+	targetEOBaseXYZ.X = targetCarXYZ.X - deltaX;
+	targetEOBaseXYZ.Y = targetCarXYZ.Y - deltaY;
+	targetEOBaseXYZ.Z = targetCarXYZ.Z - deltaZ;
+
+	return targetEOBaseXYZ;
+}
+
+PointXYZ getEOServoXYZFromEOBaseXYZ(PointXYZ targetEOBaseXYZ, double eYaw, double ePitch, double eRoll)
+{
+	// 先绕y轴方位旋转eYaw,再绕z轴俯仰旋转 ePitch,再绕x轴横滚旋转 eRoll
+	double oppRx[9];
+	double oppRy[9];
+	double oppRz[9];
+	double oppRxzy[9];
+
+	// 计算旋转矩阵
+	Rx(eRoll, oppRx);
+	Rz(ePitch, oppRz);
+	Ry(eYaw, oppRy);
+
+	MultiMat333(oppRx, oppRz, oppRy, oppRxzy);
+
+	// 坐标转换
+	return RtPoint(oppRxzy, targetEOBaseXYZ);
+
+}
+
+PointXYZ getEOCamXYZFromEOServoXYZ(PointXYZ targetEOServoXYZ, double alaph, double beta)
+{
+	// 先绕Y轴方位角旋转beta,后绕Z轴俯仰角旋转alaph
+	double oppRz[9];
+	double oppRy[9];
+	double oppRzy[9];
+
+	// 计算旋转矩阵
+	Rz(alaph, oppRz);
+	Ry(beta, oppRy);
+	MultiMat33(oppRz, oppRy, oppRzy);
+
+	// 坐标转换
+	return RtPoint(oppRzy, targetEOServoXYZ);
+
+}
+
+Pole getPoleFromXYZ(PointXYZ targetXYZ)
+{
+	double x = targetXYZ.X;
+	double y = targetXYZ.Y;
+	double z = targetXYZ.Z;
+
+	Pole result;
+	result.distance = sqrt(x*x + y*y + z*z);
+	result.alpha = ANGLE(asin(y / result.distance));
+
+	//if (x < 0)
+	//{
+	//	result.alpha = -180-result.alpha;
+	//}
+
+
+	result.beta = (ANGLE(atan2(z , x)));
+// 	if (x < 0 && z >= 0) // 90 - 180
+// 	{
+// 		result.beta = 180 - result.beta;
+// 	}
+// 	else if (x < 0 && z < 0) // 180 - 270
+// 	{
+// 		result.beta = result.beta + 180;
+// 	}
+// 	else if (x >= 0 && z < 0) // 270 - 360
+// 	{
+// 		result.beta = 360 - result.beta;
+// 	}
+
+	return result;
+
+}
+
+PointXYZ getXYZFromPole(Pole targetPole)
+{
+	// 距离未知时，异原点坐标系之间不能转换
+	// 但由于同车坐标系的原点差一般远小于目标距离，故距离较大时误差较小，误差量级可以用线偏差/距离获得的弧度进行估计
+	// 如线偏差1米，目标3000米，则转换误差为(1/1000-1/3000)约为0.04°
+	if (targetPole.distance < 100)
+	{
+		targetPole.distance = 1000;
+	}
+
+	PointXYZ result;
+	double aa, bb, cc;
+	//double ac = sqrt(targetPole.distance*targetPole.distance-bb*bb);    
+	bb = sin(RADIAN(targetPole.alpha)) * targetPole.distance;
+	double ac = sqrt(targetPole.distance*targetPole.distance - bb*bb);
+	aa = cos(RADIAN(targetPole.beta)) * ac;
+	cc = sin(RADIAN(targetPole.beta)) * ac;
+
+	result.X = aa;
+	result.Y = bb;
+	result.Z = cc;
+
+	return result;
+
+}
+
+//大地坐标转地球空间直角坐标
+PointXYZ getXYZFromBLH(PointBLH BLH)
+{
+	double B = BLH.B;
+	double L = BLH.L;
+	double H = BLH.H;
+
+	double W = sqrt(1 - ee * pow(sin(RADIAN(B)), 2));
+	double N = a / W;          // 椭球的卯酉圈曲率
+
+	// 将地理坐标变换到空间直角坐标
+	double X = (N + H) * cos(RADIAN(B)) * cos(RADIAN(L));
+	double Y = (N + H) * cos(RADIAN(B)) * sin(RADIAN(L));
+	double Z = (N * (1 - ee) + H) * sin(RADIAN(B));
+
+	PointXYZ pointXYZ;
+	pointXYZ.X = X;
+	pointXYZ.Y = Y;
+	pointXYZ.Z = Z;
+
+	return pointXYZ;
+}
+
+//地球空间直角坐标-->大地坐标
+PointBLH getBLHFromXYZ(PointXYZ pointXYZ)
+{
+	double X = pointXYZ.X;
+	double Y = pointXYZ.Y;
+	double Z = pointXYZ.Z;
+
+	double U = atan(Z * a / (sqrt(X * X + Y * Y) * b));
+	double B0 = atan((Z + b * epep * pow(sin(U), 3)) / (sqrt(X*X + Y*Y) - a*ee*pow(cos(U), 3)));
+	double N = a / sqrt(1 - ee * pow(sin(B0), 2));
+	//double L = atan(Y /X);
+	double L = atan2(Y, X); // 修改为全球范围计算          by wcw04046 @ 2021/02/18
+
+	double H = sqrt(X*X + Y*Y + pow(Z + N*ee*sin(B0), 2)) - N;
+	double B = atan(Z / sqrt(X*X + Y*Y) / (1 - ee*N / (N + H)));
+
+	PointBLH pointBLH;
+	pointBLH.B = ANGLE(B);
+	pointBLH.H = H;
+// 	// 此处仅考虑东经
+// 	pointBLH.L = ANGLE(L) > 0 ? ANGLE(L) : ANGLE(L) + 180;
+
+	// 修改为全球范围计算          by wcw04046 @ 2021/02/18
+	pointBLH.L = ANGLE(L);
+
+	return pointBLH;
+}
+
+//*********************************************************************************************
+//  由NUE直角坐标变换到CGCS直角坐标
+//  targetPointXYZ：NUE下的目标坐标
+//  selfPointXYZ：测站在CGCS下的直角坐标
+//  返回CGCS下的目标坐标
+//*********************************************************************************************
+PointXYZ getCGCSXYZFromNUEXYZ(PointXYZ targetPointXYZ, PointXYZ selfPointXYZ)
+{
+	// 将测站所在位置的CGCS直角坐标变换到地理坐标
+	PointBLH selfPointBLH = getBLHFromXYZ(selfPointXYZ);
+
+	/* 由NUE直角坐标变换到CGCS直角坐标*/
+	double A[3];
+	double G[3];
+	double OppRx[9];
+	double OppRy[9];
+	double OppRz[9];
+	double OppRzx[9];
+	double OppRzxy[9];
+
+	// 1、计算旋转矩阵
+	Rx(-selfPointBLH.B, OppRx);
+	Ry(-90, OppRy);
+	Rz(90 - selfPointBLH.L, OppRz);
+
+	MultiMat(OppRz, OppRx, OppRzx, 3, 3, 3);
+	MultiMat(OppRzx, OppRy, OppRzxy, 3, 3, 3);
+
+	// 2、将NUE直角坐标变换到CGCS直角坐标
+	A[0] = targetPointXYZ.X;
+	A[1] = targetPointXYZ.Y;
+	A[2] = targetPointXYZ.Z;
+	MultiMat(OppRzxy, A, G, 3, 3, 1);
+
+	PointXYZ result;
+	result.X = G[0] + selfPointXYZ.X;
+	result.Y = G[1] + selfPointXYZ.Y;
+	result.Z = G[2] + selfPointXYZ.Z;
+	return result;
+}
+
+//*********************************************************************************************
+// 由CGCS直角坐标变换到NUE直角坐标
+// targetPointXYZ：CGCS下的目标坐标
+// selfPointXYZ：测站在CGCS下的直角坐标
+// 返回NUE下的目标坐标
+//*********************************************************************************************
+PointXYZ getNUEXYZFromCGCSXYZ(PointXYZ targetPointXYZ, PointXYZ selfPointXYZ)
+{
+	// 将测站所在位置的CGCS直角坐标变换到地理坐标
+	PointBLH selfPointBLH = getBLHFromXYZ(selfPointXYZ);
+
+	// 计算中间值
+	double dx = targetPointXYZ.X - selfPointXYZ.X;
+	double dy = targetPointXYZ.Y - selfPointXYZ.Y;
+	double dz = targetPointXYZ.Z - selfPointXYZ.Z;
+
+	/* 由CGCS直角坐标变换到NUE直角坐标 */
+	double A[3];
+	double G[3];
+	double OppRx[9];
+	double OppRy[9];
+	double OppRz[9];
+	double OppRyx[9];
+	double OppRyxz[9];
+
+	// 1、计算旋转矩阵
+	Rx(selfPointBLH.B, OppRx);
+	Ry(90, OppRy);
+	Rz(-90 + selfPointBLH.L, OppRz);
+
+	MultiMat(OppRy, OppRx, OppRyx, 3, 3, 3);
+	MultiMat(OppRyx, OppRz, OppRyxz, 3, 3, 3);
+
+	// 2、将CGCS直角坐标变换到NUE直角坐标
+	A[0] = dx;
+	A[1] = dy;
+	A[2] = dz;
+	MultiMat(OppRyxz, A, G, 3, 3, 1);
+
+	PointXYZ result;
+	result.X = G[0];
+	result.Y = G[1];
+	result.Z = G[2];
+	return result;
+}
+
+
+
+// 绕X轴旋转矩阵
+/* 1    0      0
+0    cos    sin
+0   -sin    cos
+*/
+void Rx(double omega, double *Opp)
+{
+	double coso = cos(RADIAN(omega));
+	double sino = sin(RADIAN(omega));
+
+	Opp[0] = 1;
+	Opp[1] = 0;
+	Opp[2] = 0;
+	Opp[3] = 0;
+	Opp[4] = coso;
+	Opp[5] = sino;
+	Opp[6] = 0;
+	Opp[7] = -sino;
+	Opp[8] = coso;
+}
+
+// 绕Y轴旋转矩阵
+/* cos    0    sin
+0     1     0
+-sin  0    cos
+*/
+void Ry(double omega, double *Opp)
+{
+	double coso = cos(RADIAN(omega));
+	double sino = sin(RADIAN(omega));
+
+	Opp[0] = coso;
+	Opp[1] = 0;
+	Opp[2] = sino;
+	Opp[3] = 0;
+	Opp[4] = 1;
+	Opp[5] = 0;
+	Opp[6] = -sino;
+	Opp[7] = 0;
+	Opp[8] = coso;
+}
+
+
+// 绕Z轴旋转矩阵 俯仰角
+/* cos    sin    0
+-sin cos  0
+0   0     1
+*/
+void Rz(double omega, double *Opp)
+{
+	double coso = cos(RADIAN(omega));
+	double sino = sin(RADIAN(omega));
+
+	Opp[0] = coso;
+	Opp[1] = sino;
+	Opp[2] = 0;
+	Opp[3] = -sino;
+	Opp[4] = coso;
+	Opp[5] = 0;
+	Opp[6] = 0;
+	Opp[7] = 0;
+	Opp[8] = 1;
+}
+
+// 镜面反射矩阵计算
+void RMirror(PointXYZ normalVec, double *Opp)
+{
+	//	M = eye(3)-(2*N*N');
+	Opp[0] = 1-2*normalVec.X*normalVec.X;
+	Opp[1] = -2*normalVec.X*normalVec.Y;
+	Opp[2] = -2*normalVec.X*normalVec.Z;
+	Opp[3] = -2*normalVec.X*normalVec.Y;
+	Opp[4] = 1-2*normalVec.Y*normalVec.Y;
+	Opp[5] = -2*normalVec.Y*normalVec.Z;
+	Opp[6] = -2*normalVec.X*normalVec.Z;
+	Opp[7] = -2*normalVec.Y*normalVec.Z;
+	Opp[8] = 1-2*normalVec.Z*normalVec.Z;
+
+}
+//*********************************************************************************************
+//  矩阵乘法，矩阵A*矩阵B
+//*********************************************************************************************
+void  MultiMat(double *A, double *B, double *AB, int m, int n, int p)
+{
+	// A为m*n ; B为n*p  ;输出AB为m*p
+	int	i, j, k;
+	double Multisum;
+
+	for (i = 0; i < m; i++) //输出矩阵的m行
+	{
+		for (j = 0; j < p; j++) //输出矩阵的p列
+		{
+			Multisum = 0;
+			for (k = 0; k < n; k++)
+				Multisum += A[i*n + k] * B[k*p + j];
+
+			AB[i*p + j] = Multisum;
+		}
+	}
+}
+
+//*********************************************************************************************
+//  3阶矩阵乘法，矩阵A*矩阵B
+//*********************************************************************************************
+void MultiMat33(double * A, double * B, double * AB)
+{
+	MultiMat(A, B, AB, 3, 3, 3);
+}
+
+//*********************************************************************************************
+//  3阶矩阵乘法，矩阵A*矩阵B*矩阵C
+//*********************************************************************************************
+void MultiMat333(double * A, double * B, double * C, double * ABC)
+{
+	double BC[9];
+	MultiMat(B, C, BC, 3, 3, 3);
+	MultiMat(A, BC, ABC, 3, 3, 3);
+}
+
+// 3维空间旋转矩阵执行坐标转换
+PointXYZ RtPoint(double * M, PointXYZ Point)
+{
+	// M * [X,Y,Z]'
+	double A[3];
+	double G[3];
+	A[0] = Point.X;
+	A[1] = Point.Y;
+	A[2] = Point.Z;
+	MultiMat(M, A, G, 3, 3, 1);
+
+	PointXYZ result;
+	result.X = G[0];
+	result.Y = G[1];
+	result.Z = G[2];
+	return result;
+
+}
+
+// 归一化
+PointXYZ NormPoint(PointXYZ Point)
+{
+	double Len = sqrt(Point.X * Point.X + Point.Y * Point.Y + Point.Z * Point.Z);
+    Point.X /= Len;
+	Point.Y /= Len;
+	Point.Z /= Len;
+	return Point;
+}
+void getCarNUEXYZFromEOCamXYZ_Mat(double mYaw,double mPitch,double mRoll,double servoAlpha, double servoBeta,double* Mat)
+{
+	double oppRz[9];
+	double oppRy[9];
+	double oppRx[9];
+
+	// 1.光电镜面直角坐标 转 光电伺服直角坐标
+
+	// 先绕Z轴俯仰角旋转-alaph，后绕Y轴方位角旋转-beta
+	double oppRyz_servo[9];
+	// 计算旋转矩阵
+	Rz(-servoAlpha, oppRz);
+	Ry(-servoBeta, oppRy);
+	MultiMat33(oppRy,oppRz, oppRyz_servo);
+
+	// 2.光电伺服直角坐标 转 光电基准直角坐标
+
+	// 先绕x轴横滚旋转 -eRoll，再绕z轴俯仰旋转 -ePitch，再绕y轴方位旋转-eYaw
+	double oppRyzx_base[9];
+	// 计算旋转矩阵
+	Rx(-g_RollBias, oppRx);
+	Rz(-g_PitchBias, oppRz);
+	Ry(-g_YawBias, oppRy);
+	MultiMat333(oppRy,oppRz, oppRx, oppRyzx_base);
+
+	// 3.光电基准直角坐标 转 平台直角坐标
+	// 不考虑平移关系
+	
+	// 4.平台直角坐标 转 平台地理直角坐标
+
+	double oppRyzx_GEO[9];
+	// 计算旋转矩阵
+	Rx(-mRoll, oppRx);
+	Rz(-mPitch, oppRz);
+	Ry(-mYaw, oppRy);
+	MultiMat333(oppRy,oppRz, oppRx, oppRyzx_GEO);
+
+
+	// 连续旋转矩阵计算,注意顺序
+	MultiMat333(oppRyzx_GEO,oppRyzx_base,oppRyz_servo,Mat);
+}
+
+

Arith_VehicleCoordModule.h

@@ -0,0 +1,221 @@
+
+/*********版权所有（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__
+
+

CMakeLists.txt

@@ -0,0 +1,104 @@
+cmake_minimum_required(VERSION 3.5)
+
+project(QGuideArith VERSION 0.1 LANGUAGES CXX)
+
+
+
+
+
+
+
+
+set(CMAKE_INCLUDE_CURRENT_DIR ON)
+set(CMAKE_AUTOUIC ON)
+set(CMAKE_AUTOMOC ON)
+set(CMAKE_AUTORCC ON)
+
+set(CMAKE_CXX_STANDARD 11)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+
+
+
+
+
+
+IF(WIN32)
+set(CMAKE_PREFIX_PATH "C:/Qt/5.9.6/msvc2015_64")
+ELSE(WIN32)
+set(CMAKE_PREFIX_PATH "/home/wang/Qt/5.9.6/gcc_64")
+ENDIF(WIN32)
+
+find_package(QT NAMES Qt6 Qt5 REQUIRED COMPONENTS Widgets  LinguistTools )
+find_package(Qt${QT_VERSION_MAJOR} REQUIRED   COMPONENTS Widgets  LinguistTools)
+
+
+set(TS_FILES QGuideArith_zh_CN.ts)
+
+#include_directories(${QT_QTOPENGL_INCLUDE_DIR} ${OPENGL_INCLUDE_DIR})
+#message(${QT_QTOPENGL_INCLUDE_DIR})
+
+
+# 算法平台源文件
+SET(QGuideArith_DIR ${CMAKE_SOURCE_DIR})
+
+# 包含本地路径，否则qt提升类报错
+include_directories(${QGuideArith_DIR})
+
+
+
+FILE(GLOB PROJECT_SOURCES ${QGuideArith_DIR}/*.cpp ${QGuideArith_DIR}/*.h 
+${QGuideArith_DIR}/*.ui 
+${QGuideArith_DIR}/*.qrc 
+${QGuideArith_DIR}/*.rc
+${TS_FILES})
+
+if(${QT_VERSION_MAJOR} GREATER_EQUAL 6)
+    qt_add_executable(QGuideArith
+        MANUAL_FINALIZATION
+        ${PROJECT_SOURCES}
+    )
+# Define target properties for Android with Qt 6 as:
+#    set_property(TARGET QGuideArith APPEND PROPERTY QT_ANDROID_PACKAGE_SOURCE_DIR
+#                 ${CMAKE_CURRENT_SOURCE_DIR}/android)
+# For more information, see https://doc.qt.io/qt-6/qt-add-executable.html#target-creation
+
+    qt_create_translation(QM_FILES ${CMAKE_SOURCE_DIR} ${TS_FILES})
+else()
+    if(ANDROID)
+        add_library(QGuideArith SHARED
+            ${PROJECT_SOURCES}
+        )
+# Define properties for Android with Qt 5 after find_package() calls as:
+#    set(ANDROID_PACKAGE_SOURCE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/android")
+    else()
+        add_executable(QGuideArith
+            ${PROJECT_SOURCES})
+    endif()
+
+    qt5_create_translation(QM_FILES ${CMAKE_SOURCE_DIR} ${TS_FILES})
+endif()
+
+#可执行文件输出路径
+set(EXECUTABLE_OUTPUT_PATH ${CMAKE_SOURCE_DIR}/Bin)
+
+target_link_libraries(QGuideArith PRIVATE    
+Qt${QT_VERSION_MAJOR}::Core 
+Qt${QT_VERSION_MAJOR}::Gui 
+Qt${QT_VERSION_MAJOR}::Widgets 
+)
+
+set_target_properties(QGuideArith PROPERTIES
+    MACOSX_BUNDLE_GUI_IDENTIFIER my.example.com
+    MACOSX_BUNDLE_BUNDLE_VERSION ${PROJECT_VERSION}
+    MACOSX_BUNDLE_SHORT_VERSION_STRING ${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR}
+    MACOSX_BUNDLE TRUE
+    WIN32_EXECUTABLE TRUE
+)
+
+
+
+
+if(QT_VERSION_MAJOR EQUAL 6)
+    qt_finalize_executable(QGuideArith)
+endif()

main.cpp

@@ -0,0 +1,13 @@
+#include "mainwindow.h"
+#include <QStyleFactory>
+#include <QApplication>
+
+int main(int argc, char *argv[])
+{
+    QApplication a(argc, argv);
+
+    a.setStyle(QStyleFactory::create("fusion"));
+    MainWindow w;
+    w.show();
+    return a.exec();
+}

mainwindow.cpp

@@ -0,0 +1,45 @@
+#include "mainwindow.h"
+#include "ui_mainwindow.h"
+#include "Arith_VehicleCoordModule.h"
+MainWindow::MainWindow(QWidget *parent)
+    : QMainWindow(parent)
+    , ui(new Ui::MainWindow)
+{
+    ui->setupUi(this);
+
+    connect(ui->pushButton, &QPushButton::pressed,this,&MainWindow::cal);
+
+    this->setWindowTitle("CoordTrans");
+}
+
+MainWindow::~MainWindow()
+{
+    delete ui;
+}
+
+void MainWindow::cal()
+{
+    PointBLH self;
+    self.B = ui->lineEdit_SelfB->text().toFloat();
+    self.L = ui->lineEdit_SelfL->text().toFloat();
+    self.H = ui->lineEdit_SelfH->text().toFloat();
+    PointXYZ selfXYZ = getXYZFromBLH(self);
+
+    PointBLH target;
+    target.B = ui->lineEdit_targetB->text().toFloat();
+    target.L = ui->lineEdit_targetL->text().toFloat();
+    target.H = ui->lineEdit_targetH->text().toFloat();
+    PointXYZ targetXYZ = getXYZFromBLH(target);
+
+    PointXYZ nueXYZ = getNUEXYZFromCGCSXYZ(targetXYZ, selfXYZ);
+
+    PointXYZ CarXYZ = getCarXYZFromCarNUEXYZ(nueXYZ, 12, 0, 0);
+
+    Pole pole = getPoleFromXYZ(CarXYZ);
+
+    ui->lineEdit_Az->setText(QString::number(pole.beta));
+
+    ui->lineEdit_Pt->setText(QString::number(pole.alpha));
+
+
+}