diff --git a/Arith_VehicleCoordModule.cpp b/Arith_VehicleCoordModule.cpp new file mode 100644 index 0000000..3a5f2c3 --- /dev/null +++ b/Arith_VehicleCoordModule.cpp @@ -0,0 +1,890 @@ +#include "Arith_VehicleCoordModule.h" +#include + + + +// 标定参数：安装误差 +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); +} + + diff --git a/Arith_VehicleCoordModule.h b/Arith_VehicleCoordModule.h new file mode 100644 index 0000000..c535d7b --- /dev/null +++ b/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__ + + diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..bab49e3 --- /dev/null +++ b/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() diff --git a/main.cpp b/main.cpp new file mode 100644 index 0000000..c0ab856 --- /dev/null +++ b/main.cpp @@ -0,0 +1,13 @@ +#include "mainwindow.h" +#include +#include + +int main(int argc, char *argv[]) +{ + QApplication a(argc, argv); + + a.setStyle(QStyleFactory::create("fusion")); + MainWindow w; + w.show(); + return a.exec(); +} diff --git a/mainwindow.cpp b/mainwindow.cpp new file mode 100644 index 0000000..52b6b57 --- /dev/null +++ b/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)); + + +} \ No newline at end of file