commit 632dd9026b0afaa6f048da09c4433173c7756b6f Author: wangchongwu <759291707@qq.com> Date: Wed Jan 15 21:12:31 2025 +0800 利用 外参拼接初始方案 diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 0000000..1ff0c42 --- /dev/null +++ b/.gitattributes @@ -0,0 +1,63 @@ +############################################################################### +# Set default behavior to automatically normalize line endings. +############################################################################### +* text=auto + +############################################################################### +# Set default behavior for command prompt diff. +# +# This is need for earlier builds of msysgit that does not have it on by +# default for csharp files. +# Note: This is only used by command line +############################################################################### +#*.cs diff=csharp + +############################################################################### +# Set the merge driver for project and solution files +# +# Merging from the command prompt will add diff markers to the files if there +# are conflicts (Merging from VS is not affected by the settings below, in VS +# the diff markers are never inserted). Diff markers may cause the following +# file extensions to fail to load in VS. An alternative would be to treat +# these files as binary and thus will always conflict and require user +# intervention with every merge. To do so, just uncomment the entries below +############################################################################### +#*.sln merge=binary +#*.csproj merge=binary +#*.vbproj merge=binary +#*.vcxproj merge=binary +#*.vcproj merge=binary +#*.dbproj merge=binary +#*.fsproj merge=binary +#*.lsproj merge=binary +#*.wixproj merge=binary +#*.modelproj merge=binary +#*.sqlproj merge=binary +#*.wwaproj merge=binary + +############################################################################### +# behavior for image files +# +# image files are treated as binary by default. +############################################################################### +#*.jpg binary +#*.png binary +#*.gif binary + +############################################################################### +# diff behavior for common document formats +# +# Convert binary document formats to text before diffing them. This feature +# is only available from the command line. Turn it on by uncommenting the +# entries below. +############################################################################### +#*.doc diff=astextplain +#*.DOC diff=astextplain +#*.docx diff=astextplain +#*.DOCX diff=astextplain +#*.dot diff=astextplain +#*.DOT diff=astextplain +#*.pdf diff=astextplain +#*.PDF diff=astextplain +#*.rtf diff=astextplain +#*.RTF diff=astextplain diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..a6eb98a --- /dev/null +++ b/.gitignore @@ -0,0 +1,51 @@ +# ---> C++ +# Prerequisites +*.d + +# Compiled Object files +*.slo +*.lo +*.o +*.obj + +# Precompiled Headers +*.gch +*.pch + +# Compiled Dynamic libraries +*.so +*.dylib +#*.dll + +# Fortran module files +*.mod +*.smod + +# Compiled Static libraries +*.lai +*.la +*.a +#*.lib + +# Executables +*.exe +*.out +*.app + +# all kind of project files +Bin +out +.vs +build +.cache +*.json + + +# version control files +Version.h + + +# install dir +install +*.xraw +ref diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..c6c2b5f --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.5.0) +project(stitch VERSION 0.1.0 LANGUAGES C CXX) + +SET(ArithStitchDir stitch) + +IF(WIN32) + set(OpenCV_DIR "C:/opencv/build/x64/vc14/lib") +ELSE(WIN32) + set(OpenCV_DIR "/home/wcw/opencv-3.4.16/install/share/OpenCV") +ENDIF(WIN32) + + +find_package(OpenCV REQUIRED) + +include_directories(${OpenCV_INCLUDE_DIRS} public_include ${ArithStitchDir}/src) + + + +# 设置算法库目录 +SET(LIB_STITCH GuideStitch) + + +add_subdirectory(stitch) + +add_executable(stitch main.cpp) + + +target_link_libraries(stitch ${OpenCV_LIBS} ${LIB_STITCH}) + +# 可执行文件输出路径 +set(EXECUTABLE_OUTPUT_PATH ${CMAKE_SOURCE_DIR}/Bin) diff --git a/S729.h b/S729.h new file mode 100644 index 0000000..b08c2dc --- /dev/null +++ b/S729.h @@ -0,0 +1,686 @@ + +#include +#include +#include + +//#pragma pack(1) +const int IMAGE_WIDTH_IR = 1280; +const int IMAGE_HEIGHT_IR = 1024; +const int IMAGE_WIDTH_VL = 1920; +const int IMAGE_HEIGHT_VL = 1080; + +const int PARA_IR_LINE = 40; //红外参数行 + + + + +#define M_ST_OBJ_NUM (100) +#define M_LT_OBJ_NUM (10) + +typedef struct tag_Objinfo +{ + float x; //中心点x + float y; //中心点x + float w; //目标宽 + float h; //目标高 + float score; //置信度 + int classIndex; //类别 + int frameID; //图像帧编号 + int key_ID; //关键部位序号 +}OBJ_INFO; + +typedef struct tgPointXY +{ + double X; + double Y; +}PointXY; +typedef struct tarResult +{ + unsigned char nObjID; //目标编号，从1开始，最大值为目标关联后目标数目 + double tLon; //目标经度 + double tLat; //目标纬度 + double tH; //目标高度 + double tX; //地心地固坐标系：X + double tY; //地心地固坐标系：Y + double tZ; //地心地固坐标系：Z + double tSpeedX; //目标X方向速度 + double tSpeedY; //目标Y方向速度 + double tSpeedZ; //目标Z方向速度 + double Speed; //目标的合速度 + double tServoAz; //地理引导伺服输出方位角 + double tServoPt; //地理引导伺服输出俯仰角 + double ENUPole_Az; //北天东地理极坐标—方位角 + double ENUPole_Pt; //北天东地理极坐标—俯仰角 + double preDis; //线性预测激光测距值 +}tarResult; + + +typedef struct stTrgOutput1 +{ + int nObjID; //目标编号 + double tLon; //目标经度 + double tLat; //目标纬度 + double tH; //目标高度 + double tX; //地心地固，X + double tY; //地心地固，Y + double tZ; //地心地固，Z + double tSpeedX; //目标X方向速度 + double tSpeedY; //目标Y方向速度 + double tSpeedZ; //目标Z方向速度 + double Speed; //目标的合速度 + double sumLon; //目标经度和 + double sumLat; //目标纬度和 + double sumH; //目标高度和 + double tfLon; //目标均值滤波经度 + double tfLat; //目标均值滤波纬度 + double tfH; //目标均值滤波高度 + int T; //有源定位次数 + double BLon; //图像中B点的经度 + double BLat; //图像中B点的纬度 + double BH; //图像中B点的高度 + double ALon; //图像中心点的经度 + double ALat; //图像中心点的纬度 + double AH; //图像中心点的高度 +}stTrgOutput1; + +typedef struct +{ + uint16_t u16Gpsweek; //GPS周 + uint32_t u32Gpssec; //GPS秒 + float fAz; //偏航角0～360 + float fPt; //俯仰角-90～90 + float fRol; //横滚角-180～180 + double i32Lat; //纬度-90～90 比例系数1E-7 + double i32Lon; //经度-180～180 比例系数1E-7 + float i32Height; //高度比例系数0.001 + float fEspeed; //东向速度m/s +/-250 + float fNspeed; //北向速度m/s +/-250 + float fSkyspeed; //天向速度m/s +/-250 + double dGyroX; //罗盘X轴角速度 m/s + double dGyroY; //罗盘Y轴角速度 m/s + double dGyroZ; //罗盘Z轴角速度 m/s + char hour; //时 + char minute; //分 + char second; //秒 + int millsec; //毫秒 +}INS_DATA1; + +// 目标跟踪状态 +typedef enum tagTrackingStatus1 +{ + xNOT_TRACKING = 0,//未跟踪 + xSTABLE_TRACKING = 1,//稳跟 + xMEM_TRACKING = 2//记忆跟踪 +}TrackingStatus1; + +typedef struct tagRect32S1 //BYTES: 4*4=16 +{ + int x; /*> 左上点x坐标 */ + int y; /*> 左上点y坐标 */ + int w; /*> 矩形宽度 */ + int h; /*> 矩形高度 */ +}RECT32S1; + +// 尺度模式 +typedef enum tagSizeType1 +{ + xunKnown, //不确定目标或未识别尺寸类型 + xDimTarget, //弱小目标 + xSmallTarget, //小目标 + xMiddleTarget, //临界目标 + xAreaTarget //面目标 +}SizeType1; + + +// 目标检测算法标识 +typedef enum tagemObjSrc1 +{ + xArith_DST = 1, // 检测来源--小目标 + xArith_DAT = 2, // 检测来源--面目标 + xArith_AI = 3, // 检测来源--AI检测 + xArith_KCF = 4, // 检测来源--KCF + xArith_TLD = 5, // 检测来源--TLD + xArith_Manual = 6, + xArith_SiamRPN = 7, // 检测来源--SiamRPN + xArith_AIMap = 8 // 检测来源--AI检测映射 +}emObjSrc1; + +typedef struct tagARIDLL_OBJINFO1 +{ + //*****1.目标状态信息***** + int nFrameId; //目标当前信息所对应的帧编号 + unsigned char unObjStatus; //目标搜索状态信息，更新/新增/删除 + unsigned char bMainTracked; //目标是否为主跟踪目标 + TrackingStatus1 unTrackingStatus;//目标跟踪状态 + + + //*****2.目标管道信息***** + int nOutputID; //输出告警目标 + int nInPipesID; //目标在管道数组中的编号 + int nPipeLostCnt; //目标当前管道连续丢失计数 + int nTotalCnt; // 目标当前管道总帧数 + unsigned char bInCurrFov; //目标是否在当前视场 + int nAntiJamming; // 抗干扰状态 + + //*****3.目标核心信息***** + float nX; //目标中心点图像坐标x + float nY; //目标中心点图像坐标y + float nObjW; //目标宽度 + float nObjH; //目标高度 + float fAz; //目标当前方位角 + float fPt; //目标当前俯仰角 + + // 目标预测位置 + float fPredAz; + float fPredPt; + + //*****4.其他属性信息***** + int nObjGray; //目标灰度 + int nObjMaxGray; //目标极值灰度 + int nMaxPosX; //目标极大值点X + int nMaxPosY; //目标极大值点Y + int nPixCnts; //目标像素个数 + unsigned char ubSizeType; //目标尺寸类型: + float fProb; //目标识别置信度 + float fSNR; //目标信噪比值 + float fTgEntropy; //目标信息熵值 + float fBgEntropy; //目标背景信息熵 + float fSaliency; //目标显著性值 + + // + bool nJammingSucess; //目标成功干扰 + int nClassID; //目标类型 + + // 如果处于跟踪状态，则输出下列值 + RECT32S1 SA_SrBox;//小面目标跟踪波门 + SizeType1 SA_SizeType;//尺度信息 + RECT32S1 KCF_SrBox;//KCF波门 + RECT32S1 TLD_SrBox;//TLD波门 + float fConf;//跟踪置信度 + emObjSrc1 ArithSrc;//跟踪算法来源，决策后 + + unsigned char byte[20];//预留 + + //-----------------------------------------------// + bool MoveStatus; // 运动状态，true--运动 false -- 静止 + +}ARIDLL_OBJINFO1; + + + +////////////////////////// +//地理引导信息 +typedef struct +{ + double fLon; //载机经度 + double fLat; //载机纬度 + double fWGS_H; //载机高度 + double fPlatAz; //载机航向角 + double fPlatPt; //载机俯仰角 + double fPlatRoll; //载机横滚角 + double fServoAz; //伺服方位角 + double fServoPt; //伺服俯仰角 + double x; //CGCS直角坐标x + double y; //CGCS直角坐标y + double z; //CGCS直角坐标z + double Distance; //激光测距值 + double tLon; //目标经度（标定点真值） + double tLat; //目标纬度（标定点真值） + double tH; //目标高度（标定点真值） +}GeogGuideInfo; + + +//目标变化信息 +typedef struct +{ + char change[20]; + float x1; + float y1; + float x2; + float y2; +}variationInfo; + +//跟踪目标信息 + + + +//跟踪目标信息 +typedef struct tagxARIDLL_OUTPUT +{ + int nTimeStamp; //当前帧时间戳(透传)，单位：毫秒 + int nSysMode; // 系统工作模式(透传) + int nFrmNum; //处理帧计数 + + int nStatus; //工作状态--待命/检测/跟踪/丢失状态信息等 + + //目标检测(短时航迹点，用于用户指示) + int nAlarmObjCnts; //当前帧告警目标总个数 + ARIDLL_OBJINFO1 stAlarmObjs[M_ST_OBJ_NUM]; //检测目标信息数组 + + //目标跟踪(长时航迹点，第0个为主目标送伺服跟踪) //锁定主跟踪 + int nTrackObjCnts; //跟踪目标个数 + ARIDLL_OBJINFO1 stTrackers[M_LT_OBJ_NUM]; //跟踪器输出数组 + +}xTRACK_OUTPUT; + +//目标融合识别信息 +typedef struct tagOBJINFO_RECT +{ + float nx; // 目标框中心点图像坐标x + float ny; // 目标框中心点图像坐标y + float nW; // 目标宽度 + float nH; // 目标高度 + float fconfidence; // 目标识别置信度 + int class_id; // 目标类别 +}OBJINFO_RECT1; + +//地理引导结果 +typedef struct stGuide +{ + double Yaw1; //伺服方位（解1） + double Pitch1; //伺服俯仰（解1） + double Yaw2; //伺服方位（解2） + double Pitch2; //伺服俯仰（解2） + double ro; //目标和载机间直线距离（激光测距理论值） +}Guide; + +//定位结果 +typedef struct stTrgOutput2 +{ + unsigned char nObjID; //目标编号 + double tLon; //目标经度 + double tLat; //目标纬度 + double tH; //目标高度 + double tX; //地心地固，X + double tY; //地心地固，Y + double tZ; //地心地固，Z + double tSpeedX; //目标X方向速度 + double tSpeedY; //目标Y方向速度 + double tSpeedZ; //目标Z方向速度 + double Speed; //目标的合速度 +}stTrgOutput2; + +typedef struct tagFusionSpeed32F //BYTES: 2*4=8 +{ + float vx; //x方向速度 + float vy; //y方向速度 + float vz; //z方向速度 +}SPEEDXYZ32F; + +typedef struct tagPoint3D +{ + double x; //��άֱ������ϵx���� + double y; //��άֱ������ϵy���� + double z; //��άֱ������ϵz���� +} POINT_3D; + +typedef struct tagPoint2S +{ + short x; //��άͼ��x���� + short y; //��άͼ��y���� +} POINT_2S; +//������ά����ṹ��--������ +typedef struct tagPoint3F +{ + float x; //��άֱ������ϵx���� + float y; //��άֱ������ϵy���� + float z; //��άֱ������ϵz���� +} POINT_3F; +//告警目标信息 +typedef struct +{ + unsigned int uiTargetTimeTag; //时间标签 + unsigned short usMissleIdx; //标识告警目标索引，目标编号，（1~255） + unsigned short usMissleBatchNum; //目标批号，系统统一编号 + unsigned short usGuideDetectState; //被引导红外探测状态 0：无效 1：成功 + unsigned short usQuickWarningTag; //快速目标告警标识，0=NA，1：快速告警2：自主告警 + short sAzimuthAng; //目标方位角，单位0.01°，取值范围-180°~+180° + unsigned short usAzimuthAngErrorAbs; //方位角与均值的误差绝对值，单位：0.01° + short sPitchingAng; //目标俯仰角，单位0.01°，取值范围-90°~+90° + unsigned short usPitchingAngErrorAbs; //俯仰角与估计均值的误差绝对值，单位：0.01° + unsigned char ucTargetType; //目标类型，0=NA，1=导弹，2=未知 + unsigned char ucSpatialAttribute; //空间属性，0=NA，1=空中，2=地面 + unsigned char ucIsActiveOrPassiveStage; //主被动状态，0=NA，1=主动段，2=被动段 + unsigned char ucNewOrOld; //目标新旧标记 0=旧目标 1=新目标 + unsigned char ucTargetSize; //目标大小（尺寸） + unsigned short usTargetEnergy; //目标能量 + + POINT_2S pPos; //目标位置（图像坐标） + POINT_3F pPos3D; //目标映射到球面上的三维空间坐标 + unsigned short usPlace; //小目标坐标位于哪一副图像 + unsigned short usPipeIndex; //告警目标所在的管道序号 + unsigned char ucIsNewAlarm; //是否为新的告警，0为否，1为是 + unsigned char ucOutputAlarm; //待输出告警，0表示不是告警，1表示超出16个小于32个的候选告警目标，2表示为筛选的四个告警目标（20ms），3表示为上报的告警目标（100ms） + short sAlarmTypeFlag; //告警类型标记 + + SPEEDXYZ32F fPlaneSpeedXYZ; //载体地心直角坐标系下的绝对速度 + float fSpeedAz; //方位速度信息(相对载体) + float fSpeedPt; //俯仰速度信息(相对载体) + SPEEDXYZ32F fObjSpeedXYZ; //目标在地心直角坐标系下的绝对速度 + float nDistance; //告警目标距离估计值 +}ALARM_PARA; + +typedef struct +{ + float fAz; + float fPz; + float fservoAzSpeed; //当前帧伺服方位角速度，横滚角速度 + float fservoPtSpeed; //当前帧伺服俯仰角速度 +}Servo_Control; + +typedef struct stdWgs84GPS +{ + double Lon; //���� + double Lat; //γ�� + double Height; //�߶� +}STD_WGS84_GPS; +typedef struct stdSpeed //BYTES: 2*4=8 +{ + double vx; //x�����ٶ� + double vy; //y�����ٶ� + double vz; //z�����ٶ� +}STD_SPEED; +typedef struct stdPoint64d +{ + double X; //����ֱ������ϵX + double Y; //����ֱ������ϵY + double Z; //����ֱ������ϵZ +}STD_POINT64D; +typedef struct stdFilter_State +{ + //��صع̵���ֱ������ϵϵ + STD_WGS84_GPS nKalManBLH; //Ŀ��������ϵBLH��WGS84����Ϣ�����ƽ�� + STD_POINT64D pfKalmanPos; //Ŀ���˲�����λ�� //BYTES: 2*4=8 + STD_WGS84_GPS nKalManBLHNext; //Ŀ����һ֡�������ϵBLH��WGS84����Ϣ�����ƽ�� + STD_POINT64D pfKalmanPosNext; //Ŀ����һ֡�˲�����λ�� //BYTES: 2*4=8 + STD_POINT64D fKalmanSpeed; //Ŀ���˲������ٶ� x\y\z���� //BYTES: 2*4=8 + STD_POINT64D fKalmanAccSpeed; //Ŀ���˲����Ƽ��ٶ� x\y\z���� //BYTES: 2*4=8 +}STD_FILTER_ST; + +typedef struct stdServo +{ + double fAz; //方位角 + double fPt; //俯仰角 + double nDistance; //目标激光距离 +}STD_SERVO; +typedef struct stdFilterVideoOutput //BYTES: 3*1 + 1*2 + 7*4 + 3*8 = 57 +{ + unsigned char nOutputNums; //输出滤波目标个数 + bool bObject; //标识目标是否已被创建。1-目标已创建，0-目标未创建 + unsigned int unFrmTime; //目标截获时刻 + unsigned int unFrmID; //帧编号 + //目标滤波信息 + STD_WGS84_GPS nObjBLH; //大地坐标系wgs84 目标经纬高 + STD_SPEED pfObjSpeed; //目标地心直角坐标系下的绝对速度 + STD_POINT64D pfObjCurrent; //目标的大地直角空间坐标系 + STD_FILTER_ST pfObj_filterInfo; //目标估计状态 + //载体信息 + STD_WGS84_GPS nPlatBLH; //大地坐标系wgs84 目标经纬高 + STD_SPEED pfPlatSpeed; //目标地心直角坐标系下的绝对速度 + STD_POINT64D pfPlatCurrent; //目标的大地直角空间坐标系 + STD_FILTER_ST pfPlat_filterInfo; //载体估计状态 + //估计信息 + STD_SERVO pfObjZtAangle; //目标在载体坐标的指向 + STD_SERVO pfObjServeAangle; //目标在伺服坐标的指向 + //预测信息 + STD_SERVO pfObjZtAangleNext; //目标在载体坐标的指向 + STD_SERVO pfObjServeAangleNext; //目标在伺服坐标的指向 + //量测信息 + STD_SERVO pfServeAangle; //伺服码盘值 //输入值【可不移植】 + + //长时间外推 + STD_SERVO pfServeAanglePredict[10]; //伺服长时间外推,10s + STD_WGS84_GPS nPlatPreictBLH[10]; + STD_WGS84_GPS nObjPredictBLH[10]; +}STD_FILTER_VIDEOOUTPUT; +typedef struct Angle2DSpeed32f +{ + double fPt;//俯仰角 + double fAz;//方位角 +}ANGLE2DSPEED32F; + +//2.大地坐标系WGS84_GPS结构体 +typedef struct tagGPS +{ + float Lon; //经度 + float Lat; //纬度 + float Height; //高度 +} WGS84_GPS; + +//载机姿态角度结构体 +typedef struct tagAngle3D32F +{ + float fAz; //偏航角 + float fPt; //俯仰角 + float fRt; //滚转角 +}ANGLE3D32F; +//融合信息 +typedef struct tagFusionTarget +{ + bool bOccupy; //标识管道是否已经被占用 + unsigned char nTargetID; //目标批号 + unsigned char nObjClassID; //目标类别 + float score; //融合后目标识别的置信度 + + //目标信息（未定位时，以下3个结构体变量输出为0） + WGS84_GPS ObjBLH; //目标大地坐标系BLH（WGS84）信息 + POINT_3D ObjXYZ; //目标地心直角坐标系下XYZ + SPEEDXYZ32F fObjSpeedXYZ; //目标地心直角坐标系下的绝对速度 + + //本机载机信息 + WGS84_GPS PlaneBLH; //载机大地坐标系BLH（WGS84）信息） + POINT_3D fPlanePnt; //本机位置 + SPEEDXYZ32F sfPlaneSpeed; //本机速度 + ANGLE3D32F fPlaneAngle; //载机姿态（机体坐标系） + + //目标融合位置信息：地固地心坐标系 + POINT_3D ptBornPos; //目标初始位置 + POINT_3D ptPredictPos; //目标预测位置 + + //只能提供已定位的本机以及外机的光电目标信息，外机未定位目标无法提供相对本机的方位、俯仰 + //(缺少距离信息，或这友机信息只提供定位后的目标信息) + float fAz; //目标方位角（相对于载体，定位目标的相对角度） + float fPt; //目标俯仰角（相对于载体） + ANGLE2DSPEED32F sfSpeed; //目标速度 + //ANGLE2DSPEED32F sfPredictAngle; //目标角度位置 + bool bLocation; //已定位数据标志位定位模块可以提供 + bool JMBMonitor; //敌方是否投放干扰弹（透传） + bool MissMonitor; //敌方是否发射导弹（透传） +}FUSION_OBJECT; + +//目标意图 +typedef struct TarGetPurpose +{ + unsigned char ucFoeState; //敌机状态，具体为上面几种宏定义的状态 + float fConfidence; //当前意图判断的置信度（作为备用字段） +}TarGetPurpose; + +//意图结果 +typedef struct FusedDataDst +{ + unsigned char ucNumber; //敌目标编号 + unsigned char ucClassType; //敌目标类型, 1防空导弹车，2雷达车，3高炮，4飞机，5是装甲、汽车、油罐车等地面车辆 + + //事件信息 + bool bFoeRadarIsOpen; //雷达是否展开 + bool bFoeMissileIsOpen; //导弹车是否竖起 + bool bFoeCannonIsopen; //高炮是否竖起 + bool bFoeCarsIsAround; //高炮或防空导弹是否散开 + bool bFoeCarsIsArrange; //高炮或防空导弹是否成一字型 + bool bFoeIsFire; //敌是否发射导弹或高炮 + float fFoeSpeed; //敌速度 + float fFoeDistance; //敌我距离 + float fAzimuthAng; //目标方位角 + float fPitchingAng; //目标俯仰角 + POINT_3D FoeLocation; //目标坐标 + POINT_3D ObjPrePath[10]; //目标预测轨迹 + // POINT_3D FoeGroundLoc[10]; //地面遭遇位置 + + float fOurPlaneSpeed; //我机飞行速度 + bool bOurPlaneSearchRadar; //我机是否开启搜索雷达 + bool bOurPlaneTrackRadar; //我机是否开启跟踪雷达 + bool bOurMissile; //我机是否发射导弹 + POINT_3D OurPlaneLocation; //我机坐标 + + TarGetPurpose ObjPurPose; //目标意图 +}FusedDataDst; + + + +//威胁等级排序 +typedef struct ParaThreatOutput +{ + char bDangerLevel; //目标威胁等级(编号为1，目标威胁等级最高) + char uObjID; //威胁目标对应的编号 + float fThreat; //目标威胁程度（值越大等级越小，即威胁程度越高） +}PTOutput; + + +//激光测距值 +typedef struct +{ + int type; //激光测距类型 + int freq; //激光测距频率 + double laserDistance; //激光测距值 +}LaserInfo; + + +typedef enum tagGLB_VIDE_TYPE +{ + xGLB_VIDEO_IR_SW = 0x01, //红外短波 + xGLB_VIDEO_IR_MW = 0x02, //红外中波 + xGLB_VIDEO_IR_LW = 0x03, //红外长波 + xGLB_VIDEO_VL = 0x04 //电视 +}GLB_VIDE_TYPE; + +//相机信息 +typedef struct +{ + GLB_VIDE_TYPE unVideoType; //视频源类型 + float dx; //光轴与图像中心像素偏差x + float dy; //光轴与图像中心像素偏差y + int ImgWidth; //图像分辨率宽 + int ImgHeight; //图像分辨率高 + float nPixleSize; //像元尺寸 单位：um + float nFocal; //相机焦距 单位：mm +}CamInfo1; + + +//载机信息 +typedef struct +{ + unsigned char nPlaneID; //载机ID + double B; //纬度 + double L; //经度 + double H; //高 + double fRoll; //横滚角 + double fPitch; //俯仰角 + double fYaw; //方位角 +}AirCraftInfo1; + + +// //伺服信息 +// typedef struct +// { +// float fServoAz; //当前帧伺服方位角 +// float fServoPt; //当前帧伺服俯仰角 +// float fServoAzSpeed; //当前帧伺服方位角速度 +// float fServoPtSpeed; //当前帧伺服俯仰角速度 +// }ServoInfo1; +//UTC时间 +typedef struct TIME +{ + unsigned short year; //年，0-65535 + unsigned char month; //月，1-12 + unsigned char day; //日，1-31 + unsigned char hour; //时，0-23 + unsigned char minute; //分，0-59 + unsigned char second; //秒，0-59 + unsigned short msecond; //毫秒，0-990，LSB=10ms +}UTC_TIME; + +//跟踪目标输出结构体 +typedef struct tagPARALINE_OUTPUT +{ + unsigned int index; + CamInfo1 caminfo; //相机信息 + AirCraftInfo1 airCraftInfo; //载机信息 + Servo_Control servoInfo; //伺服信息 + GeogGuideInfo stGuideInfo; //地理引导信息 + LaserInfo stLaserInfo; //激光测距 + + + //*************************** 各模块算法结果与状态 **************************** + uint32_t str_identifyIR_obj_cnt; //红外图像智能识别目标个数 + OBJ_INFO str_identifyIR_objs[50]; //红外图像智能识别目标结果 + uint32_t str_identifyVL_obj_cnt; //可见光图像智能识别目标个数 + OBJ_INFO str_identifyVL_objs[50]; //可见光图像智能识别目标结果 + + uint32_t m_segmentIR_obj_cnt; //红外地物分类个数 + OBJ_INFO m_segmentIR_objs[10]; //红外地物分类目标 + uint32_t m_segmentVL_obj_cnt; //可见光地物分类个数 + OBJ_INFO m_segmentVL_objs[10]; //可见光地物分类目标 + + // uint32_t m_ChangeinfoIR_obj_cnt; //红外变化检测个数 + // variationInfo m_ChangeinfoIR_objs[30]; //红外变化检测目标 + // uint32_t m_ChangeinfoVL_obj_cnt; //可见光变化检测个数 + // variationInfo m_ChangeinfoVL_objs[30]; //可见光变化检测目标 + + + xTRACK_OUTPUT m_trackIR_objs; //红外跟踪目标结果 + xTRACK_OUTPUT m_trackVL_objs; //可见光跟踪目标结果 + + int ObjCnts_IR; //目标总个数（融合后红外视场内） + OBJINFO_RECT1 AIDetFusion_IR[50]; //红外图上AI融合结果 + int ObjCnts_VL; //目标总个数（融合后可见光视场内） + OBJINFO_RECT1 AIDetFusion_VL[50]; //可见光图上AI融合结果 + + Guide m_geogGuideInfo; //地理引导结果 + + uint32_t m_activeLoc_cnt; //多目标有源定位个数 + stTrgOutput1 m_mulActiveLoc[50]; //多目标定位结果 + + stTrgOutput1 m_singleActiveLoc; //单目标有源定位测速结果 + + stTrgOutput1 m_passiveLocate[50]; //无源定位结果 + + // uint32_t m_Alarm_obj_cnt; //红外告警目标个数 + // ALARM_PARA m_Alarm_Papa[16]; //红外告警目标结果 + + // uint32_t m_Filter_obj_cnt; //航迹滤波目标个数 + // STD_FILTER_VIDEOOUTPUT objFilterRes[50]; //航迹滤波结果 + + // unsigned int nFusionNumber; //融合后目标总个数 + // FUSION_OBJECT objFusionList[60]; //融合输出结果 + + // int targetPurpnum; //目标意图个数 + // FusedDataDst fusedatasrc[50]; //意图识别结果(输入透传输出) + + // int m_threadLevel_cnt; //威胁等级目标个数 + // PTOutput m_threadLevel_obj[100]; //威胁等级排序结果 + + UTC_TIME utcTime_laser; + UTC_TIME utcTime_ir; + +}PARALINE_OUTPUT; + + +typedef struct S729paras_VL +{ + //unsigned char IMG_VL[IMAGE_WIDTH_VL * IMAGE_HEIGHT_VL * (3 / 2)]; + PARALINE_OUTPUT Paras_VL; + +}Paras_VL; + +typedef struct S729paras_IR +{ + //unsigned char IMG_IR[IMAGE_WIDTH_IR * IMAGE_HEIGHT_IR * 2]; + PARALINE_OUTPUT Paras_IR; + +}Paras_IR; + +//TRACK_OUTPUT outputIR = { 0 }; +//TRACK_OUTPUT outputVL = { 0 }; + + +//#pragma pack() \ No newline at end of file diff --git a/doc/0104002.pdf b/doc/0104002.pdf new file mode 100644 index 0000000..b01d00d Binary files /dev/null and b/doc/0104002.pdf differ diff --git a/main.cpp b/main.cpp new file mode 100644 index 0000000..93821f2 --- /dev/null +++ b/main.cpp @@ -0,0 +1,161 @@ +#include +#include "API_VideoStitch.h" +#include "S729.h" +#include +#include "opencv2/opencv.hpp" +using namespace std; +using namespace cv; + +void Map16BitTo8Bit(unsigned short *psh16BitData, long lDataLen, BYTE *pby8BitData) +{ + if (psh16BitData == NULL || pby8BitData == NULL || lDataLen <= 0) + { + return; + } + + //指向直方图的数据指针 + int *pHist = new int[65536]; + memset(pHist, 0, 65536 * sizeof(int)); + + int i = 0; + for(i = 0; i < lDataLen; i+=10) + { + pHist[psh16BitData[i]]++; + } + + //设置阈值大小为: AreaSigma*图象大小/100 + int nSigma = 0.02*lDataLen; + int nSum = 0; + int nMin = 0; + int nMax = 0; + + //求映射的最大最小值 + for(i = 0; i < 65536; i++) + { + nSum += pHist[i]; + if(nSum >= nSigma) + { + nMin = i; + break; + } + } + + nSum = 0; + + for(i = 65535; i >= 0; i--) + { + nSum += pHist[i]; + if(nSum >= nSigma) + { + nMax = i; + break; + } + } + + //计算对比度亮度 + float K = (float)(120.0/(nMax - nMin + 1)); + float C = (float)(-K * nMin); + + //图像映射 + for (i = 0; i < lDataLen; i++) + { + int nValue = (int)(K * psh16BitData[i] + C); + if (nValue < 0) + { + pby8BitData[i] = 0; + } + else if (nValue > 255) + { + pby8BitData[i] = 255; + } + else + { + pby8BitData[i] = nValue; + } + } + + delete[] pHist; +} + + +// 红外帧 +unsigned short pFrameIR[IMAGE_WIDTH_IR*(IMAGE_HEIGHT_IR + PARA_IR_LINE)] = {0}; +unsigned char pImageIR[IMAGE_WIDTH_IR*(IMAGE_HEIGHT_IR)] = {0}; + +int main(int, char**) +{ + auto stitcher = API_VideoStitch::Create(IMAGE_WIDTH_IR,IMAGE_HEIGHT_IR); + //stitcher->Test(); + + + FILE* file = fopen("../20241219152917_4.xraw","rb"); + + + for (size_t i = 0; i < 900; i++) + { + + fread(pFrameIR,2,IMAGE_WIDTH_IR*(IMAGE_HEIGHT_IR + PARA_IR_LINE),file); + + if(i % 3 != 0) + { + continue; + } + + S729paras_IR Paras_IR = { 0 }; + memcpy(&Paras_IR, (unsigned char*)(pFrameIR + IMAGE_WIDTH_IR*IMAGE_HEIGHT_IR), sizeof(S729paras_IR)); + + FrameInfo info = {0}; + info.camInfo.nFocus = Paras_IR.Paras_IR.caminfo.nFocal; + info.camInfo.fPixelSize = Paras_IR.Paras_IR.caminfo.nPixleSize; + + info.craft.stAtt.fYaw = Paras_IR.Paras_IR.airCraftInfo.fYaw; + info.craft.stAtt.fPitch = Paras_IR.Paras_IR.airCraftInfo.fPitch; + info.craft.stAtt.fRoll = Paras_IR.Paras_IR.airCraftInfo.fRoll; + + info.craft.stPos.B = Paras_IR.Paras_IR.airCraftInfo.B; + info.craft.stPos.L = Paras_IR.Paras_IR.airCraftInfo.L; + info.craft.stPos.H = Paras_IR.Paras_IR.airCraftInfo.H; + + + info.nEvHeight = Paras_IR.Paras_IR.airCraftInfo.H; + + info.servoInfo.fServoAz = Paras_IR.Paras_IR.servoInfo.fAz; + info.servoInfo.fServoPt = Paras_IR.Paras_IR.servoInfo.fPz + 90; + + info.nWidth = IMAGE_WIDTH_IR; + info.nHeight = IMAGE_HEIGHT_IR; + + Map16BitTo8Bit(pFrameIR,IMAGE_WIDTH_IR*IMAGE_HEIGHT_IR,pImageIR); + + cv::Mat src(IMAGE_HEIGHT_IR,IMAGE_WIDTH_IR,CV_8UC1,pImageIR); + + if(i == 0) + { + stitcher->Init(info); + } + else + { + // info.craft.stAtt.fYaw = 0; + // info.craft.stAtt.fPitch = 0; + // info.craft.stAtt.fRoll = 0; + // info.servoInfo.fServoAz = 0; + //info.servoInfo.fServoPt = 0; + + cout << info.craft.stPos.B << " " <Updata(src,info); + + + } + + imshow("src", src); + waitKey(1); + + } + + + + +} diff --git a/public_include/AI_API.h b/public_include/AI_API.h new file mode 100644 index 0000000..06f7771 --- /dev/null +++ b/public_include/AI_API.h @@ -0,0 +1,274 @@ +/* + * @Author: turboLIU + * @Date: 2022-11-02 15:04:08 + * @LastEditTime: 2024-10-25 09:17:48 + * @Description: Do not edit + * @FilePath: /Algo_ACL_Common/API/AI_API.h + */ +#ifndef AI_API +#define AI_API +#include +#include "detection_type_api.h" + +using namespace AIGO; + + +#ifdef __cplusplus +extern "C"{ +#endif +/** + * @brief 算法启动前，最先年调用该函数，用于设置NPU硬件环境 + * + * @return int 非0表示环境异常 + */ +int Init_ACL(); +/** + * @brief Set the deque size object + * + * @param irdetin + * @param visdetin + * @param irsegin + * @param vissegin + * @param irdetout + * @param visdetout + * @param irsegout + * @param vissegout + * @param irdetres + * @param visdetres + * @param irsegres + * @param vissegres + * @return int + */ +int set_deque_size(int irdetin=3, int visdetin=3, int irsegin=3, int vissegin=3, int irdetout=5, int visdetout = 5, int irsegout = 5, int vissegout = 5, + int irdetres = 10, int visdetres = 10, int irsegres = 10, int vissegres = 10); +int get_deque_size(int &irdetin, int &visdetin, int &irsegin, int &vissegin, int &irdetout, int &visdetout, int &irsegout, int &vissegout, + int &irdetres, int &visdetres, int &irsegres, int &vissegres); +int reset_dequeu_size(int irdetin=30, int visdetin=30, int irsegin=30, int vissegin=30, int irdetout=50, int visdetout = 50, int irsegout = 50, int vissegout = 50, + int irdetres = 100, int visdetres = 100, int irsegres = 100, int vissegres = 100); +/** + * @brief Set the stream size object + * init 函数调用之前，设置模型宽高；默认 1280， 1024 + * + * @param width 模型宽 默认1280 + * @param height 模型高 默认1024 + * @return int + */ +int set_stream_size(int width, int height, MODE mode); +int get_stream_size(int &width, int &height, MODE mode); +int reset_stream_size(int width, int height, MODE mode); +/** + * @brief 同时初始化检测和分割模型 + * + * @param irdetmodel 红外检测模型 + * @param visdetmodel 可见光检测模型 + * @param irsegmodel 红外分割模型 + * @param vissegmodel 可见光分割模型 + * @return int + */ +int Init_AI(const char* irdetmodel, const char* visdetmodel, const char* irsegmodel, const char* vissegmodel); + +int Det_VL(ImgMat frame, std::vector &outputs); + +int Det_IR(ImgMat frame, std::vector &outputs); + +int Seg_VL(ImgMat frame, TenMat* output); + +int Seg_IR(ImgMat frame, TenMat* output); + +/** + * @brief 运行检测分割算法，异步接口，调用get_result函数获取结果 + * + * @param img + * @param GoDet 是否执行检测算法 + * @param GoSeg 是否执行分割算法 + * @param cropFlag 是否中心裁减，裁减大小通过 set_ROIArea 设置 + * @return int + */ +int runAsync(ImgMat img, bool GoDet=true, bool GoSeg=true, bool cropFlag=false); +/** + * @brief Get the result object，获取已异步结果 + * + * @param timestamp + * @param dets + * @param maskImg + * @param front: true: 先进先出， false: 后进先出 + * @param single true：仅获取当前光源的算法结果， false：获取双光结果 + * @return int -1:队列被释放； 1: 队列为空; 0: 成功从队列获取结果； + */ +int get_result(long long ×tamp, std::vector &dets, TenMat * maskImg, bool front=true, MODE mode=VIS); // async +/** + * @brief 由可见光为主光源，切换至红外为主光源 + * + * @return int + */ +int switch_vis_to_ir(); +/** + * @brief 由红外为主光源，切换至可见光为主光源 + * + * @return int + */ +int switch_ir_to_vis(); +/** + * @brief Set the dropout param + * + * @param drop true: drop deque front data, false: not drop data, 默认: false + * @return int + */ +int set_dropout_param(bool drop); +/** + * @brief Get the dropout param + * + * @param drop + * @return int + */ +int get_dropout_param(bool &drop); +/** + * @brief 设置分割类型，可时时生效 + * + * @param mode IR or VIS + * @param road segment road or not + * @param bridge segment bridge or not + * @param water segment water or not + * @param building segment building or not + * @return int + */ +int set_seg_classes(MODE mode, bool road, bool bridge, bool water, bool building); + +/** + * @brief Set the nms threshold object + * + * @param nms + * @return int + */ +int set_nms_threshold(float nms); +/** + * @brief Get the nms threshold object + * + * @param nms + * @return int + */ +int get_nms_threshold(float& nms); +/** + * @brief Set the det threshold object + * + * @param threshold 置信度 + * @param idx 1～15，设置哪一个种类的置信度，种类编号参见CLASSTYPE + * @return int + */ +int set_det_threshold(float threshold, int idx); +/** + * @brief Get the det threshold object + * + * @param threshold 置信度 + * @param idx 1～15，设置哪一个种类的置信度，种类编号参见CLASSTYPE + * @return int + */ +int get_det_threshold(float &threshold, int idx); +/** + * @brief Set the seg threshold object + * + * @param thr + * @return int + */ +int set_seg_threshold(float thr); +/** + * @brief Get the seg threshold object + * + * @return float + */ +float get_seg_threshold(); +/** + * @brief 设置数据流模式，仅支持: BGR，RGB，NV12， NV21； 默认RGB + * + * @param m + * @return int + */ +int set_StreamMode(INPUT_LAYOUT m); +/** + * @brief Get the StreamMode object + * + * @return INPUT_LAYOUT + */ +INPUT_LAYOUT get_StreamMode(); + +/** + * @brief 算法关闭时调用，释放算法内部资源 + * + * @return int + */ +int Uninit_AI(); +/** + * @brief only deinit hardware + * + * @return int + */ +int Uninit_ACL(); +/** + * @brief Set the ROIArea object + * + * @param x1 lefttop + * @param y1 lefttop + * @param x2 botright + * @param y2 botright + * @return int + */ +int set_ROIArea(int x1, int y1, int x2, int y2); + +int setLabelMap(uint32_t A[], uint32_t B[], uint32_t len); + + +class DETECTION_API +{ +private: + /* data */ +public: + DETECTION_API(/* args */); + ~DETECTION_API(); +public: + + int netWidth=0; + int netHeight = 0; + int clsnum = 0; + std::vector> anchors;// = {{7,11, 9,21, 21,15},{16,39, 31,24, 57,27},{43,59, 90,59, 131,116}}; + + void* AIGOhandle=NULL; + + int AIGOinit(const char* modename, int deviceID=0); + int AIGOpreprocess(ImgMat img, bool doCopy=true); + int AIGOinfer(); + int AIGOpostprocess(std::vector &results); + int AIGOdeinit(); +}; + +class SEGMENTION_API +{ +private: + /* data */ +public: + SEGMENTION_API(/* args */); + ~SEGMENTION_API(); + + int netWidth = 0; + int netHeight = 0; + int clsnum = 0; + + void* AIGOhandle = NULL; + + int AIGOinit(const char* modename, int deviceID=0); + int AIGOpreprocess(ImgMat img, bool doCopy=true); + int AIGOinfer(); + int AIGOpostprocess(); + int AIGOdeinit(); +}; + + + + + + + +#ifdef __cplusplus +} +#endif + +#endif \ No newline at end of file diff --git a/public_include/Arith_CommonDef.h b/public_include/Arith_CommonDef.h new file mode 100644 index 0000000..3677bf7 --- /dev/null +++ b/public_include/Arith_CommonDef.h @@ -0,0 +1,601 @@ +#ifndef _ARITH_COMMONDEF_ +#define _ARITH_COMMONDEF_ + + +#include +/*********版权所有（C）2020，武汉高德红外股份有限公司*************************************** +* 文件名称： Arith_CommonDef.h +* 文件标识：通用数据结构定义 +* 内容摘要： +* 其它说明： +* 当前版本： +* 创建作者： +* 创建日期：2020/12/05 +* 包含关系： +*****************************************************************************************/ + +typedef void* ArithHandle; // 算法句柄类型 + + +// 跟踪器输入规模定义，注意栈空间 +#define INPUT_OBJ_NUM (100) //外部最大输入目标个数 + +// 跟踪器输出规模定义 +#define ST_OBJ_NUM (100) //短时目标个数 +#define LT_OBJ_NUM (10) //长时跟踪目标个数 + + +//外部系统状态（总体下发） +typedef enum tagGLB_SYS_MODE +{ + GLB_SYS_WAIT = 0x01, // 空闲 + GLB_SYS_STARE = 0x02, // 凝视 + GLB_SYS_FSCAN = 0x03, // 扇扫 + GLB_SYS_AUTO = 0x04, // 自主搜跟（无人值守） + GLB_SYS_SCAN = 0x05 // 周扫 +}GLB_SYS_MODE; + +// 场景模式 +typedef enum tagGLB_SCEN_MODE +{ + GLB_SCEN_SKY = 0x01, // 对空模式 + GLB_SCEN_GROUND = 0x02, // 对地模式 + GLB_SCEN_SEA = 0x03, // 对海模式 + GLB_SCEN_NONE = 0x00, // 未设置 + GLB_SCEN_LAB = 0x04, // 实验室靶标 + GLB_SCEN_CUSTOM = 0x05 // 自定义模式：用于算法开发验证，不稳定版本 add @ 20241010 +}GLB_SCEN_MODE; + +// 外引导来源 +typedef enum tagGLB_GUIDE_SOURCE +{ + GLB_GUIDE_RADAR = 0x01, //载机雷达 + GLB_GUIDE_WARNINGPLANE = 0x02 //预警机 +}GLB_GUIDE_SOURCE; + + +// 视频源 +typedef enum tagGLB_VIDEO_TYPE +{ + GLB_VIDEO_IR_SW = 0x01,//红外短波 + GLB_VIDEO_IR_MW = 0x02,//红外中波 + GLB_VIDEO_IR_LW = 0x03,//红外长波 + GLB_VIDEO_VL = 0x04 //电视 +}GLB_VIDEO_TYPE; + + +// 红外截波段类型，用于多光谱目标识别，根据需要添加 +typedef enum tagGLB_IR_BAND_TYPE +{ + GLB_BAND_MW_0 = 0x020,//中波全色 + GLB_BAND_MW_1 = 0x021,//中波1 + GLB_BAND_MW_2 = 0x022//中波2 +}GLB_IR_BAND_TYPE; + + + + +// 捕控导引阶段定义（算法内部状态） +typedef enum tagGLB_STATUS +{ + GLB_STATUS_UNKOWN = 0, //捕控导引阶段 - 未知 + GLB_STATUS_WAIT = 1, //捕控导引阶段 - 待命状态 + GLB_STATUS_SEARCH = 2, //捕控导引阶段 - 搜索状态 + GLB_STATUS_TRACK = 3, //捕控导引阶段 - 跟踪 + GLB_STATUS_SCAN = 4, //捕控导引阶段 - 周扫 + GLB_STATUS_LOST = 5, //捕控导引阶段 - 跟踪丢失 + GLB_STATUS_FSCAN = 6, //捕控导引阶段 - 扇扫 + GLB_STATUS_MOTRACK = 9, //多目标跟踪状态 + GLB_STATUS_AIM = 10 //内校轴阶段 +}GLB_STATUS; + +// 算法标识 +typedef enum tagemObjSrc +{ + Arith_DST = 1, + Arith_DAT = 2, + Arith_AI = 3, + Arith_KCF = 4, + Arith_TLD = 5, + Arith_Manual = 6, + Arith_SiamRPN = 7, + Arith_NanoTrack = 8, + Arith_VitTrack = 9 +}ObjSrc; + +// 尺度模式 +typedef enum tagSizeType +{ + unKnown, //不确定目标或未识别尺寸类型 + DDimTarget, //超级弱小目标 + DimTarget, //弱小目标 + SmallTarget, //小目标 + MiddleTarget, //临界目标 + AreaTarget //面目标 +}SizeType; + +// 灰度模式 +typedef enum tagGrayType +{ + BrightTarget = 1, //亮目标 + DarkTarget = 2, //暗目标 + AllGray = 3 //亮暗目标 +}GrayType; + + + +// 目标跟踪状态 +typedef enum tagTrackingStatus +{ + NOT_TRACKING = 0,//未跟踪 + STABLE_TRACKING = 1,//稳跟 + MEM_TRACKING = 2//记忆跟踪 +}TrackingStatus; + + +// 遮挡事件等级判断 +typedef enum tagOccLev +{ + NO_OCC = 0, //无遮挡 + SIM_OCC = 1,//轻微遮挡 + MID_OCC = 2,//中等遮挡 + FULL_OCC = 3//完全遮挡 +}OccLev; + +// 目标形变事件 +typedef enum tagShapeTransLev +{ + NO_ShapeTrans = 0,//无形变 + MID_ShapeTrans = 1,//轻微形变 + FULL_ShapeTrans = 2//显著形变 +}ShapeTransLev; + + +// 跟踪事件(可包含所有跟踪期间需要特殊处理的事件 +// 如强制记忆跟踪、遮挡、形变、变焦、变倍等) +typedef struct tagTrackEvent +{ + OccLev occ; + ShapeTransLev shapeTrans; +}TrackEvent; + +//跟踪决策事件 +typedef enum tagTrackDecision +{ + NO_Deci = 0, //无决策结果 + KCF_Deci = 1, //KCF + AIT_Deci = 2, //AI + AID_Deci = 3, //AI识别 + Predict_Deci =4 //长时预测 +}TrackDecision; + +// 定义红外目标类别编号 +#define DT_TARGET_CLS_ID 99 //红外检测无目标区分能力，赋值为99 + + +//////////////////////////////////////////////////////////////////////////////// +// 系统符号定义 +//////////////////////////////////////////////////////////////////////////////// +typedef bool BBOOL; +typedef unsigned char UBYTE8; +typedef signed char BYTE8; +typedef unsigned short UINT16; +typedef short SINT16; +typedef unsigned int UINT32; +typedef int SINT32; +typedef unsigned long COLORREF; +typedef unsigned short WORD; +typedef unsigned long DWORD; +typedef unsigned char BYTE; +typedef float FLOAT32; +typedef double DOUBLE64; +typedef long double LDOUBLE80; + +//////////////////////////////////////////////////////////////////////////////// +// 系统数据结构 +//////////////////////////////////////////////////////////////////////////////// +// 定义坐标结构体--整型32位 +typedef struct tagPoint32S // BYTES: 2*4=8 +{ + SINT32 x; // 图像像素x坐标 + SINT32 y; // 图像像素y坐标 +} POINT32S; + +// 定义坐标结构体--整型16位 +typedef struct tagPoint16S // BYTES: 2*2=4 +{ + SINT16 x; // 图像像素x坐标 + SINT16 y; // 图像像素y坐标 +} POINT16S; + +// 定义坐标结构体--浮点型 +typedef struct tagPoint32F // BYTES: 2*4=8 +{ + FLOAT32 x; // 图像像素x坐标 + FLOAT32 y; // 图像像素y坐标 +} POINT32F; + + +// RPY姿态角 +typedef struct tgEulerRPY +{ + double fRoll; // 横滚角 + double fPitch; // 俯仰角 + double fYaw; // 方位角 +} EulerRPY; // 单位：° + + + +// 极坐标系 +typedef struct tgPointPole +{ + double alpha; // 俯仰角 + double beta; // 方位角 + double distance; // 极径 +} Pole; // 单位：° + + + +// 空间直角坐标系,也可以看做向量 +typedef struct tgPointXYZ +{ + double X; + double Y; + double Z; +} PointXYZ; // 单位：米 + + +typedef struct tgPointBLH +{ + double B; // 纬度 + double L; // 经度 + double H; // 高程 +} PointBLH; // 地理坐标系 //单位：° + + +// 相机内参 +typedef struct tgCameraParam +{ + int ImageWidth; + int ImageHeight; + float f; // 焦距 mm + float dSize; // 像元尺寸 um + int dx; // 像主点偏移x，0代表图像中心 + int dy; // 像主点偏移y，0代表图像中心 + float fAglReso;// 角分辨率，内部计算便于使用 + + bool bImageRataSys; //像旋系统标记，S731实物样机1，数字样机未模拟像旋 -0 +} CameraParam; + +// 伺服信息 +typedef struct tgServoParam +{ + float fAz; // 方位 + float fPt; // 俯仰 + + // S731扩展 + float fFSMAz; // 快反方位 + float fFSMPt; // 快反俯仰 +} ServoParam; + +typedef struct tgDeviceSetupError +{ + double g_YawBias; + double g_PitchBias; + double g_RollBias; + double g_XBias; + double g_YBias; + double g_ZBias; +} DeviceSetupError; + +// 定义目标宽高结构体--32位 //BYTES: 3*4=12 +typedef struct tagSize32S +{ + SINT32 w; // 宽度 + SINT32 h; // 高度 + SINT32 s; // 面积 + +} SIZE32S; + +// 定义目标宽高结构体--浮点型 +typedef struct tagSize32F // BYTES: 3*4=12 +{ + FLOAT32 w; // 宽度 + FLOAT32 h; // 高度 + FLOAT32 s; // 面积 +} SIZE32F; + +//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ +// 定义速度结构体--整型32位 //BYTES: 2*4=8 +typedef struct tagSpeed32S +{ + SINT32 vx; // x方向速度 + SINT32 vy; // y方向速度 +} SPEED32S; + +// 定义速度结构体--浮点型 +typedef struct tagSpeed32F // BYTES: 2*4=8 +{ + FLOAT32 vx; // x方向速度 + FLOAT32 vy; // y方向速度 +} SPEED32F; + +//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ +// 定义矩形结构体--左上角坐标及宽高--整型16位 +typedef struct tagRect16S // BYTES: 4*2=8 +{ + SINT16 x; // 左上点x坐标 + SINT16 y; // 左上点y坐标 + SINT16 w; // 矩形宽度 + SINT16 h; // 矩形高度 +} RECT16S; + +// 定义矩形结构体--左上角坐标及宽高--整型32位 +typedef struct tagRect32S // BYTES: 4*4=16 +{ + SINT32 x; // 左上点x坐标 + SINT32 y; // 左上点y坐标 + SINT32 w; // 矩形宽度 + SINT32 h; // 矩形高度 +} RECT32S; + +// 定义矩形结构体--左上角坐标及宽高--浮点型 +typedef struct tagRect32F // BYTES: 4*4=16 +{ + FLOAT32 x; // 左上点x坐标 + FLOAT32 y; // 左上点y坐标 + FLOAT32 w; // 矩形宽度 + FLOAT32 h; // 矩形高度 +} RECT32F; + +//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ +// 定义矩形结构体--中心点坐标及宽高、大小-整型 +typedef struct tagCENTERRECT // BYTES: 5*2=10 +{ + SINT16 cx; // 中心点x坐标 + SINT16 cy; // 中心点y坐标 + SINT16 w; // 矩形宽度 + SINT16 h; // 矩形高度 + SINT32 s; // 目标大小 +}CENTERRECT; + +// 定义矩形结构体--中心点坐标及宽高、大小-整型32位 +typedef struct tagCENTERRECT32S // BYTES: 4*4=16 +{ + SINT32 cx; // 中心点x坐标 + SINT32 cy; // 中心点y坐标 + SINT32 w; // 矩形宽度 + SINT32 h; // 矩形高度 + SINT32 s; // 目标大小 +} CENTERRECT32S; + +// 定义矩形结构体--中心点坐标及宽高、大小-浮点型 +typedef struct tagCENTERRECT32F // BYTES: 5*4=20 +{ + FLOAT32 cx; // 中心点x坐标 + FLOAT32 cy; // 中心点y坐标 + FLOAT32 w; // 矩形宽度 + FLOAT32 h; // 矩形高度 + FLOAT32 s; // 目标大小 +} CENTERRECT32F; + +//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ +// 定义矩形结构体--左上角及右下角坐标 +typedef struct tagMINMAXRECT // BYTES: 4*2=8 +{ + SINT16 minX; // 左上点x坐标 + SINT16 minY; // 左上点y坐标 + SINT16 maxX; // 右下点x坐标 + SINT16 maxY; // 右下点y坐标 +} MINMAXRECT; + +// 定义矩形结构体--左上角及右下角坐标-整型32位 +typedef struct tagMINMAXRECT32S // BYTES: 4*4=16 +{ + SINT32 minX; // 左上点x坐标 + SINT32 minY; // 左上点y坐标 + SINT32 maxX; // 右下点x坐标 + SINT32 maxY; // 右下点y坐标 +} MINMAXRECT32S; + +// 定义矩形结构体--左上角及右下角坐标-浮点32位 +typedef struct tagMINMAXRECT32F // BYTES: 4*4=16 +{ + FLOAT32 minX; // 左上点x坐标 + FLOAT32 minY; // 左上点y坐标 + FLOAT32 maxX; // 右下点x坐标 + FLOAT32 maxY; // 右下点y坐标 +} MINMAXRECT32F; + +//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ +// 定义目标角度结构体 +typedef struct tagAngle32F // BYTES: 2*4=8 +{ + FLOAT32 fAz; // 方位角 + FLOAT32 fPt; // 俯仰角 + FLOAT32 Dr; // 极坐标角度 +} ANGLE32F; + + + +// 定义目标宽高结构体--16位 //BYTES: 2*2 + 1*4=8 +typedef struct tagSize16S +{ + SINT16 w; // 宽度 + SINT16 h; // 高度 + SINT32 s; // 面积 +} SIZE16S; + +// 定义距离结构体--整型32位 +typedef struct tagDist32S // BYTES: 2*4=8 +{ + SINT32 dx; // x方向距离 + SINT32 dy; // y方向距离 +} DIST32S; + +// 定义距离结构体--浮点型 +typedef struct tagDist32F // BYTES: 2*4=8 +{ + FLOAT32 dx; // x方向距离 + FLOAT32 dy; // y方向距离 +} DIST32F; + + +// 外部引导目标列表，用于批量转锁定 +typedef struct tagTargetGuide +{ + SINT32 ID; //目标批号，传递给锁定后目标 + UBYTE8 bIsCoLocate; //协同定位标记，直接透传到目标 + Pole stTargetPole; //目标极坐标 + PointBLH stTargetPos; //目标GPS坐标 + FLOAT32 fGuideAzSpeed; //实际锁定点方位角速度 + FLOAT32 fGuidePtSpeed; //实际锁定点俯仰角速度 + SINT32 nLockX; //锁定点当前图像坐标X + SINT32 nLockY; //锁定点当前图像坐标Y + BBOOL bInFOV; //在视场判断 +}TargetGuide; + +// 载体信息 +typedef struct tagAirCraftInfo +{ + unsigned char nPlaneID; //载机ID + PointBLH stPos; //位置 + EulerRPY stAtt; //姿态 +}AirCraftInfo; + +// 相机信息 +typedef struct tagCamInfo +{ + int nFocus; //实时焦距值 + float fPixelSize; //像元尺寸 + GLB_VIDEO_TYPE unVideoType; //视频源类型 + int dCamx; //像主点偏移x，0- 图像中心 ，不知道就填0 + int dCamy; //像主点偏移y，0- 图像中心 ，不知道就填0 + float fAglReso; //角分辨率 +}CamInfo; + +// 伺服状态 +typedef struct tagServoInfo +{ + float fServoAz; //当前帧伺服方位角 + float fServoPt; //当前帧伺服俯仰角 + float fServoAzSpeed; //当前帧伺服方位角速度 + float fServoPtSpeed; //当前帧伺服俯仰角速度 +}ServoInfo; + +// 引导信息 +typedef struct tagGuideInfo +{ + BBOOL bGuideUpdate; //引导当前帧更新标志（有效标志） + FLOAT32 fGuideYaw; + BBOOL fGuidePitch; + BBOOL fGuideDistance; + GLB_GUIDE_SOURCE nGuideInfSource; // 引导信息来源，本机雷达 +}GuideInfo; + + +//***************************************************************************** +#define AIT_DETECTION_MAX_NUM 50 + +typedef struct tagAITPATH +{ + //DasimaRPN + const char *siam_model; + const char *siam_cls; + const char *siam_reg; +}AIT_PATH; + + +typedef struct tagAITTARGET +{ + CENTERRECT32F tTarget; + float fScore; +}AIT_TARGET; + +// AI跟踪器类型 +typedef enum tagAITrackerType +{ + DaSaimRPN = 0, + NanoTrack = 1, + VitTrack = 2 +}AITrackerType; + + + +// nanotrack输出结构 +typedef struct tagTrack_Res_Nano +{ + RECT32S rect; // 跟踪器自带评估分值 + float score; // 跟踪器自带评估分值 + int peak_num; // 超过阈值的波峰数量 + float peak_ratio; // 超过阈值的波峰投影占比 + int peak_status; // 通过波峰图分析的干扰判断结果 1:有干扰 0：无干扰 + int disturb_status; // 通过CLS特征图二分类网络模型的干扰判断结果 1:有干扰 0：无干扰 +}Track_Res_Nano; + + +//定义ai跟踪器的输出 + +typedef struct tagAITInfo +{ + //*******工作状态******* + int nStatus; //空闲/跟踪/定位/重捕状态信息等 + int nErrorCode; //错误码编号 + int nFrameId; + //*****跟踪目标信息***** + int nX; //目标中心点图像坐标x + int nY; //目标中心点图像坐标y + int nObjW; //目标宽度 + int nObjH; //目标高度 + unsigned char ubIDType; //目标识别类别标签 + float fProb; //目标识别置信度 + + //******调试信息********* + unsigned char bInited; + int nTotalCnt; //跟踪帧数 + int nContiLostCnt; //丢失 + //int nDetectNum; + int nContiDetectNum; + + + RECT32F rfBox; //x,y 这里表示中心点 + RECT32F rfBoxPre; //x,y 这里表示中心点 + + float fX; //表示中心点 + float fY; + float fObjW; + float fObjH; + + + int nSearchX; //搜索区域中心X + int nSearchY; //搜索区域中心Y + int nSearchW; //搜索区域宽度W + int nSearchH; //搜索区域高度H + + + //******AI跟踪高得分检测结果******* + int nDetectNum; + AIT_TARGET nDetectArray[AIT_DETECTION_MAX_NUM]; + int nClusterNum; + CENTERRECT32F nSimilarArray[AIT_DETECTION_MAX_NUM]; + + //******AI nanotrack输出******* + Track_Res_Nano stNanoTrack_out; + + AITrackerType type;//AI跟踪算法类型标记 + +}AIT_OUTPUT; + + +//定义ai跟踪器的控制指令 +typedef struct tagAITCommand +{ + bool bTrack; //持续跟踪指令，清空0表明解锁 + bool bControlAIT; //AITrack可自循环跟踪，也可由决策结果指导跟踪 + unsigned char nInitMode; //1-AITrack自初始化跟踪 2-采用决策box初始化 + CENTERRECT32F InitBox; //初始化跟踪指令 + CENTERRECT32F TargetBox; //指示目标框，用于确认下一帧的搜索范围，范围在AI跟踪内部根据提示目标框自行计算 +}AIT_Command; + + +#endif \ No newline at end of file diff --git a/public_include/PlatformDefine.h b/public_include/PlatformDefine.h new file mode 100644 index 0000000..3580b38 --- /dev/null +++ b/public_include/PlatformDefine.h @@ -0,0 +1,77 @@ +/* + * @Author: Jacky + * @Date: 2024-06-19 10:37:58 + * @LastEditors: Jacky + * @LastEditTime: 2024-09-27 11:17:29 + * @FilePath: /GeneralTracker/Adaptation/PlatformDefine.h + */ +#ifndef _PLATFORM_DEFINE_H_ +#define _PLATFORM_DEFINE_H_ + +typedef enum _gd_pixel_format_e +{ + GD_PIXEL_FORMAT_NONE = 0, + GD_PIXEL_FORMAT_YUV420P, + GD_PIXEL_FORMAT_YUV422P, + GD_PIXEL_FORMAT_NV12, + GD_PIXEL_FORMAT_NV21, + GD_PIXEL_FORMAT_NV16, + GD_PIXEL_FORMAT_NV61, + GD_PIXEL_FORMAT_RGB_PLANAR, + GD_PIXEL_FORMAT_BGR_PLANAR, + GD_PIXEL_FORMAT_RGB_PACKED, + GD_PIXEL_FORMAT_BGR_PACKED, + GD_PIXEL_FORMAT_GRAY_Y8, + GD_PIXEL_FORMAT_GRAY_Y16, + GD_PIXEL_FORMAT_GRAY_Y16Y8, + GD_PIXEL_FORMAT_BUTT +} GD_PIXEL_FORMAT_E; + +typedef struct _gd_video_frame_s +{ + unsigned int u32Width; + unsigned int u32Height; + GD_PIXEL_FORMAT_E enPixelFormat; + unsigned int u32Stride[3]; + unsigned long long u64PhyAddr[3]; + unsigned char* u64VirAddr[3]; + unsigned int u32TimeRef; + unsigned int u32FrameCnt; + void* private_data; + void* param_data; +} GD_VIDEO_FRAME_S; + +inline float fixelBite(GD_PIXEL_FORMAT_E format){ + float val = 1.0; + switch (format) + { + case GD_PIXEL_FORMAT_YUV420P: + case GD_PIXEL_FORMAT_NV12: + case GD_PIXEL_FORMAT_NV21: + val = 1.5; + break; + case GD_PIXEL_FORMAT_YUV422P: + case GD_PIXEL_FORMAT_GRAY_Y16: + val = 2.0; + break; + case GD_PIXEL_FORMAT_RGB_PLANAR: + case GD_PIXEL_FORMAT_BGR_PLANAR: + case GD_PIXEL_FORMAT_RGB_PACKED: + case GD_PIXEL_FORMAT_BGR_PACKED: + val = 1.0; + break; + case GD_PIXEL_FORMAT_GRAY_Y16Y8: + val = 3.0; + break; + case GD_PIXEL_FORMAT_GRAY_Y8: + val = 1.0; + break; + default: + break; + } + return val; +} + + + +#endif \ No newline at end of file diff --git a/public_include/README.MD b/public_include/README.MD new file mode 100644 index 0000000..a6f8a4f --- /dev/null +++ b/public_include/README.MD @@ -0,0 +1,2 @@ +# AI识别库的接口文件，使用识别结果时需引用，如项目确实需要修改，联系AI识别库设计师。 +# AI跟踪与传统跟踪联系紧密，因此AI输出的结构体在NeoTracker中维护，必要时做协议转换。 \ No newline at end of file diff --git a/public_include/detection_type_api.h b/public_include/detection_type_api.h new file mode 100644 index 0000000..82379ce --- /dev/null +++ b/public_include/detection_type_api.h @@ -0,0 +1,131 @@ +/* + * @Author: turboLIU + * @Date: 2022-07-27 14:12:19 + * @LastEditTime: 2024-09-27 09:18:16 + * @Description: Do not edit + * @FilePath: /C/include/detection_type_api.h + */ +#ifndef DETECTION_TYPE_API +#define DETECTION_TYPE_API + + +#define NMS_UNION 1 +#define NMS_MIN 2 +#include +#include +#include "PlatformDefine.h" + +namespace AIGO{ + + + +typedef enum{ + IR = 1, + VIS = 2 +}MODE; + +typedef enum{ + CHW = 1, + HWC = 2, + WHC = 3, +}DATA_LAYOUT; + +typedef enum{ + RGB = 1, + BGR = 2, + RGBA = 3, + BGRA = 4, + NV12 = 5, // uv + NV21 = 6, // vu +}INPUT_LAYOUT; + + +typedef enum{ + AI_DT_UNDEFINED = -1, //未知数据类型，默认值。 + AI_FLOAT = 0, + AI_FLOAT16 = 1, + AI_INT8 = 2, + AI_INT32 = 3, + AI_UINT8 = 4, + AI_INT16 = 6, + AI_UINT16 = 7, + AI_UINT32 = 8, + AI_INT64 = 9, + AI_UINT64 = 10, + AI_DOUBLE = 11, + AI_BOOL = 12, + AI_STRING = 13, + AI_COMPLEX64 = 16, + AI_COMPLEX128 = 17, + AI_BF16 = 27 +}DATATYPE; + + +typedef struct tag_ID_idx +{ + /* data */ + int x_loc; + int y_loc; + int z_loc; + float score; +}ID_idx; + +typedef struct tag_IDX +{ + /* data */ + float x; + float y; + float z; + float score; +}IDX; + + +typedef struct tag_Objinfo +{ + /* data */ + float x1; + float y1; + float x2; + float y2; + float score; + int classNum; + int ID; + int trackID; + float area; + +}objinfo; + + + +typedef struct tag_ImgMat +{ + /* data */ + unsigned char* data=NULL; + int width=0; + int height=0; + int depth=0; + DATA_LAYOUT layout=HWC; + INPUT_LAYOUT inlayout = RGB; + int frameID=0; + MODE mode = IR; + // bool relayout=false; + long long timestamp = 0; + long long cfg = 0; // address of GD_VIDEO_FRAME_S structure +}ImgMat; + +typedef struct tag_TenMat +{ + /* data */ + void * data = nullptr; + int width=0; + int height=0; + int depth=0; + size_t dataByte=0; + DATATYPE type; + DATA_LAYOUT layout; + int elementNum = 0; + long long timestamp = 0; +}TenMat; + +} +#endif \ No newline at end of file diff --git a/script/Untitled.asv b/script/Untitled.asv new file mode 100644 index 0000000..22170d8 --- /dev/null +++ b/script/Untitled.asv @@ -0,0 +1,72 @@ +% 吊舱下视坐标转换 +format short g +% 相机信息 +imgW = 1280; +imgH = 1024; +f = 48; +dsize = 7.5; +fd = f/dsize*1000; + +%吊舱 +fAz = 44.1876869; +fPt = 18.3241043; + +% 姿态 +fYaw = 165.08250427246094; +fPitch = 4.2472858428955078; +fRoll = -0.37968909740447998; + +% 深度 +dH = 2920; + +M_G2I = getM_G2I( dH,fYaw, fPitch,fRoll,fAz,fPt,imgW,imgH,fd); + +M_I2G = inv(M_G2I); + +t = [0,0,1]; + +npoint = warpPoint([640,512],M_I2G,t); + + +[R1,t1] = invForm(M_I2G,t); + + +pxpoint = warpPoint(npoint',R1,t1); + + + +R1 * [npoint;1]/3144.6 + +for fAz = 44:5:60 + for fPt = 18:10:45 + M_G2I = getM_G2I( dH,fYaw, fPitch,fRoll,fAz,fPt,imgW,imgH,fd); + M_I2G = inv(M_G2I); + + p1 = M_I2G*[0,0,1]'; + p2 = M_I2G*[640,0,1]'; + p3 = M_I2G*[640,512,1]'; + p4 = M_I2G*[0,512,1]'; + + p1 = p1/p1(3); + p2 = p2/p2(3); + p3 = p3/p3(3); + p4 = p4/p4(3); + + p5 = p1; + x(1) = p1(1); x(2) = p2(1); x(3) = p3(1); x(4) = p4(1);x(5) = p5(1); + y(1) = p1(2); y(2) = p2(2); y(3) = p3(2); y(4) = p4(2);y(5) = p5(2); + + plot(x,y); + set(gca,'DataAspectRatio',[1 1 1]) + hold on + pause(0.1) + end +end + + + + + + + + diff --git a/script/Untitled.m b/script/Untitled.m new file mode 100644 index 0000000..6e35233 --- /dev/null +++ b/script/Untitled.m @@ -0,0 +1,72 @@ +% 吊舱下视坐标转换 +format short g +% 相机信息 +imgW = 1280; +imgH = 1024; +f = 48; +dsize = 7.5; +fd = f/dsize*1000; + +%吊舱 +fAz = 44.1876869; +fPt = 18.3241043; + +% 姿态 +fYaw = 165.08250427246094; +fPitch = 4.2472858428955078; +fRoll = -0.37968909740447998; + +% 深度 +dH = 2920; + +M_G2I = getM_G2I( dH,fYaw, fPitch,fRoll,fAz,fPt,imgW,imgH,fd); + +M_I2G = inv(M_G2I); + +t = [200,1000,1]; + +npoint = warpPoint([640,512],M_I2G,t); + + +[R1,t1] = invForm(M_I2G,t); + + +pxpoint = warpPoint(npoint',R1,-t); + + + +R1 * [npoint;1]/2773 + +for fAz = 44:5:60 + for fPt = 18:10:45 + M_G2I = getM_G2I( dH,fYaw, fPitch,fRoll,fAz,fPt,imgW,imgH,fd); + M_I2G = inv(M_G2I); + + p1 = M_I2G*[0,0,1]'; + p2 = M_I2G*[640,0,1]'; + p3 = M_I2G*[640,512,1]'; + p4 = M_I2G*[0,512,1]'; + + p1 = p1/p1(3); + p2 = p2/p2(3); + p3 = p3/p3(3); + p4 = p4/p4(3); + + p5 = p1; + x(1) = p1(1); x(2) = p2(1); x(3) = p3(1); x(4) = p4(1);x(5) = p5(1); + y(1) = p1(2); y(2) = p2(2); y(3) = p3(2); y(4) = p4(2);y(5) = p5(2); + + plot(x,y); + set(gca,'DataAspectRatio',[1 1 1]) + hold on + pause(0.1) + end +end + + + + + + + + diff --git a/script/getM_G2I.m b/script/getM_G2I.m new file mode 100644 index 0000000..43706b7 --- /dev/null +++ b/script/getM_G2I.m @@ -0,0 +1,47 @@ +function [ M ] = getM_G2I( dH,fYaw, fPitch,fRoll,fAz,fPt,imgW,imgH,fd) +%GETM_G2I 此处显示有关此函数的摘要 +% 此处显示详细说明 +% 设备坐标系定义，ENG 东北地 +%//[u,v,1]'=Z*M(内参)*M(alaph)*M(beta)*M(roll)*M(pitch)*M(yaw)*[1,0,0;0,1,0;0,0,DH]'*[X,Y,1]' + + +%% 深度矩阵 +M_het = [1 0 0 + 0 1 0 + 0 0 dH]; + + +%% 姿态矩阵 +M_yaw = [cosd(fYaw) -sind(fYaw) 0 +sind(fYaw) cosd(fYaw) 0 +0 0 1]; + +M_pitch=[1 0 0 + 0 cosd(fPitch) -sind(fPitch) + 0 sind(fPitch) cosd(fPitch)]; + +M_roll = [ cosd(fRoll) 0 sind(fRoll) + 0 1 0 + -sind(fRoll) 0 cosd(fRoll) + ]; + +%% 伺服矩阵 +M_beta = [cosd(fAz) -sind(fAz) 0 +sind(fAz) cosd(fAz) 0 +0 0 1]; + +M_alaph = [1 0 0 + 0 cosd(fPt) -sind(fPt) + 0 sind(fPt) cosd(fPt)]; + +%% 内参矩阵 +M_cam = [fd 0 imgW/2 + 0 -fd imgH/2 + 0 0 1]; + + +M = M_cam*M_alaph*M_beta*M_roll*M_pitch*M_yaw * M_het; + + +end + diff --git a/script/invForm.m b/script/invForm.m new file mode 100644 index 0000000..f452330 --- /dev/null +++ b/script/invForm.m @@ -0,0 +1,5 @@ +function [R1,t1] = invForm(R,t) + R1 = inv(R); + t1 = -(R1*t'); +end + diff --git a/script/warpPoint.m b/script/warpPoint.m new file mode 100644 index 0000000..9568c1d --- /dev/null +++ b/script/warpPoint.m @@ -0,0 +1,10 @@ +function [newPoint] = warpPoint(point,R,t) + + point = [point,1]; + tmp = R * point'; + tmp = tmp / tmp(3); + newPoint = tmp + t'; + newPoint = newPoint(1:2); + +end + diff --git a/stitch/CMakeLists.txt b/stitch/CMakeLists.txt new file mode 100644 index 0000000..9342228 --- /dev/null +++ b/stitch/CMakeLists.txt @@ -0,0 +1,59 @@ +# 获取编译时间 +string(TIMESTAMP COMPILE_TIME 20%y_%m_%d-%H.%M.%S) +set(build_time ${COMPILE_TIME}) + +# 获取当前版本信息 +configure_file(${CMAKE_CURRENT_SOURCE_DIR}/src/Version.h.in ${CMAKE_CURRENT_SOURCE_DIR}/src/Version.h) + +if(MSVC) + add_definitions(-D_CRT_SECURE_NO_WARNINGS) + add_compile_options(/wd4996) +endif() + +find_package(OpenMP REQUIRED) + + +# 公共头文件（放置在最前面，所有后面定义的模块才都能找到） +SET(ArithTrkPubInc ${CMAKE_SOURCE_DIR}/public_include) +include_directories(${ArithTrkPubInc}) # 引入算法公共头文件 + +# 全局包含opencv +link_directories(${OpenCV_LIBS_DIR}) +include_directories(${OpenCV_INCLUDE_DIRS}) + + + + +set(ArithSrcDIR_MAIN "src") # 库源文件路径 + +# 使用当前目录、Common目录源码构建 +file(GLOB libsrcs ${ArithSrcDIR_MAIN}/*.cpp ${ArithSrcDIR_MAIN}/*.c ${ArithSrcDIR_MAIN}/*.h ${ArithSrcDIR_MAIN}/*.hpp) +file(GLOB CommonSrc ${ArithSrcDIR_MAIN}/utils/*.cpp ${ArithSrcDIR_MAIN}/utils/*.c ${ArithSrcDIR_MAIN}/utils/*.h ${ArithSrcDIR_MAIN}/utils/*.hpp) +file(GLOB utmsrc ${ArithSrcDIR_MAIN}/utils/*.cpp ${ArithSrcDIR_MAIN}/utils/*.c ${ArithSrcDIR_MAIN}/utils/*.h ${ArithSrcDIR_MAIN}/utils/*.hpp) + +add_library(${LIB_STITCH} SHARED ${libsrcs} ${CommonSrc} ${utmsrc}) # 构建算法库 + +# 设置包含路径 +target_include_directories(${LIB_STITCH} PUBLIC + ${ArithTrkPubInc} + ${ArithSrcDIR_MAIN} + ${ArithSrcDIR_MAIN}/utils + ${ArithSrcDIR_MAIN}/utm +) + +# 算法库链接 +target_link_libraries(${LIB_STITCH} + OpenMP::OpenMP_CXX + ${OpenCV_LIBS}) + +# # gcc编译器要求0警告 +if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU") + # 只导出指明的符号 + add_definitions(-fvisibility=hidden) + + # 警告：必须在如下编译器配置下编译通过 + target_compile_options(${LIB_STITCH} PRIVATE -Werror -Wreturn-type) +endif() + +set(LIBRARY_OUTPUT_PATH ${CMAKE_SOURCE_DIR}/Bin) # 输出算法库路径 + diff --git a/stitch/src/API_VideoStitch.h b/stitch/src/API_VideoStitch.h new file mode 100644 index 0000000..12c0f93 --- /dev/null +++ b/stitch/src/API_VideoStitch.h @@ -0,0 +1,45 @@ +/*********版权所有（C）2024，武汉高德红外股份有限公司*************** +* 文件名称：API_VideoStitch.h +* 文件标识： +* 内容摘要： +* 其它说明： +* 当前版本： +* 创建作者： +* 创建日期： +*******************************************************************/ +#pragma once + +#ifdef _WIN32 +#define STD_STITCH_API __declspec(dllexport) +#else +#define STD_STITCH_API __attribute__ ((visibility("default"))) +#endif + +#include "Arith_CommonDef.h" +#include "opencv2/opencv.hpp" +// 帧内外方位元素 +struct FrameInfo +{ + AirCraftInfo craft; + CamInfo camInfo; + ServoInfo servoInfo; + int nEvHeight;//相对高差 + int nWidth; + int nHeight; +}; + + +class STD_STITCH_API API_VideoStitch +{ +public: + virtual ~API_VideoStitch() = default; + + virtual BBOOL Init(FrameInfo info) = 0; + virtual BYTE8 Updata(cv::Mat img, FrameInfo para) = 0; + + virtual void Test() = 0; + +public: + static API_VideoStitch* Create(SINT32 nWidth, SINT32 nHeight); + static void Destroy(API_VideoStitch* obj); +}; diff --git a/stitch/src/Arith_VideoStitch.cpp b/stitch/src/Arith_VideoStitch.cpp new file mode 100644 index 0000000..4a10463 --- /dev/null +++ b/stitch/src/Arith_VideoStitch.cpp @@ -0,0 +1,302 @@ +#include "API_VideoStitch.h" +#include "Arith_VideoStitch.h" +#include "Arith_Utils.h" +#include "Arith_CoordModule.h" +#include +#include + +using namespace std; +using namespace cv; + +API_VideoStitch * API_VideoStitch::Create(SINT32 nWidth, SINT32 nHeight) +{ + return new VideoStitch(nWidth,nHeight); +} + + +void API_VideoStitch::Destroy(API_VideoStitch * obj) +{ + delete obj; +} + + +VideoStitch::VideoStitch(SINT32 nWidth, SINT32 nHeight) +{ + +} + +VideoStitch::~VideoStitch() +{ + +} + +void VideoStitch::Test() +{ + FrameInfo info = {0}; + info.camInfo.nFocus = 200; + info.camInfo.fPixelSize = 25; + + info.craft.stAtt.fYaw = 1; + info.craft.stAtt.fPitch = 2; + info.craft.stAtt.fRoll = 3; + + info.nEvHeight = 1000; + + info.servoInfo.fServoAz = 4; + info.servoInfo.fServoPt = 5; + + info.nWidth = 1280; + info.nHeight = 1024; + + Mat R = Mat_TransENG2uv(info); + + Mat R_inv = R.inv(); + + //auto point = getWarpPoint(540,340, (double*)R_inv.data); + //std::cout << point.x << " " << point.y << std::endl; + +} + +BBOOL VideoStitch::Init(FrameInfo info) +{ + // 原点信息 + originPoint = info; + + // 转换关系计算 + AnlayseTform(info); + + // 帧中心的大地坐标，将其平移到全景图中心 + cv::Point2f ct_geo = Trans_uv2Geo(cv::Point2f(info.nWidth / 2, info.nHeight / 2), m_Proj.tf_p2g); + + // 全景图初始化 + m_Proj.panPara.m_pan_width = 1000;//全景宽 + m_Proj.panPara.m_pan_height = 1000;//全景高 + m_Proj.panPara.scale = 0.3;//比例尺,1m = ?pix + + // 直接无平移解算 + auto cur = Trans_Geo2pan(ct_geo, m_Proj.panPara); + + m_Proj.panPara.map_shiftX = m_Proj.panPara.m_pan_width / 2 - (cur.x);//平移X + m_Proj.panPara.map_shiftY = m_Proj.panPara.m_pan_height / 2 - (cur.y);//平移Y + + // 重新计算,验证已平移到中心 + //auto cur2 = Trans_Geo2pan(ct_geo, m_Proj.panPara); + + m_pan = Mat::zeros(m_Proj.panPara.m_pan_height, m_Proj.panPara.m_pan_width,CV_8UC1); + + return true; +} + +BYTE8 VideoStitch::Updata(Mat img, FrameInfo para) +{ + AnlayseTform(para); + + // 计算帧的map四至 + cv::Point2f leftTop_map = back_project(cv::Point2f(0,0), m_Proj); + cv::Point2f rightTop_map = back_project(cv::Point2f(img.cols,0), m_Proj); + cv::Point2f rightBottom_map = back_project(cv::Point2f(img.cols,img.rows), m_Proj); + cv::Point2f leftBottom_map = back_project(cv::Point2f(0,img.rows), m_Proj); + + + // 计算全景图的范围 + int right = max(max(max(leftTop_map.x, leftBottom_map.x), rightTop_map.x), rightBottom_map.x); + int left = min(min(min(leftTop_map.x, leftBottom_map.x), rightTop_map.x), rightBottom_map.x); + int top = min(min(min(leftTop_map.y, leftBottom_map.y), rightTop_map.y), rightBottom_map.y); + int bottom = max(max(max(leftTop_map.y, leftBottom_map.y), rightTop_map.y), rightBottom_map.y); + + + int xRange = right - left; + int yRnage = bottom - top; + + //反映射到像素坐标 + + for (int i = top; i < bottom; i++) + { + for (int j = left; j < right; j++) + { + if (i < 0 || j < 0 || i >= m_pan.rows || j >= m_pan.cols) + continue; + //转换为pixel坐标 + cv::Point2f p_img = project(Point2f(j, i), m_Proj); + + if (p_img.x < 0 || p_img.y < 0 || p_img.x > img.cols || p_img.y > img.rows) + { + continue; + } + //线性插值 + m_pan.data[i* m_pan.cols + j] = + FourPointInterpolation(img.data, img.cols, img.rows, p_img.x, p_img.y); + } + } + + + + cv::imshow("pan", m_pan); + cv::waitKey(1); + + return BYTE8(); +} + + +cv::Point2f VideoStitch::project(cv::Point2f pos_pan, Proj m_Proj) +{ + cv::Point2f pos_geo = Trans_pan2Geo(pos_pan, m_Proj.panPara); + cv::Point2f pos_frame = Trans_Geo2uv(pos_geo, m_Proj.tf_g2p); + return pos_frame; +} + +cv::Point2f VideoStitch::back_project(cv::Point2f pos_frame, Proj m_Proj) +{ + cv::Point2f pos_geo = Trans_uv2Geo(pos_frame, m_Proj.tf_p2g); + + cv::Point2f pos_pan = Trans_Geo2pan(pos_geo, m_Proj.panPara); + + return pos_pan; +} + +cv::Point2f VideoStitch::Trans_uv2Geo(cv::Point2f pos_frame, TForm form) +{ + Mat point = (Mat_(3, 1) << pos_frame.x, pos_frame.y, 1); + Mat result = form.R * point; + // 转局部地理系 + double warpedX = result.at(0, 0) / result.at(2, 0); + double warpedY = result.at(1, 0) / result.at(2, 0); + // 平移到原点地理系 + warpedX += form.T.at(0, 0); + warpedY += form.T.at(1, 0); + + return cv::Point2f(warpedX, warpedY); +} + +cv::Point2f VideoStitch::Trans_Geo2uv(cv::Point2f pos_geo, TForm form_inv) +{ + // 先平移到当前相机位置 + cv::Point2f pos_cam = pos_geo; + pos_cam.x = pos_geo.x + form_inv.T.at(0, 0); + pos_cam.y = pos_geo.y + form_inv.T.at(1, 0); + + Mat point = (Mat_(3, 1) << pos_cam.x, pos_cam.y, 1); + Mat result = form_inv.R * point; + + // 转像方 + double warpedX = result.at(0, 0) / result.at(2, 0); + double warpedY = result.at(1, 0) / result.at(2, 0); + + return cv::Point2f(warpedX, warpedY); +} + +cv::Point2f VideoStitch::Trans_pan2Geo(cv::Point2f pos_pan, PanInfo panPara) +{ + double x = (pos_pan.x - panPara.map_shiftX)/ panPara.scale; + double y = (panPara.m_pan_height - (pos_pan.y - panPara.map_shiftY)) / panPara.scale; + + return cv::Point2f(x, y); +} + +cv::Point2f VideoStitch::Trans_Geo2pan(cv::Point2f pos_geo, PanInfo panPara) +{ + double pan_x = pos_geo.x * panPara.scale + panPara.map_shiftX; + double pan_y = (panPara.m_pan_height - pos_geo.y * panPara.scale) + panPara.map_shiftY; + return cv::Point2f(pan_x, pan_y); +} + + + +Mat VideoStitch::Mat_TransENGMove(FrameInfo info) +{ + PointXYZ ptCurr = getXYZFromBLH(info.craft.stPos); + PointXYZ ptOrig = getXYZFromBLH(originPoint.craft.stPos); + PointXYZ diff = getNUEXYZFromCGCSXYZ(ptCurr, ptOrig); + + Mat move = Mat::zeros(3, 1, CV_64F); + + move.at(0, 0) = diff.Z; + move.at(1, 0) = diff.X; + move.at(2, 0) = diff.Y; + return move; +} + +Mat VideoStitch::Mat_TransENG2uv(FrameInfo info) +{ + //从地理坐标系转像素坐标 + + //[u,v,1]'=Z*M*[X,Y,DH]' = Z*M*[1,0,0;0,1,0;0,0,DH]'*[X,Y,1]' + //[u,v,1]'=Z*M(内参)*M(alaph)*M(beta)*M(roll)*M(pitch)*M(yaw)*[X,Y,DH] + + + // 深度矩阵 + Mat M_het = (Mat_(3, 3) << 1,0,0, + 0,1,0, + 0,0,info.nEvHeight + ); + + + + float yaw = info.craft.stAtt.fYaw; + Mat M_yaw = (Mat_(3, 3) << cosd(yaw), -sind(yaw),0, + sind(yaw), cosd(yaw),0, + 0,0,1 + ); + + float pit = info.craft.stAtt.fPitch; + Mat M_pitch = (Mat_(3, 3) << 1,0,0, + 0,cosd(pit),-sind(pit), + 0, sind(pit), cosd(pit) + ); + + /* 1 0 0 + 0 cos sin + 0 -sin cos + */ + + float roll = info.craft.stAtt.fRoll; + Mat M_roll = (Mat_(3, 3) << cosd(roll),0, sind(roll), + 0,1,0, + -sind(roll),0,cosd(roll) + ); + + + float beta = info.servoInfo.fServoAz; + + Mat M_beta = (Mat_(3, 3) << cosd(beta), -sind(beta),0, + sind(beta),cosd(beta),0, + 0,0,1 + ); + + + float alaph = info.servoInfo.fServoPt; + + Mat M_alaph = (Mat_(3, 3) << 1,0,0, + 0, cosd(alaph),-sind(alaph), + 0,sind(alaph),cosd(alaph) + + ); + + // 内参 + FLOAT32 fd = info.camInfo.nFocus /info.camInfo.fPixelSize * 1000; + + Mat M_cam = (Mat_(3, 3) << fd,0,info.nWidth/2, + 0,-fd,info.nHeight/2, + 0,0,1 + ); + + Mat M = M_cam*M_alaph*M_beta*M_roll*M_pitch*M_yaw * M_het; + //cout << M_cam * M_alaph * M_beta * M_roll * M_pitch * M_yaw * M_het; + return M; +} + + + +void VideoStitch::AnlayseTform(FrameInfo info) +{ + // 从像方->地理 + m_Proj.tf_p2g.R = Mat_TransENG2uv(info).inv(); + m_Proj.tf_p2g.T = Mat_TransENGMove(info); + + // 从地理->像方 + m_Proj.tf_g2p.R = Mat_TransENG2uv(info); + m_Proj.tf_g2p.T = -m_Proj.tf_p2g.T; +} + + + diff --git a/stitch/src/Arith_VideoStitch.h b/stitch/src/Arith_VideoStitch.h new file mode 100644 index 0000000..4cb0628 --- /dev/null +++ b/stitch/src/Arith_VideoStitch.h @@ -0,0 +1,84 @@ +#include "API_VideoStitch.h" +#include "opencv2/opencv.hpp" + + +// 全景图配置 +struct PanInfo +{ + int m_pan_width; + int m_pan_height; + float scale;// 比例尺 + float map_shiftX;// 平移X + float map_shiftY;// 平移Y +}; + +// 像方-物方转换关系 +struct TForm +{ + cv::Mat R; + cv::Mat T; +}; + + +// 投影关系,描述反算过程 +struct Proj +{ + TForm tf_p2g;//从帧到地理坐标系的Rt矩阵 + TForm tf_g2p;//从地理坐标系到帧的Rt矩阵 + PanInfo panPara;//全景图配置 +}; + +class VideoStitch:public API_VideoStitch +{ +public: + VideoStitch(SINT32 nWidth, SINT32 nHeight); + ~VideoStitch(); + + void Test(); + + +public: + BBOOL Init(FrameInfo info); + + BYTE8 Updata(cv::Mat img, FrameInfo para); + + // 投影：从全景图到帧 + cv::Point2f project(cv::Point2f pos_pan, Proj m_Proj); + + // 反投影：从帧到全景图 + cv::Point2f back_project(cv::Point2f pos_frame, Proj m_Proj); + +private: + + FrameInfo originPoint;//成图初始点，作为拼接参考 + + // 帧像方-地理坐标 + cv::Point2f Trans_uv2Geo(cv::Point2f pos_frame, TForm form); + + // 地理坐标-帧像方 + cv::Point2f Trans_Geo2uv(cv::Point2f pos_geo, TForm form); + + // 从全景图->地理 + cv::Point2f Trans_pan2Geo(cv::Point2f pos_pan, PanInfo panPara); + + // 地理->全景图 + cv::Point2f Trans_Geo2pan(cv::Point2f pos_geo, PanInfo panPara); + + + // 平移矩阵，以初始化点为基准，计算当前位置在初始点的地理坐标，那么当前帧所有点的坐标做此平移 + cv::Mat Mat_TransENGMove(FrameInfo info); + + // 机体ENG(东北地)到像方的 旋转矩阵 + cv::Mat Mat_TransENG2uv(FrameInfo info); + + // 计算当前帧像方-成图坐标系R t（反投影关系） + void AnlayseTform(FrameInfo info); + +private: + + Proj m_Proj;// 当前帧投影矩阵集合 + + cv::Mat m_pan;//全景图 + + +}; \ No newline at end of file diff --git a/stitch/src/Version.h.in b/stitch/src/Version.h.in new file mode 100644 index 0000000..a1fe2aa --- /dev/null +++ b/stitch/src/Version.h.in @@ -0,0 +1,6 @@ +//VersionConfig.h.in + +#pragma once +#include +std::string BUILD_TIME = "BUILD_TIME @build_time@"; +std::string VERSION = "BUILD_VERSION 1.0.1"; diff --git a/stitch/src/utils/Arith_CoordModule.cpp b/stitch/src/utils/Arith_CoordModule.cpp new file mode 100644 index 0000000..d1cc950 --- /dev/null +++ b/stitch/src/utils/Arith_CoordModule.cpp @@ -0,0 +1,915 @@ +#include "Arith_CoordModule.h" + +//#include "Arith_SysVar.h" +#include +#include "Arith_Utils.h" + + + +// 从惯性系（建航坐标系）到 像坐标系 +POINT32F getImagePosFromStablePole(Pole stablePole, CameraParam cam, ServoParam servoInfo, EulerRPY att, DeviceSetupError err) +{ + POINT32F imgPos = { 0 }; + + // 从稳定性到设备系 + Pole devicePole = getDevicePoleFromStablePole(stablePole, att, err); + + // 从设备系到像方 + imgPos = getImagePosFromDevicePole(devicePole, cam, servoInfo); + + return imgPos; + +} + +// 从像坐标系 到 惯性系（建航坐标系） +Pole getStablePoleFromImagePos(POINT32F imagePos, CameraParam cam, ServoParam servoInfo, EulerRPY att, DeviceSetupError err) +{ + Pole StablePole = { 0 }; + + // 从像方到设备系 + Pole devicePole = getDevicePoleFromImagePos(imagePos, cam, servoInfo); + + // 从设备系到惯性系 + StablePole = getStablePoleFromDevicePole(devicePole, att, err); + + return StablePole; +} + + + +BBOOL bTargetInScanFov(ANGLE32F targetPos, Pole scanNueRange1, Pole scanNueRange2) +{ + BBOOL AzInScan = TRUE; + BBOOL PtInScan = TRUE; + // 俯仰扫描区间有效 + if (scanNueRange1.alpha != 0 && scanNueRange2.alpha != 0) + { + if (targetPos.fPt < scanNueRange1.alpha || targetPos.fPt > scanNueRange2.alpha) + { + PtInScan = FALSE; + } + + } + + // 方位扫描区间有效 + if (scanNueRange1.beta != 0 && scanNueRange2.beta != 0) + { + if (targetPos.fAz < scanNueRange1.beta || targetPos.fAz > scanNueRange2.beta) + { + AzInScan = FALSE; + } + } + + return (AzInScan && PtInScan); + +} + +Pole getDevicePoleFromStablePole(Pole stablePole, EulerRPY att, DeviceSetupError err) +{ + Pole targetCarNUEPole = stablePole; + + // 地理极坐标 转 地理直角坐标 + PointXYZ targetCarNUEXYZ = getXYZFromPole(targetCarNUEPole); + + // 地理直角坐标 转 平台直角坐标 + PointXYZ targetCarXYZ = getCarXYZFromCarNUEXYZ(targetCarNUEXYZ, att.fYaw, att.fPitch, att.fRoll); + + // 平台直角 转 光电基准系直角 + PointXYZ targetEOBaseXYZ = getEOBaseXYZFromCarXYZ(targetCarXYZ, err.g_XBias, err.g_YBias, err.g_ZBias); + + // 光电基准系直角 转 光电伺服直角 + PointXYZ targetEOServoXYZ = getEOServoXYZFromEOBaseXYZ(targetEOBaseXYZ, err.g_YawBias, err.g_PitchBias, err.g_RollBias); + + // 光电伺服直角 转 光电伺服极坐标 + Pole targetEOServoPole = getPoleFromXYZ(targetEOServoXYZ); + + return targetEOServoPole; + +} + +Pole getStablePoleFromDevicePole(Pole devicePole, EulerRPY att, DeviceSetupError err) +{ + // 设备系转伺服直角坐标 + PointXYZ targetServoXYZ = getXYZFromPole(devicePole); + + // 光电伺服直角坐标 转 光电基准直角坐标 + PointXYZ targetEOBaseXYZ = getEOBaseXYZFromEOServoXYZ(targetServoXYZ, err.g_YawBias, err.g_PitchBias, err.g_RollBias); + + // 光电基准直角坐标 转 平台直角坐标 + PointXYZ targetCarXYZ = getCarXYZFromEOBaseXYZ(targetEOBaseXYZ, err.g_XBias, err.g_YBias, err.g_ZBias); + + // 平台直角坐标 转 平台地理直角坐标 + PointXYZ targetCarNUEXYZ = getCarNUEXYZFromCarXYZ(targetCarXYZ, att.fYaw, att.fPitch, att.fRoll); + + // 平台地理直角坐标 转 平台地理极坐标 + Pole targetCarNUEPole = getPoleFromXYZ(targetCarNUEXYZ); + + return targetCarNUEPole; + +} + + + +// 像方转设备系 +Pole getDevicePoleFromImagePos(POINT32F imagePos, CameraParam cam, ServoParam servoInfo) +{ + Pole targetDevicePole = { 0 }; + + if(cam.bImageRataSys) + { + double refmat[9] = { 0 }; + PMS_MultiRefMat(servoInfo.fAz, servoInfo.fPt, servoInfo.fFSMAz, servoInfo.fFSMPt, refmat); + + targetDevicePole = PMS_ProjectPixelToWorld((int)imagePos.x, (int)imagePos.y, refmat, + cam.f / cam.dSize * 1000, cam.dx + cam.ImageWidth / 2, cam.dy + cam.ImageHeight / 2); + } + else + { + // 像方转相机坐标系 + Pole targetCamPole = { 0 }; + float fAglRes = FLOAT32(ANGLE(cam.dSize / cam.f / 1000)); + SINT32 cx = cam.dx + cam.ImageWidth / 2; + SINT32 cy = cam.dy + cam.ImageHeight / 2; + targetCamPole.beta = (imagePos.x - cx) * fAglRes; + targetCamPole.alpha = (cy - imagePos.y) * fAglRes; + + // 1:一般旋转矩阵方法 + // 光电镜面极坐标 转 光电镜面直角坐标 + PointXYZ targetCamXYZ = getXYZFromPole(targetCamPole); + + // 光电镜面直角坐标 转 光电伺服直角坐标 + PointXYZ targetServoXYZ = getEOServoXYZFromEOCamXYZ(targetCamXYZ, servoInfo.fPt, servoInfo.fAz); + + // 2:正割补偿方法 + // 光电镜面极坐标 转 光电伺服极坐标 + // Pole targetEOServoPOLE; + // targetEOServoPOLE.alpha = alaph + targetCamPole.alpha; + // targetEOServoPOLE.beta = beta + targetCamPole.beta/cos(RADIAN(alaph)); + // targetEOServoPOLE.distance = targetCamPole.distance; + // // 光电伺服极坐标 转 光电伺服直角坐标 + // targetServoXYZ = getXYZFromPole(targetEOServoPOLE); + + targetDevicePole = getPoleFromXYZ(targetServoXYZ); + } + + + return targetDevicePole; +} + + +// 设备系转像方坐标 +POINT32F getImagePosFromDevicePole(Pole devicePole, CameraParam cam, ServoParam servoInfo) +{ + POINT32F imgpos = { 0 }; + + // 常规光电成像模型 + if(cam.bImageRataSys) + { + double refmat[9] = { 0 }; + double refmat_inv[9] = { 0 }; + PMS_MultiRefMat(servoInfo.fAz, servoInfo.fPt, servoInfo.fFSMAz, servoInfo.fFSMPt, refmat); + //invMat3(refmat, refmat_inv); + TransposeMat(refmat, 3, 3, refmat_inv);//等价 + + PointXYZ imgPosition = + PMS_ProjectWorldToPixel((float)devicePole.beta, (float)devicePole.alpha, refmat_inv, + cam.f / cam.dSize * 1000, cam.dx + cam.ImageWidth/2, cam.dy + cam.ImageHeight/2); + + imgpos.x = (float)imgPosition.X; + imgpos.y = (float)imgPosition.Y; + + } + else + { + // 设备极坐标转直角坐标 + PointXYZ targetEOServoXYZ = getXYZFromPole(devicePole); + + // 光电伺服直角 转 光电镜面直角坐标(相机系) + PointXYZ targetEOCamXYZ = getEOCamXYZFromEOServoXYZ(targetEOServoXYZ, servoInfo.fPt, servoInfo.fAz); + + // 光电镜面极坐标 + Pole targetEOCamPole = getPoleFromXYZ(targetEOCamXYZ); + + float fAglRes = FLOAT32(ANGLE(cam.dSize / cam.f / 1000.0f)); + SINT32 cx = cam.dx + cam.ImageWidth / 2; + SINT32 cy = cam.dy + cam.ImageHeight / 2; + imgpos.x = FLOAT32(targetEOCamPole.beta / fAglRes + cx); + imgpos.y = FLOAT32(cy - targetEOCamPole.alpha / fAglRes); + } + + return imgpos; +} + + + + + +void PMS_MultiRefMat(float fMainAz, float fMainAPt, float fFSMAz, float fFSMPt, double* mat) +{ + // 反射镜1 + // fa -- 方位角输出-外框 + // fb -- 俯仰角输出-内框 + PointXYZ N1_0; + N1_0.X = 1 / sqrt(2.0); + N1_0.Y = 1 / sqrt(2.0); + N1_0.Z = 0; + + // n1绕z轴旋转fb（向上为正） + double Sz[9] = { cosd(fMainAPt),sind(fMainAPt),0, + -sind(fMainAPt),cosd(fMainAPt),0, + 0,0,1 }; + + // 再绕y轴旋转fa（向右为正） + double Sy[9] = { cosd(fMainAz),0,-sind(fMainAz), + 0,1,0, + sind(fMainAz),0,cosd(fMainAz) }; + + // 反射镜1法向量 %% 前两个法向量在XOY面里 + //N1_F = Sy * Sz * N1_0; + PointXYZ N1_F = RtPoint(Sy, RtPoint(Sz, N1_0)); + + // 反射镜1反射矩阵 + //R1 = eye(3) - (2 * N1_F * N1_F'); + double R1[9] = { 0 }; + RMirror(N1_F, R1); + + + // 反射镜23，没有动 %%注意：第3、4面镜子已经在XOZ面里了，注意空间关系 + PointXYZ N2_0; + N2_0.X = -1 / sqrt(2.0); + N2_0.Y = -1 / sqrt(2.0); + N2_0.Z = 0; + double R2[9] = { 0 }; + RMirror(N2_0, R2); + + PointXYZ N3_0; + N3_0.X = 1 / sqrt(2.0); + N3_0.Y = 0; + N3_0.Z = 1 / sqrt(2.0); + double R3[9] = { 0 }; + RMirror(N3_0, R3); + + // 反射镜4是快反 + // Qa -- 方位角输出-外框,向右偏转为正 + // Qb -- 俯仰角输出-内框，抬头为正 + PointXYZ N4_0; + N4_0.X = 1 / sqrt(2.0); + N4_0.Y = 0; + N4_0.Z = -1 / sqrt(2.0); + + // 外框扫描轴为y轴，内框扫描轴L0为x=z 即(1,0,1)' + double Sy2[9] = { cosd(fFSMAz),0,-sind(fFSMAz), + 0,1,0, + sind(fFSMAz),0,cosd(fFSMAz) }; + + // 内框旋转向量,注意左手系，会引入负号 + PointXYZ L0; + L0.X = 1; + L0.Y = 0; + L0.Z = 1; + double SL2[9] = { 0 }; + rotationVectorToMatrix(L0, -fFSMPt, SL2); + + //快反镜法向量，经过内框和外框旋转 + // N4_F = Sy2 * SL2 * N4_0; + PointXYZ N4_F = RtPoint(Sy2, RtPoint(SL2, N4_0)); + + // 快反的反射矩阵 + double R4[9] = { 0 }; + RMirror(N4_F, R4); + + // 整个光路的反射模型,从像方到物方 + double tmp[9] = { 0 }; + MultiMat333(R2, R3, R4, tmp); + MultiMat33(R1, tmp, mat); + + +} + +// 像方到物方的映射,转到方位俯仰角 +Pole PMS_ProjectPixelToWorld(int x, int y, float fMainAz, float fMainAPt, float fFSMAz, float fFSMPt, float f_px, int cx, int cy) +{ + // 计算反射矩阵 + double mat[9] = { 0 }; + PMS_MultiRefMat(fMainAz, fMainAPt, fFSMAz, fFSMPt, mat); + return PMS_ProjectPixelToWorld(x, y, mat, f_px, cx, cy); +} + +Pole PMS_ProjectPixelToWorld(int x, int y, double* mat, float f_px, int cx, int cy) +{ + Pole worldPole = { 0 }; + // 内参信息 + // const int nWidth = 860; + // const int nHeight = 715; + // const float f = 350; + // const int piexlSize = 7.5; + // float fpiexl = f/piexlSize * 1000; //像素焦距 + + // 从靶面出射的矢量 + PointXYZ PO; + PO.X = -f_px; + PO.Y = -(y - cy); + PO.Z = -(x - cx); + + //%%猜测y方向设置了-Y，否则成像应该是倒着的，所以这里 + PO.Y = -PO.Y; + + // 出射矢量 + PointXYZ OQ; + OQ = RtPoint(mat, PO); + + + + // 物方角度 + double len = sqrt(PO.X * PO.X + PO.Y * PO.Y + PO.Z * PO.Z); + + + worldPole.alpha = ANGLE(asin(-OQ.Y / len)); + worldPole.beta = ANGLE(atan2(OQ.Z, OQ.X)); + + return worldPole; +} + +// PointXYZ PMS_ProjectPixelToWorldVec(int x, int y, double* mat, float f_px, int cx, int cy) +// { +// Pole worldPole = { 0 }; +// // 内参信息 +// // const int nWidth = 860; +// // const int nHeight = 715; +// // const float f = 350; +// // const int piexlSize = 7.5; +// // float fpiexl = f/piexlSize * 1000; //像素焦距 + +// // 从靶面出射的矢量 +// PointXYZ PO; +// PO.X = -f_px; +// PO.Y = -(y - cy); +// PO.Z = -(x - cx); + +// //%%猜测y方向设置了-Y，否则成像应该是倒着的，所以这里 +// PO.Y = -PO.Y; + +// // 出射矢量 +// PointXYZ OQ; +// OQ = RtPoint(mat, PO); + +// return OQ; + +// } +//物方到像方的映射,转到像素位置 +PointXYZ PMS_ProjectWorldToPixel(float fAz, float fPt, double* refmat, float f_px, int cx, int cy) +{ + PointXYZ pxPos = { 0 }; + + // 物方直角系出射矢量 + PointXYZ OQ = { 0 }; + float Dep = 1000;//单位向量 + OQ.Y = -Dep * sind(fPt); + OQ.X = Dep * cosd(fPt) * cosd(fAz); + OQ.Z = Dep * cosd(fPt) * sind(fAz); + + + // 投影,得到靶面出射矢量 + PointXYZ PO = RtPoint(refmat, OQ); + + PointXYZ OP = { 0 };//射向靶面 + OP.X = -PO.X; + OP.Y = -PO.Y; + OP.Z = -PO.Z; + + + // 内参矩阵 + pxPos.X = f_px * OP.Z / OP.X + cx; + pxPos.Y = -f_px * OP.Y / OP.X + cy; + + + return pxPos; +} + +// PointXYZ PMS_ProjectWorldVecToPixel(PointXYZ worldVec, double* refmat, float f_px, int cx, int cy) +// { +// PointXYZ pxPos = { 0 }; +// // 投影,得到靶面出射矢量 +// PointXYZ PO = RtPoint(refmat, worldVec); + +// PointXYZ OP = { 0 };//射向靶面 +// OP.X = -PO.X; +// OP.Y = -PO.Y; +// OP.Z = -PO.Z; + + +// // 内参矩阵 +// pxPos.X = f_px * OP.Z / OP.X + cx; +// pxPos.Y = -f_px * OP.Y / OP.X + cy; + + +// return pxPos; +// } + + +// // S726像方到物方转换 +// 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; + +// } + +// // S726物方到像方转换 +// 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); + + + +// } + + + + +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); + +} + +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° + + // 吊舱无源定位允许最低高度10米 + if (targetPole.distance < 10) + { + targetPole.distance = 10000; + } + + 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; +} + + diff --git a/stitch/src/utils/Arith_CoordModule.h b/stitch/src/utils/Arith_CoordModule.h new file mode 100644 index 0000000..c3b93f6 --- /dev/null +++ b/stitch/src/utils/Arith_CoordModule.h @@ -0,0 +1,204 @@ + +/*********版权所有（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__ + + diff --git a/stitch/src/utils/Arith_SysStruct.h b/stitch/src/utils/Arith_SysStruct.h new file mode 100644 index 0000000..1e922fd --- /dev/null +++ b/stitch/src/utils/Arith_SysStruct.h @@ -0,0 +1,867 @@ +/*********版权所有（C）2020，武汉高德红外股份有限公司*************************************** + * 文件名称： Arith_Global.h + * 文件标识： + * 内容摘要： 业务无关的通用符号系统 + * 其它说明： + * 当前版本： + * 创建作者：软件室算法组 + * 创建日期：2020/02/25 + * 包含关系：本文件被全局引用，除运行平台系统文件外不得添加任何依赖 + *****************************************************************************************/ +#ifndef _ARITH_SYSSTRUCT_ +#define _ARITH_SYSSTRUCT_ + +#include "Arith_CommonDef.h" +#include +#include +#include +#include +#include + +#define SysDelay (0) //系统延时，指图像比当前帧角度慢的时间 单位ms，有符号 + + +//系统参数初始化。 +#define GLB_CMD_SYSINITIAL 0x99 + +//定义雷达导引区域大小 qw 20211227 +#define GLB_RADARGUIDE_RECT 400 + +// 默认帧频 +#define GLB_FRM_FREQ 100 + +// 目标尺寸类型定义 +#define GLB_OBJ_SIZE_SMALL 1 //目标类型 - 小目标 +#define GLB_OBJ_SIZE_FACE 2 //目标类型 - 面目标 +#define GLB_OBJ_SIZE_MIDDLE 3 //目标类型 - 小目标与面目标临界 +#define GLB_OBJ_SIZE_DIMSMALL 4 //目标类型 - 弱小目标 +#define GLB_OBJ_SIZE_UNKOWN 5 //目标类型 - 未知 + +// 目标灰度类型定义 +#define GLB_OBJ_GRAY_BRIGHT 1 +#define GLB_OBJ_GRAY_DARK 2 +#define GLB_OBJ_GRAY_ALL 3 + + + +// 目标状态定义，搜索和跟踪阶段通用 +#define GLB_OBJ_NEW 1 +#define GLB_OBJ_UPDATE 2 +#define GLB_OBJ_LOST 3 +#define GLB_OBJ_DELETE 4 + +// 跟踪参数 +#define GLB_MEMORY_FRM_NUM 500 //默认记忆跟踪帧数 + + +//debug 解锁原因 +#define GLB_UNLOCK_MANUAL 1 //手动解锁指令，或来自上位机的解锁 +#define GLB_UNLOCK_RADAR_CHANGE 2 //雷达批号改变解锁 +#define GLB_UNLOCK_OVERTIME_TST 31 //TST超时解锁 +#define GLB_UNLOCK_OVERTIME_CEND 32 //CEND超时解锁 +#define GLB_UNLOCK_OVERTIME_KCF 33 //KCF超时解锁 +#define GLB_UNLOCK_ATTDIR_CHANGE 4 //攻击方向改变解锁，对空对地立靶模式切换 +#define GLB_UNLOCK_FOVOUT 5 //出视场解锁 + + + +// 目标类型标记 +#define TargetClass_Plane 98 +#define TargetClass_Missle 99 + + +#define DT_TARGET_MAX_NUM 100 // 定义检测器最大检测个数 + +#define DT_MATCHER_MAX_NUM 6 // 定义模板匹配最大检测个数 + +// 管道规模 +#define GLB_PIPE_DEPTH_MAX 10 //管道的最大深度 + +#define MAIN_ALARM_DEPTH_MAX 200 //主管道告警的最大深度 + + +#define GLB_GROUP_NUM_MAX 5 //组的最大个数 +#define GLB_GROUP_PIPENUM_MAX 3 //组包含的管道个数（只支持3个合并） + +//管道确认帧数 +#define GLB_PIPE_AWS_FRAME_STARE 5 //管道目标确认(报警)的最少帧数:凝视 +#define GLB_PIPE_AWS_FRAME_SCAN 3 //管道目标确认(报警)的最少帧数：扫描 +#define GLB_PIPE_NUM_SECTOR 9 //目标运动方向分割 qw 20220112 + +//管道丢失帧数 +#define GLB_PIPE_UNFOUND_CNT 3 //15管道目标在其视场内没有被找到的次数 +#define GLB_PIPE_DEL_MIN_SCAN 5 //管道最少延迟删除帧数- 扫描模式 +#define GLB_PIPE_DEL_MIN_STARE 5 //管道最少延迟删除帧数- 凝视/跟踪模式 + +//管道滤波帧数 +#define GLB_PIPE_FILTER_CNT 10 //管道的滤波帧数 +#define PIPE_ACOS_TABLE_LEN 1001 // 管道余弦表间隔 qw 20220112 + +//管道搜索半径 +#define GLB_PIPE_RADIUS_SEARCH 50//30//20 //目标搜索时，管道搜索范围直径 +#define GLB_PIPE_RADIUS_LOCK 50//12 //目标锁定时，管道搜索范围直径 +#define GLB_PIPE_RADIUS_TRACK 50//15 //目标跟踪时，管道搜索范围直径 +#define GLB_PIPE_RADIUS_LOST 80//25 //目标记忆跟踪时，管道搜索范围直径 + +//简称LogT LogD LogI LogW LogE LogC LogO +#define ARITH_LOG_LEVEL_TRACE 0 +#define ARITH_LOG_LEVEL_DEBUG 1 +#define ARITH_LOG_LEVEL_INFO 2 +#define ARITH_LOG_LEVEL_WARN 3 +#define ARITH_LOG_LEVEL_ERROR 4 +#define ARITH_LOG_LEVEL_CRITICAL 5 +#define ARITH_LOG_LEVEL_OFF 6 + +#define PI 3.14159265358979f // 圆周率 +#define PI2 2 * PI +#define TRUE 1 // 真1 +#define FALSE 0 // 假0 + + // OPENCV 中的定义冲突 +//#define MIN(a, b) ((a) < (b) ? (a) : (b)) // 取a、b两者中的最小值 +//#define MAX(a, b) ((a) > (b) ? (a) : (b)) // 取a、b两者中的最大值 +#ifndef MIN +# define MIN(a,b) ((a) > (b) ? (b) : (a)) +#endif + +#ifndef MAX +# define MAX(a,b) ((a) < (b) ? (b) : (a)) +#endif + +#define MINMAX_VAR(a, b, c) MAX(MIN(b, c), MIN(a, MAX(b, c))) // 取a、b、c三者中位于中间大小的值，即非最大值亦非最小值 +#define MINMAX_NUM(a, b, c) MAX(b, MIN(a, c)) // 当b<=c时，将a限制在b和C之间；当b>c时，取b +#define ABS(x) ((x) < 0 ? -(x) : (x)) // 取x的绝对值 +#define ROUND(x) ((SINT32)((x) - ((x) >= 0 ? 0 : 1) + 0.5f)) // 取整 - 对x四舍五入取整 +#define FLOOR(a) (SINT32)(a) // 取整 - 小数位截断 +#define POW2(x) ((x) * (x)) // 平方值计算 +//#define FLT_EPSILON 1.192092896e-07F +#define RANDOM(x) (rand() % x) +#define RADIAN(x) (x * PI / 180) +#define ANGLE(x) (x / 3.141592653589793 * 180) +//#define NULL 0 +#define ICV_CUBIC_1(x) (((x)*1.5 - 2.5) * (x) * (x) + 1.0) +#define ICV_CUBIC_2(x) ((((x) * (-0.5) + 2.5) * (x)-4.0) * (x) + 2) +#define cosd(x) (cos(RADIAN(x))) +#define sind(x) (sin(RADIAN(x))) +#define RELEASE(x) (delete[] (x);) +#define EPSILON 0.0000001f //最小值-浮点数精度 +#define UNEPSILON 100000000 //最大值浮点数精度 +#define FEPSILON 0.000001f //最小值-浮点数精度2 + +// 角度输入输出限值（0-360°） +#define DEGLIM(DEG) ( (DEG > 180) ? (DEG-360) : ( (DEG < -180) ? (DEG+360) : DEG ) ) +#define DEGLIM360(DEG) ( (DEG > 360) ? (DEG-360) : ( (DEG < 0) ? (DEG+360) : DEG ) ) + +// 目标类别来源标识 +typedef enum tagemClsSrc +{ + Arith_CLS_DST = 1, + Arith_CLS_Area = 2, + Arith_CLS_Lands = 3, + Arith_CLS_Hole = 4, + Arith_CLS_SVM = 5, + Arith_CLS_AID = 6, + Arith_CLS_Manual = 7 +}ClsSrc; + +typedef struct tagPixcelsCnt +{ + SINT32 nObjCnt; //目标的像素数 + SINT32 nObjXHoleCnt; //X方向的孔洞的像素数 + SINT32 nObjYHoleCnt; //Y方向的孔洞的像素数 +}PixcelsCnt; + +// 定义目标滤波结构体(32bit×9) +typedef struct tagObjectFilter // BYTES: 9*4=36 +{ + UINT32 unFilterCnt; // 目标的滤波帧数 + ANGLE32F afAngle; // 目标方位角、俯仰角 + POINT32F pfCenPos; // 目标的中心点坐标（滤波结果） + SIZE32F sfSize; // 管道中目标的宽度、高度（滤波结果） + FLOAT32 fGray; // 管道中目标的灰度均值（滤波结果） + FLOAT32 fPxlsCnt; // 管道中目标的像素大小（滤波结果） + FLOAT32 fSNR; // 管道中目标的信噪比（滤波结果） mww,20130819 + ANGLE32F fAngleSpeed; // 目标方位角、俯仰角速度 + UINT16 fGrayNeighbor[4]; //管道中目标的四邻阈灰度均值（TRBL滤波结果） +} OBJECT_FILTER; + +// 定义小目标SNR调试结构体 +typedef struct tagObjectSNR // BYTES: 4*2 + 4*4 = 24 +{ + POINT16S Pnt; // 中心点x,y + SINT16 nTGR; // 目标窗口半径 + SINT16 nBGR; // 背景窗口半径 + FLOAT32 ObjGray; //(x,y)点灰度 + FLOAT32 BGMean; // 背景均值 + FLOAT32 BGStd; // 背景标准差 + FLOAT32 fSNR; //(x,y)点信噪比 + FLOAT32 ObjStd; +} OBJECTSNR; + + +// 定义目标结构体 +typedef struct tagTargetObject // BYTES: 3*1 + 1*2 + 7*4 + 3*8 = 57 +{ + // 全局 + BBOOL bObject; // 标识目标是否已被创建。1-目标已创建，0-目标未创建 + UINT32 unFrmID; // 记录目标被检测到的帧编号 + SINT32 unClsType; // 目标类别 + FLOAT32 fDetConf; // 检测置信度(AI目标) + + // 位置 + POINT32F pfCenPos; // 区域的中心点坐标 //BYTES: 2*4=8 + POINT16S pnMaxPos; // 目标的极大值位置（小目标检测用） //BYTES: 2*2=4 + ANGLE32F afAngle; // 目标方位角、俯仰角 + FLOAT32 fDist; // 距离视场中心距离 + PointXYZ pos3d; // 3D定位，使用稳定系极坐标对应的直角坐标 //04046wcw 20241203 + + // 尺寸 + MINMAXRECT mrnRect; // 目标上下左右边界坐标 //BYTES: 4*2=8 + SIZE16S snSize; // 目标宽高 //BYTES: 2*2 + 1*4=8 + UINT32 unObjPxlsCnt; // 目标的象素点的个数 + UBYTE8 nObjTypeSize; // 表示目标尺寸类型：1--小目标，2--面目标，3--临界目标（即二者均是），4--弱小目标 + + // 强度 + UINT16 pxObjGray; // 目标的灰度：可以是中心点灰度、或目标均值等 + UINT16 pxObjMaxGray; // 目标极值灰度 //04046wcw 20221019 + UINT16 fGrayNeighbor[4]; /*> 目标四周的灰度均值, TRBL */ + FLOAT32 fObjStd; // 目标方差 4 + FLOAT32 fBGStd; // 目标背景方差 + FLOAT32 fBGMean; // 目标背景均值 + FLOAT32 fSNR; // 目标背景信噪比 + UBYTE8 nObjTypeGray; // 表示目标灰度类型：1--亮目标，2--暗目标，0--其它 + + // 管道相关 + UINT32 nInPipesID; // 是否拥有管道：0-目标未加入管道，大于0-目标已加入第（nInPipesID-1）个管道 + SINT32 nInPipeDist; // 记录目标被加入管道时，与该管道中心的距离 + FLOAT32 nInPipeDistIou; + UBYTE8 ubDirection; + + // 来源 + ObjSrc nObjTypeSrc; // 目标来源 01-小目标 02-面目标 03-AI目标 04-KCF 05-TLD 06-手动起 + FLOAT32 fMatchConf; //跟踪阶段匹配度 + ClsSrc emClsSrc; // 目标类别来源 + +} TARGET_OBJECT; + + +// 定义模板匹配目标结构体 +typedef struct tagMatcherTarget // BYTES: 3*1 + 1*2 + 7*4 + 3*8 = 57 +{ + // 全局 + BBOOL bObject; // 标识目标是否已被创建。1-目标已创建，0-目标未创建 + UINT32 unFrmID; // 记录目标被检测到的帧编号 + FLOAT32 fDetConf; // 检测置信度 + + // 位置 + POINT32F pfCenPos; // 区域的中心点坐标 //BYTES: 2*4=8 + POINT16S pnMaxPos; // 目标的极大值位置（小目标检测用） //BYTES: 2*2=4 + ANGLE32F afAngle; // 目标方位角、俯仰角 + FLOAT32 fDist; // 距离视场中心距离 + + // 尺寸 + MINMAXRECT mrnRect; // 目标上下左右边界坐标 //BYTES: 4*2=8 + SIZE16S snSize; // 目标宽高 //BYTES: 2*2 + 1*4=8 + UINT32 unObjPxlsCnt; // 目标的象素点的个数 + + FLOAT32 Angle; // 目标角度 + // 来源 + ObjSrc nObjTypeSrc; // 目标来源 01-小目标 02-面目标 03-AI目标 04-KCF 05-TLD 06-手动起 +} MATCHER_TARGET; + + +// 跟踪器结构体 +typedef struct tagObjectStatus +{ + UINT32 unFrmId; //当前帧编号 + // 跟踪批号，对应外导引批号 by wcw04046 @ 2021/12/09 + UBYTE8 nTrackID; + + BBOOL bObjAssigned; // 是否初始化了目标的起始位置和大小。1-目标已初始化，0-目标未初始化。 + + BBOOL bInsideFOV;//目标是否在视场 + UINT32 unTotalCnt; // 目标已锁定的帧计数器(基于0计数) + UINT32 unTrackedCnt; // 目标实际存在的帧计数器(基于0计数) + UINT32 unContiTrackedCnt; // 目标连续非预测的帧计数器(基于0计数) + UINT32 unContiLostCnt; // 目标连续预测的帧计数器(基于0计数) + BBOOL bObjMiss; // 目标丢失/跟踪失败，解锁 + BBOOL bObjLost; //目标丢失/记忆跟踪 + + POINT32F ptPosPre; // 目标坐标 - 上一帧 //BYTES: 2*4=8 + POINT32F ptPos; // 目标坐标 - 当前帧 //BYTES: 2*4=8 + POINT32S ptPosRect; //目标坐标-修正值 + POINT32F ptPosFilter; // 目标坐标 - 滤波后 //BYTES: 2*4=8 + POINT32F ptPosBorn; // 目标初始位置 //BYTES: 2*4=8 + SIZE32F sfSize; // 目标宽高 //BYTES: 3*4=12 + SIZE32F sfSizeFilter; //宽滤波值 + SIZE32F sfSizeBorn; // 目标初始宽高 //BYTES: 3*4=12 + FLOAT32 fObjPxlsCnt; // 目标的象素点的个数 + SPEED32F sfSpeed; // 目标速度 //BYTES: 2*4=8 + UINT16 pxObjGray; // 目标灰度值 + FLOAT32 fSNR; // 目标信噪比 + FLOAT32 fObjStd; // 目标方差 4 + FLOAT32 fBGStd; // 目标背景方差 + UINT16 fGrayFilter; // 灰度滤波值 + POINT32F ptCentroid; // 质心定位置 + + ANGLE32F afAngle; // 目标伺服角度（方位角、俯仰角） //BYTES: 2*4=8 + SPEED32F sfAglSpeed; // 目标角速度（方位角速度、俯仰角速度） //BYTES: 2*4=8 + ANGLE32F afAngleBorn; // 目标初始伺服角度（方位角、俯仰角） //BYTES: 2*4=8 + FLOAT32 fConfidence; // 置信度 + SINT32 unClsType; // 目标类别 + ClsSrc emClsSrc; // 目标类别来源 + + PointXYZ stPos3D; // 目标3D坐标 + + ObjSrc nObjTypeSrc; // 目标来源 01-小目标 02-面目标 03-AI目标 04-KCF 05-TLD 06-手动起 + + UBYTE8 nOcclude_flag; //跟踪器遮挡状态 + + UBYTE8 nDeciStatus; //多跟踪器决策结果 0-无 1-kcf 2-AI + +} OBJECTSTATUS; + +//跟踪决策信息 +typedef struct tagTrackDecisionInfo +{ + FLOAT32 fIouAID_KCF;//识别与跟踪交并比 + FLOAT32 fIouTLD_AID;//TLD与识别交并比 + FLOAT32 fIouAIT_KCF;//AI跟踪与KCF交并比 + CENTERRECT32F deciRectAID; //重置KCF的AI识别结果 + BBOOL bAITResetKCF; //AITrack重置KCF + BBOOL bSelfResetAI; //AITrack自重置 + BBOOL bKCFResetAIT; //KCF重置AITrack + BBOOL bAIDResetKCF; //AI识别重置KCF + FLOAT32 fDisAID_KCF; // AI识别结果和跟踪的相对距离，用于稳跟判断 +}TrackDecisionInfo; + +// 算法处理的锁定指令，可根据需要扩展，尽量扁平化，不要设计多重控制。 +typedef enum tagLockMode +{ + LOCK_NONE = 0,//未锁定,无意义 + LOCK_AUTO = 10,//自动锁定处理，锁定探测目标管道（视场内、视场外） + LOCK_POINT = 21,//点选 + LOCK_RECT = 22,//框选 + LOCK_UNLOCK = 3//解锁 +}LockMode; + + + +// 以下几个数据类型根据项目需求修改，此处只做最关键字段的定义或示范性定义 +#define GLB_GUIDE_TARGET_NUM 10 //支持的一次性最大引导目标个数 + +//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ +// 定义操控盒到算法控制命令结构体 +typedef struct tagGlbPCCommand // BYTES: 15*2=30 +{ + // 外部下发系统工作状态 by wcw04046 @ 2021/12/06 + GLB_SYS_MODE ubSysStatusCmd; + + // 场景模式 + GLB_SCEN_MODE ubScenCmd;//对空、对地、对海、通用等 + + // 锁定命令字 + UBYTE8 ubCmd; // 控制命令：初始化、跟踪、解锁、切换视频源、切视场，所有的命令都在这里区分 + UINT16 ubCmdLast; // 上一个遥控指令 + UINT32 unSetLockunFrmId; // 命令下发帧编号 + BYTE8 ubSetID; // 命令特指目标编号，自动判断给-1 + + //-- 命令字附带参数 + + // 视场内锁定指令字,单一目标 + UINT16 unSetLockPosX; // 锁定点目标图像坐标中心点X + UINT16 unSetLockPosY; // 锁定点目标图像坐标中心点Y + UINT16 unSetLockBoxW; // 锁定波门宽度, 目标宽度 + UINT16 unSetLockBoxH; // 锁定波门高度, 目标高度 + + BBOOL bGuideUpDate; // 导引更新标志，多目标共享，下一次引导覆盖当前批目标 + + // 多目标引导锁定接口，最多支持10批次目标 + TargetGuide target[GLB_GUIDE_TARGET_NUM]; + +} GLB_PCCOMMAND; + +//+++++++定义算法控制参数结构体 +typedef struct tagGlbParameters +{ + GLB_SYS_MODE nSysMode; // 系统工作模式（响应外部下发） + GLB_STATUS nStatus; // 当前工作状态（算法自控） + GLB_STATUS nStatusBeforeLock; // 锁定前工作状态 + SINT32 nCatchFlag; // 捕获过程标记，处理初始跟踪到稳定跟踪过程 + GLB_VIDEO_TYPE nVideoSoureType; // 当前视频源类型 + GLB_SCEN_MODE nWorkScene; // 工作场景 01-对空 01-对海 00-未知 + SINT32 nPipeMaxNum; // 系统航迹规模（管道数量） + SINT32 nAlarmMaxNum; // 系统告警目标规模（告警队列长度） + + // 目标锁定控制 + LockMode stLockCtrl; // 是否锁定/修正攻击点：01目标锁定/修正攻击点，02 强制锁定 00不操作。 + POINT32F ptLockPos; // 实际锁定点坐标（实际锁定点X坐标、实际锁定点Y坐标） //BYTES: 2*4=8 + SIZE32S snLockBoxSize; // 锁定目标的宽和高 + BYTE8 ubLockID; //指定目标编号 + SINT32 nLockPipeInd; //锁定管道编号【重要】 + BBOOL bSelectObjManual; // 是否手动选择目标转跟踪。0-手动选择跟踪目标，1-自动选择跟踪目标 + + // 外引导 + BBOOL bEnRadarGuide; // 开启雷达引导 + SINT32 nGuideTargetNumber; // 外引导批号 + FLOAT32 fGuideAz; // 引导方位角 + FLOAT32 fGuidePt; // 引导高低角 + FLOAT32 fGuideAzSpeed; // 引导方位角速度 + FLOAT32 fGuidePtSpeed; // 引导俯仰角速度 + SINT32 nGuideDis; // 引导距离 + SINT32 nDetectRegionX; // 引导精度范围X + SINT32 nDetectRegionY; // 引导精度范围Y + BBOOL bGuideUpDate; // 引导更新标志 + SINT16 nGuideLastTime; // 导引有效时间，结束后没有收到新的导引，原导引失效 //S726 by wcw04046 @ 2021/11/25 + BBOOL bGuideTrackAway; // 当前跟踪与引导偏离标志 + POINT32F ptGuidePOS; // 引导位置换算坐标值 + BBOOL bGuidePosInFOV; // 导引位置在当前视场标志 + + // 多目标引导信息 + TargetGuide targetGuide[GLB_GUIDE_TARGET_NUM]; +} GLB_PARAMETERS; + +// ++++++++定义算法每帧输入信息 +typedef struct tagGlbInput // BYTES: 60 + 6*4 = 84 +{ + UBYTE8 unpxType; // 像素类型 + UBYTE8 unVideoSrc; // 视频源类型 + SINT32 nImageWidth; // 图像宽度 + SINT32 nImageHeight; // 图像高度 + UINT32 unFreq; // 帧频 + UINT32 unFrmId; // 当前帧图像帧编号 + ANGLE32F afSvoAgl; // 当前帧伺服角度（方位角、俯仰角） + SPEED32F sfSvoAglSpeed; // 当前帧伺服角速度（方位角速度、俯仰角速度） + PointBLH stPlanePos; // 当前载体位置 + EulerRPY afPlatformRPY; // 安装平台姿态角（方位角、俯仰角、横滚角） + CameraParam stCamera; // 相机内参 + ServoParam servoInfo; // 广义伺服编码器参数，不代表物方指向 + DeviceSetupError setupErr; // 安装误差 + SINT32 nElevationDiff; // 机载高程差 + SINT32 parallelFlg; // 平台优化使用标记，勿删。 +} GLB_INPUT; + + + + +//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ +// 定义算法输出结果结构体 +typedef struct tagGlbOutput // BYTES: 8*1 + 6*4 + 115*4 + 48 = 540 +{ + //SINT32 nPipeObjsCnt; // 当前管道目标个数 + //SINT32 nAlarmObjsCnt; // 当前告警目标个数 + //SINT32 nAlarmIDCnt; // 当前告警目标批号占用个数 + + //OBJECTSTATUS ObjectStatusTST; // 小目标跟踪输出的目标状态 //BYTES: 3*1 + 28*4 = 115 + //OBJECTSTATUS ObjectStatusCEND; // 形心跟踪输出的目标状态 //BYTES: 3*1 + 28*4 = 115 + //OBJECTSTATUS ObjectStatusKCF; // KCF跟踪输出的目标状态 + + //对地跟踪信息调试 + float fKCFRes; //KCF响应， + float fAITRes; // AI响应， + int nLearnCnt; //TLD学习计数 + int nTLDNum; // TLD聚类检测个数 + float fMaxNNConf; //TLD检测最大响应 + int nAITNum; //AI聚类检测个数() + int nAIJamCnt; //AI干扰状态 + int nAIChangCnt; //AI遮挡感知状态 + float fLargeResTH; //KCF重捕阈值 + float fArrKCFRes; + + //对空调试信息 + bool sky_bComplexEnv; // 复杂背景标志位 + bool sky_bInterferenceMem; // 干扰近记忆标志位 + bool sky_bResetByAIDet; // 跟踪被AI重置标记 + int sky_nClsType; // 目标类型 + unsigned char sky_nClassSource; // 目标类别输出来源 + unsigned char sky_nDecisionStatus; // 决策状态，输出来源 + int sky_nObjTypeSrc; // 目标跟踪来源 + int sky_TrkDownRatio; // 对空跟踪降采样倍数 + int sky_TrkMemFrm; // 跟踪目标进记忆帧数 + int sky_nTrkCX; // 决策目标信息中心点X + int sky_nTrkCY; // 决策目标信息中心点Y + int sky_nTrkW; // 决策目标信息宽度 + int sky_nTrkH; // 决策目标信息高度 + int sky_nTrkPxlsCnt; // 跟踪目标像素数 + float sky_fTrkConf; // 跟踪目标置信度 + + + OBJECTSTATUS ObjectStatus; // 跟踪决策输出的目标状态 + +} GLB_OUTPUT; + + +#ifndef _ALARM_TARGET_ +#define _ALARM_TARGET_ +typedef struct tagALARM_TARGET +{ + UINT32 unFrmID; //目标帧编号 + SINT32 nBatchID_1; //输出告警目标序号【目标列表显示】。0-不是告警目标，大于0-表示为第（nOutputID）个输出的告警目标 + SINT32 nInPipesID_1; //目标在管道数组中的编号。0-无对应管道，大于0-对应于第（nInPipesID-1）个管道目标。 + SINT32 nDetectCnt; //目标真实被检测到的帧数【目标列表显示】 + SINT32 nCntSinceCreate;// 被创建后总帧计数 + SINT32 nClassID; //目标类型，直接遗传管道结果 + SINT32 unClsType; // 目标类别 + UBYTE8 ubGrayType; //目标灰度类型 + UBYTE8 ubSizeType; //目标类型。1--小目标，2--面目标，3--临界目标（即二者均是），4--弱小目标 + UBYTE8 nObjStatusChange;//目标状态更改标记 0：持续观测目标 1：新目标 2：目标丢失中 -1：目标待删除 + UBYTE8 bInFOV; //是否属于当前视场 + SINT32 nInFOVCnt; //在视场帧计数 + FLOAT32 nThreatConf; //威胁度估计 + +}ALARM_TARGET; +#endif + + + +// AB滤波器 +typedef struct tagAglFilter +{ + float m_px;//方位角预测值 + float m_pdx;//方位角速度预测值 + float m_py;//俯仰角预测值 + float m_pdy;//俯仰角速度预测值 + + // 滤波参数 + float m_paraA; + float m_paraB; + + int m_ind; //序号 + float m_T;//1/系统频率 + bool m_Init;//初始化标记 + + int m_LastFrmID;//上一帧更新的帧编号，扫描模式下防止帧频不稳定（有时一圈更新一次，有时连续帧更新） + + POINT16S pnt_Predict; // 预测到像方 +}AglFilter; + + +#define GLB_OBJTRACK_LEN 100 //目标轨迹数组长度 + + +// 定义目标角度轨迹结构体 +typedef struct tagObjAglTrack32F +{ + UINT32 unFrmID; // 记录目标被检测到的帧编号 + ANGLE32F afAngle; // 目标方位角、俯仰角 + FLOAT32 fDist; // 距离 +} ANGLE_R; + +// 定义目标角度信息结构体 +typedef struct tagObjAngleR32F // BYTES: 5*4+28=48 +{ + SINT32 nCnt; // 已记录的历史角度信息个数 + SINT32 nStep; // 记录帧间隔 + SINT32 nEnd; // 历史循环队列末尾 + SINT32 nListSize; // 目标角度数组长度 + ANGLE_R parHistoryList[GLB_OBJTRACK_LEN]; // 目标角度信息历史记录 + SINT32 nFilterCnt; // 滤波帧数 + ANGLE_R arfFilter; // 滤波结果 + ANGLE_R arfSpeed; // 目标速度 + ANGLE_R arfPredict; // 目标当前帧预测 +} OBJ_ANGLE_R; + + +// 长短时滤波器结构 +typedef struct tagFilterMeanNL +{ + OBJ_ANGLE_R ObjAglListsNear; //目标短时间轨迹数组（每帧记录一次） + OBJ_ANGLE_R ObjAglListsLong; //目标长时间轨迹数组（每秒记录一次） + CENTERRECT crnObjPrediRtNear; //根据目标短时轨迹在当前帧预测到的位置中心矩形 + CENTERRECT crnObjPrediRtLong; //根据目标长时轨迹在当前帧预测到的位置中心矩形 + MINMAXRECT32S mrnObjPrediRtNear;//根据目标短时轨迹在当前帧预测到的位置边界矩形 + MINMAXRECT32S mrnObjPrediRtLong;//根据目标长时轨迹在当前帧预测到的位置边界矩形 + DIST32S dnObjPredictDist; //当前帧目标长短轨迹预测位置距离 + SINT32 nObjPredictFarCnt; //目标长短轨迹位置预测距离连续变大计数器 + BBOOL bObjPredictAbnormal; //目标长短轨迹位置预测异常标志 + BBOOL bObjTrackRefoundSucc; //在目标长短轨迹位置预测异常时，重新成功找到目标的标志 + SINT32 nObjTrackLostCntNear; //目标短时轨迹预测失败帧数 + SINT32 nObjTrackLostCntLong; //目标长时轨迹预测失败帧数 + SINT32 nAbnormalCnt; //跟踪异常计数器（连续---坏点 +}FilterMeanNL; + + +// XYZ-滤波器（吊舱对地无源定位） +typedef struct tagXYZFilter +{ + float m_px;//x预测值 + float m_pdx;//x速度预测值 + float m_py;//y预测值 + float m_pdy;//y速度预测值 + float m_pz;//z预测值 + float m_pdz;//z速度预测值 + + // 滤波参数 + float m_paraA; + float m_paraB; + + int m_ind; //序号 + float m_T;//1/系统频率 + bool m_Init;//初始化标记 + int m_LastFrmID; + POINT16S pnt_Predict; // 预测到像方 +}XYZFilter; + + + + + + +// 管道 +#ifndef _PIPE_ +#define _PIPE_ +typedef struct tagPIPE +{ + SINT32 nPipeID; // 管道编号，即队列ID + BBOOL bOccupy; // 标识管道是否已经被占用 + BBOOL bTarget; // 标识管道是否是目标 + UBYTE8 ubEventStatus; // 事件状态 + BBOOL bInsideFOV; // 标记管道是否属于当前视场（目标检测区域） + BBOOL bLost; // 管道消失标记：0-不是，1-是。 + BBOOL bTrackingPipe; // 已跟踪标记 + BBOOL bAlarm; // 标识管道是否是告警目标 + SINT32 unClsType; // 管道目标类别 + ClsSrc emClsSrc; // 目标类别来源 + UINT32 unTotalCnt; // 管道已创建的帧计数器 + UINT32 unInsideFOVCnt; // 记录管道经过其视场的次数 + UINT32 unContinueInFOVCnt; // 连续在视场帧数 + UINT32 unExistCnt; // 管道中目标在其视场内真实被检测到的次数 + UINT32 unLostCnt; // 管道已消失（目标在其视场内没有被检测到）的帧计数器 + UINT32 unContinueExistCnt; // 连续检测到次数 new by wcw + UINT32 nLostFrmID; // 记录管道目标消失前的帧编号 + SINT32 nInTargetID_1; // 是否在当前帧找到目标：0-管道在当前帧尚未找到对应的目标，大于0-管道对应当前帧第（nInTargetID-1）个目标 + UINT32 unLastExistTime; // 管道目标最后一次被检测到的帧时间（单位：10ms） + UBYTE8 ubEnd; // 队列尾 + TARGET_OBJECT objHistoryList[GLB_PIPE_DEPTH_MAX]; // 数组实现循环队列 //BYTES: （3*1 + 1*2 + 7*4 + 3*8 = 57）* 10 = 570 + OBJECT_FILTER ObjectFilter; // 目标的滤波结果 //BYTES: 9*4=36 + POINT32F ptBornPos; // 目标初始位置 //BYTES: 2*2=4 + POINT32F ptCurrentPnt; // 管道目标在当前帧预测位置 //BYTES: 2*2=4 + POINT32F ptStopPnt; // 管道目标在目标转入预测前的位置 //BYTES: 2*2=4 + ANGLE32F afBornAgl; // 目标初始角度 + ANGLE32F afCurrentAgl; // 管道目标在当前帧预测角度 + ANGLE32F afStopAgl; // 管道目标在目标转入预测前的角度 + SPEED32F sfAglSpeed; // 目标角速度 + SINT32 nMoveCnt; // 目标图像坐标的运动次数 + SINT32 bAreaChange; // 标记管道中的目标面积大小变化趋势 //CJ, 20121008 + UBYTE8 nPipeType; // 表示管道目标类型：1--小目标，2--面目标，3--临界目标（即二者均是），4--弱小目标 + FLOAT32 fConfidence; // 管道为目标的置信度 + SINT32 nBPCnt; // 管道被认为是坏点的次数 + UBYTE8 ubDirection; // 管道的方向性扇区编号,运动方向（0表示静止） + SINT32 nSearchRadius; // 管道搜索半径 + SINT32 nDelCntThres; // 自适应的删除帧数，提高稳定管道生存周期 + + SINT32 nInAlarmArrayIndex_1; //管道位于告警队列位置，从1开始 + SINT32 nAlarmBatchID_1; //管道告警批号，从1开始 + SINT32 nGuideBatchID; //外部批号，如果存在外部批号，则使用外部批号送显，内部批号不变，灵活度高 + + XYZFilter stXYZMotionMod; //3D运动AB滤波器，可用于吊舱下视场景的无源定位+预测。 + AglFilter stMotionMod; //极坐标AB运动模型 + FilterMeanNL stMotionMod_mean; //长短时运动模型 + + UINT32 unContinueOutFOVCnt; //管道连续出视场次数 + UINT32 unSimTargetNum; //波门内相似目标个数 + BBOOL blookout; //是否警戒周边干扰 +}PIPE; +#endif + + +//管道处理参数（上位机软件界面可调） +#ifndef _PIPEParameters_ +#define _PIPEParameters_ +typedef struct tagPIPEParameters +{ + SINT32 nPipeFOVLostCnt; //管道目标在其视场内没有被找到的次数 + SINT32 nPipeFilterCnt; //管道的滤波帧数 + SINT32 nPipeRadiusSearch; //管道搜索范围直径 -- 搜索 + SINT32 nPipeRadiusLock; //管道搜索范围直径 -- 锁定 + SINT32 nPipeRadiusTrack; //管道搜索范围直径 -- 跟踪 + SINT32 nPipeRadiusLost; //管道搜索范围直径 -- 跟踪丢失 + + //管道确认与删除帧数 + SINT32 nAwsFrmNumScan; //管道目标的确认帧数- 扫描模式 4 + SINT32 nAwsFrmNumStare; //管道目标的确认帧数- 凝视模式 4 + SINT32 nPipeDelMinScan; //管道最少延迟删除帧数- 扫描模式 4 + SINT32 nPipeDelMinStare; //管道最少延迟删除帧数- 凝视/跟踪模式 4 + + //最优管道目标决策控制参数 + FLOAT32 fPriorDistWeight; //最优目标的距离差异权重 + FLOAT32 fPriorGrayWeight; //最优目标的灰度权重 + FLOAT32 fPriorSNRWeight; //最优目标的信噪比权重 + FLOAT32 fPriorSizeWeight; //最优目标的大小权重 + FLOAT32 fPriorSpeedWeitht; //最优目标的速度权重 + + SINT32 nBinSector; //hist扇区 + BBOOL bEnableBPJudge; //坏点判断。1-使能坏点判断，0-不使能坏点判断 + BBOOL bCloseBadPointPipe; //关闭坏点管道 + //BBOOL bEnableBPJudgTrack; //跟踪坏点判断。1-使能跟踪坏点判断，0-不使能跟踪坏点判断 + //BBOOL bEnableBPJudgDetect; //检测坏点判断。1-使能检测坏点判断，0-不使能检测坏点判断 + + + // dengs 2018.12.25 speed + BBOOL bEnableSpeedMin; //是否使用目标速度下限限制。1-使用，0-不使用。 + BBOOL bEnableSpeedMax; //是否使用目标速度上限限制。1-使用，0-不使用。 + FLOAT32 fObjAglSpeedMin; //目标角速度下限（单位：度/秒） + FLOAT32 fObjAglSpeedMax; //目标角速度上限（单位：度/秒） + + //凝视去虚警策略 + BBOOL bEnableMoveDirLimit; //是否使用运动方向限制（要求方向一致性） + BBOOL bEnableMoveDet; // 动目标检测 by wcw04046 @ 2019/01/30 + + // 周扫管道处理去虚警参数ds tn-2 + BBOOL bUseMotionFA; //是否使用运动去虚警策略开关 1 + UBYTE8 ubPreScanNum; //预扫描圈数 1 + + //屏蔽区 + FLOAT32 fScanAzMin; + FLOAT32 fScanAzMax; + FLOAT32 fScanPtMin; + FLOAT32 fScanPtMax; + + // 默认滤波器参数 + FLOAT32 fFilterA; + FLOAT32 fFilterB; + +}PIPE_PARAMETERS; +#endif + + +// 对空检测器参数 +#ifndef _SkyDetectParameters_ +#define _SkyDetectParameters_ +typedef struct tagParam_SkyDetect +{ + BBOOL bEnableDetcetSmallTarget; //小目标检测开关 + BBOOL bEnableDetcetAreaTarget; //面目标检测开关 + BBOOL bEnableDetcetDimTarget; //弱小目标检测开关 + + //小目标检测参数设置（上位机软件界面可调） + ///////////////////////////////////////////////////////// + FLOAT32 fSmallDetectGDK; //小目标检测snr阈值 + FLOAT32 fDimGdk; //弱小目标检测的gdk下限 + SINT32 nObjSizeMin; //目标大小阈值下限 + SINT32 nObjSizeMax; //目标大小阈值上限 + + ///////////////////////////////////////////////// + //面目标检测参数 + SINT32 nDSmpScale; //搜索区域降采样倍数 + + SINT32 fAreaDetectGradDiffThre; //面目标检测梯度差阈值 + + SINT32 nGrayThresMinBright; //亮面目标检测灰度差阈值 + SINT32 nGrayThresMinDark; //暗面目标检测灰度差阈值 + + + ///////////////////////////////////////////////// + //小面公共参数 + SINT32 nDetectGrayType; //检测灰度类型 + +}Param_SkyDetect; +#endif + +// 对空跟踪器参数 +#ifndef _SkyTrackerParameters_ +#define _SkyTrackerParameters_ +typedef struct tagParam_SkyTracker +{ + //跟踪开关及参数设置 + BBOOL Sky_bEnableFullImgDet; //对空跟踪过程中是否使能全图目标检测 + BBOOL Sky_bEnableTrackSA; //小面目标跟踪开关(小目标管道或面目标质心) + BBOOL Sky_bEnableKCF; //KCF跟踪算法开关。1-开，0-关。 + BBOOL Sky_bEnableMatcher; //模板匹配算法开关。1-开，0-关。 + BBOOL Sky_bSelectObjManual; //是否手动选择目标转跟踪。1-手动选择跟踪目标，0-自动选择跟踪目标 + BBOOL Sky_bUseAIDet; //是否使用AI检测信息 + SINT32 Sky_nTrackMemFrmNum; //记忆跟踪帧数 + SINT32 nSmallObjSizeMax; //小面目标的像素数切换阈值 + //用于控制跟踪局部的检测器 + Param_SkyDetect prmTSkyDet; //对空跟踪局部检测器参数 + +}Param_SkyTracker; +#endif + +// 对地跟踪器参数 +#ifndef _GroundTrackerParameters_ +#define _GroundTrackerParameters_ +typedef struct tagParam_GroundTracker +{ + BBOOL bEnableAccuracyTrack; //跟踪精度优化 默认1 + BBOOL bUseServePredict; //采用伺服预测 默认1 + BBOOL bEnableLKCorrect; //LK光流算法开关 默认1 + BBOOL bEnableKCF; //KCF跟踪算法开关 默认1 + UBYTE8 nKcfUpdataStep; //KCF更新频率 默认2 + BBOOL bKCFMutiScales; //KCF多尺度设置 默认1 + FLOAT32 fArrestKCFMinThre; //KCF重捕阈值下限 默认0.38 + FLOAT32 fArrestKCFMaxThre; //KCF重捕阈值上限 默认0.68 + FLOAT32 fKCFResthre; //KCF跟踪阈值 默认0.2 + BBOOL bTLDOccJudge; //辅助遮挡判断 默认0 + BBOOL bEnableDaSiamRPN; //AI跟踪算法开关 默认1 + BBOOL bEnableAIOccJudge; //AI遮挡条件开关 默认1 + FLOAT32 fAIOccThre; //AI跟踪阈值 默认0.85 + BBOOL bEnableAIDetect; //AI识别开关 默认0 + //重捕参数控制 + BBOOL bEnableArrestCorr; //重捕范围限制 默认0 + BBOOL bEnableArrestAngle; //重捕角度限制 默认0 + BBOOL bEnableAreestEsay; //弱重捕开关 默认1 + SINT32 nArrestEsayCnt; //弱重捕时长 默认30帧 + + // 运动模型 + BBOOL bEnable3dPredict; // 3D预测开关 默认0 + + +}Param_GroundTracker; +#endif + +//跟踪算法控制参数 +#ifndef _ARIDLL_Parameters_ +#define _ARIDLL_Parameters_ +typedef struct tagARIDLL_PARMA +{ + //// 对空检测器参数 + Param_SkyDetect PrmSkyDetect; //对空检测器算法参数 + + // 管道参数 + SINT32 nPipeRadiusSearch; //管道搜索范围直径 -- 搜索 + SINT32 nPipeRadiusLock; //管道搜索范围直径 -- 锁定 + SINT32 nPipeRadiusTrack; //管道搜索范围直径 -- 跟踪 + SINT32 nPipeRadiusLost; //管道搜索范围直径 -- 跟踪丢失 + SINT32 nAwsFrmNumScan; //管道目标的确认帧数- 扫描模式 4 + SINT32 nAwsFrmNumStare; //管道目标的确认帧数- 凝视模式 4 + SINT32 nPipeDelMinScan; //管道最少延迟删除帧数- 扫描模式 4 + SINT32 nPipeDelMinStare; //管道最少延迟删除帧数- 凝视/跟踪模式 4 + FLOAT32 fA; //轨迹滤波器AB参数 + FLOAT32 fB; //轨迹滤波器AB参数 + + SINT32 nLockPointW; //点选锁定目标默认宽度 + SINT32 nLockPointH; //点选锁定目标默认高度 + + BBOOL bCloseBadPointPipe; //关闭坏点管道 + + // 跟踪控制 + Param_SkyTracker stSkyParam; //对空跟踪器算法参数 + Param_GroundTracker stGrdParam; //对地跟踪算法参数 + + // 日志控制 + UBYTE8 nLogLevel; //允许的日志级别 + + UBYTE8 res[200];//预留 +}ARIDLL_PARMA; +#endif + + +typedef struct tagTSky_Output +{ + SizeType m_SizeMode; // 跟踪目标类型 + Param_SkyTracker mTrakingPara_Output; // 对空跟踪输出参数 + + SINT32 m_nTargetNum; // 对空跟踪局部目标检测个数 + TARGET_OBJECT * mTarget_Array; // 对空跟踪局部目标检测队列 + + SINT32 m_nMatcherNum; // 对空模板匹配目标检测个数 + MATCHER_TARGET* mMatcher_Array; // 对空模板匹配目标检测队列 + + OBJECTSTATUS ObjectStatusTST; // 小目标跟踪输出的目标状态 //BYTES: 3*1 + 28*4 = 115 + OBJECTSTATUS ObjectStatusCEND; // 形心跟踪输出的目标状态 //BYTES: 3*1 + 28*4 = 115 + OBJECTSTATUS ObjectStatusKCF; // KCF跟踪输出的目标状态 + OBJECTSTATUS ObjectStatusDesc; // 决策跟踪输出的目标状态 +}TSky_Output; + +#endif diff --git a/stitch/src/utils/Arith_Utils.cpp b/stitch/src/utils/Arith_Utils.cpp new file mode 100644 index 0000000..1260e6d --- /dev/null +++ b/stitch/src/utils/Arith_Utils.cpp @@ -0,0 +1,375 @@ +#include "Arith_Utils.h" +#include + +#include "Arith_SysStruct.h" + + + + + +void AddMat(double* A, double* B, int mn, double* AaddB) +{ + for (size_t i = 0; i < mn; i++) + { + AaddB[i] = A[i] + B[i]; + } + +} + +void MultiMatScalar(double scalar, double* A, double* sA, int mn) +{ + for (size_t i = 0; i < mn; i++) + { + sA[i] = scalar * A[i]; + } +} + +void TransposeMat(double* A, int w, int h, double* AT) +{ + for (int i = 0; i < h; i++) + { + for (int j = 0; j < w; j++) + { + AT[j * w + i] = A[i * h + j]; + } + } +} + + +/* 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; +} + +/* 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; +} + + + +/* 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; + +} +//********************************************************************************************* +// 矩阵乘法 +//********************************************************************************************* +void MultiMat(double* A, double* B, double* AB, int m, int n, int p) +{ + // A：m*n ; B：n*p ; + int i, j, k; + double Multisum; + + for (i = 0; i < m; i++) + { + for (j = 0; j < p; j++) + { + Multisum = 0; + for (k = 0; k < n; k++) + Multisum += A[i * n + k] * B[k * p + j]; + + AB[i * p + j] = Multisum; + } + } +} + + + +//********************************************************************************************* +// +//********************************************************************************************* +void MultiMat33(double* A, double* B, double* AB) +{ + MultiMat(A, B, AB, 3, 3, 3); +} + +//********************************************************************************************* +// +//********************************************************************************************* +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); +} + +// 对点/矢量旋转 +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 eye3(double* M) +{ + memset(M, 0, sizeof(double) * 9); + M[0] = 1; + M[4] = 1; + M[8] = 1; +} + +void invMat3(double* A, double* invA) +{ + double a1 = A[0]; + double a2 = A[1]; + double a3 = A[2]; + double b1 = A[3]; + double b2 = A[4]; + double b3 = A[5]; + double c1 = A[6]; + double c2 = A[7]; + double c3 = A[8]; + + //A* + double Astar[9] = { 0 }; + Astar[0] = b2 * c3 - c2 * b3; + Astar[1] = c1 * b3 - b1 * c3; + Astar[2] = b1 * c2 - c1 * b2; + + Astar[3] = c2 * a3 - a2 * c3; + Astar[4] = a1 * c3 - c1 * a3; + Astar[5] = a2 * c1 - a1 * c2; + + Astar[6] = a2 * b3 - b2 * a3; + Astar[7] = b1 * a3 - a1 * b3; + Astar[8] = a1 * b2 - a2 * b1; + + double detA = a1 * (b2 * c3 - c2 * b3) - a2 * (b1 * c3 - c1 * b3) + a3 * (b1 * c2 - c1 * b2); + + for (size_t i = 0; i < 9; i++) + { + invA[i] = Astar[i] / detA; + } + +} + + +// 旋转向量转矩阵 +void rotationVectorToMatrix(PointXYZ rotaVec_One, float rotaAgl, double* Mat) +{ + // + PointXYZ vec_norm = NormPoint(rotaVec_One); + + // K + double K[9] = { 0,-vec_norm.Z,vec_norm.Y, + vec_norm.Z,0,-vec_norm.X, + -vec_norm.Y,vec_norm.X,0 }; + + // eye(3) + double eye[9] = { 0 }; + eye3(eye); + + // eye + sind(r) * K + (1-cos(r)) * K * K + double sK[9] = { 0 }; + MultiMatScalar(sin(RADIAN(rotaAgl)), K, sK, 9); + + double KK[9] = { 0 }; + MultiMat33(K, K, KK); + + double scK[9] = { 0 }; + MultiMatScalar(1 - cos(RADIAN(rotaAgl)), KK, scK, 9); + + double tmp[9] = { 0 }; + AddMat(eye, sK, 9, tmp); + + AddMat(tmp, scK, 9, Mat); + + +} + + + +#ifdef _WIN32 // Windows平台 + +#include + +std::string GetDynamicLibraryPath() +{ + HMODULE hModule = GetModuleHandle(NULL); + char path[MAX_PATH]; + GetModuleFileName(hModule, path, MAX_PATH); + std::string fullPath(path); + size_t pos = fullPath.find_last_of("\\/"); + return fullPath.substr(0, pos); +} + +#elif __linux__ // Linux平台 + + +#include +#include + +std::string GetDynamicLibraryPath() +{ + Dl_info dlInfo; + dladdr((void*)GetDynamicLibraryPath, &dlInfo); + char* path = realpath(dlInfo.dli_fname, NULL); + std::string fullPath(path); + free(path); + size_t pos = fullPath.find_last_of("/"); + return fullPath.substr(0, pos); +} + +#else + +#error Unsupported platform + +#endif + + + +unsigned char FourPointInterpolation(unsigned char* pSrcImage, int iWidth, int iHeight, float x, float y) +{ + if (x < 0 || y < 0 || x > iWidth || y > iHeight) + return 0; + // 四个最临近象素的坐标(i1, j1), (i2, j1), (i1, j2), (i2, j2) + int i1, i2; + int j1, j2; + + // 四个最临近象素值 + unsigned char f1, f2, f3, f4; + + // 计算四个最临近象素的坐标 + i1 = fmax((int)x, 0); + i2 = fmin(i1 + 1, iWidth - 1); + j1 = fmax((int)y, 0); + j2 = fmin(j1 + 1, iHeight - 1); + + float t, s; //t = x - [x], s = y- [y]; + + t = x - i1; + s = y - j1; + + //四个最临近象素点的值的加权值 + float uv0, uv1, uv2, uv3; //权重之和为1：uv0+uv1+uv2+uv3 = 1 + uv0 = (1 - s) * (1 - t); + uv1 = (1 - s) * t; + uv2 = (1 - t) * s; + uv3 = s * t; + + + //根据不同情况分别处理 + if (x - iWidth + 1 > 0) + { + // 要计算的点在图像右边缘上 + if (y - iHeight + 1 < 0) + { + // 要计算的点正好是图像最右/左下角那一个象素，直接返回该点象素值 + f1 = *(pSrcImage + iWidth * j1 + i1); + return f1; + } + else + { + // 在图像右/左边缘上且不是最后一点，直接一次插值即可 + f1 = *(pSrcImage + iWidth * j1 + i1); + f3 = *(pSrcImage + iWidth * j2 + i1); + + // 返回插值结果 + return ((unsigned char)(f1 + s * (f3 - f1))); + } + } + else if (y - iHeight + 1 > 0) + { + // 要计算的点在图像下边缘上且不是最后一点，直接一次插值即可 + f1 = *(pSrcImage + iWidth * j1 + i1); + f2 = *(pSrcImage + iWidth * j1 + i2); + + // 返回插值结果 + return ((unsigned char)(f1 + t * (f2 - f1))); + } + else + { + // 计算四个最临近象素值 + f1 = *(pSrcImage + iWidth * j1 + i1); + f2 = *(pSrcImage + iWidth * j1 + i2); + f3 = *(pSrcImage + iWidth * j2 + i1); + f4 = *(pSrcImage + iWidth * j2 + i2); + + return (unsigned char)(uv0 * f1 + uv1 * f2 + uv2 * f3 + uv3 * f4); + } +} \ No newline at end of file diff --git a/stitch/src/utils/Arith_Utils.h b/stitch/src/utils/Arith_Utils.h new file mode 100644 index 0000000..a3ef8e7 --- /dev/null +++ b/stitch/src/utils/Arith_Utils.h @@ -0,0 +1,65 @@ +#ifndef ARITH_UTILS_H +#define ARITH_UTILS_H + +#pragma once +#include "Arith_SysStruct.h" +#include "opencv2/opencv.hpp" + +using cv::Point2d; +using cv::Point2f; + + + + +unsigned char FourPointInterpolation(unsigned char* pSrcImage, int iWidth, int iHeight, float x, float y); + +// 矩阵加法 +void AddMat(double* A, double* B, int mn, double* AaddB); + +// 矩阵乘法 +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); + + +// 标量乘以矩阵 +void MultiMatScalar(double scalar, double* A, double* sA, int mn); + +// 矩阵转置 +void TransposeMat(double* A, int m, int n, double* AT); + +// 3阶矩阵逆，不判断逆是否存在 +void invMat3(double* A, double* invA); + + +// 获得三阶单位阵 +void eye3(double* M); + +// 绕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); + +// 坐标(矢量)旋转: M1(3*3) * M2(3,1) +PointXYZ RtPoint(double* M, PointXYZ Point); + +// 坐标(矢量)归一化 +PointXYZ NormPoint(PointXYZ Point); + +// 罗得里格公式 +void rotationVectorToMatrix(PointXYZ rotaVec_One, float rotaAgl, double* Mat); + +// 获取动态库所在路径 +std::string GetDynamicLibraryPath(); +#endif \ No newline at end of file