StdTracker_Board_App/hi3559av100/NeoTracker/src/Arith_EOController.cpp

#include "Arith_EOController.h"
#include "Arith_CoordModule.h"
#include "Arith_Common.hpp"
#include "Arith_ImgOperate.h"
#include "Arith_AI_Tracker.h"
#include "opencv2/opencv.hpp"

Arith_EOController::Arith_EOController()
{
    g_GLB_bInitialize = FALSE;      //上电初始化

    memset(&g_GLB_stPara, 0, sizeof(GLB_PARAMETERS)); // 算法控制参数
    memset(&g_GLB_stInput, 0, sizeof(GLB_INPUT)); //参数行中的输入信息，包含帧编号/伺服等
    memset(&g_GLB_stCommand, 0, sizeof(GLB_PCCOMMAND));//跟踪舱上位机软件到PC算法控制命令
    memset(&g_GLB_stCommand_Pre, 0, sizeof(GLB_PCCOMMAND));
    memset(&g_GLB_stOutput, 0, sizeof(GLB_OUTPUT));//算法输出结果
}

Arith_EOController::~Arith_EOController()
{
    // 待补充资源释放过程
}

/**********************************************************
 * 函数名称：Arith_Controller()
 * 功能描述：系统主控制函数
 * 输入参数：SINT32 nWidth   --  图像宽度
 *            SINT32 nHeight  --  图像高度
 * 输出参数：无
 * 返 回 值：无
 * 调用关系：无
 * 其它说明：无
 **********************************************************/
BBOOL Arith_EOController::Arith_Controller(GD_VIDEO_FRAME_S img)
{
    BBOOL ubSuccessFlag = FALSE;

    //XLOG_DEBUG("XLOGGER IS RUNNIG!");

    //获取并响应任务机指令
    CMD_GetCommand();

    //响应算法控制命令
    CMD_RespondCommand();


    //删除坏点
    //if (g_GLB_stPara.bEnableBPJudgDetect)
    //{
    //    BP_BPReplace(pSrc, nWidth, nHeight);
    //}
    //else
    //{
    //    BP_CleanUpBPList();
    //}


    //处理系统工作模式逻辑
    //switch (g_GLB_stPara.nSysMode)
    //{
    //    // 直接透传
    //}

    // 处理跟踪器工作状态逻辑
    switch (g_GLB_stPara.nStatus)
    {
        // 扇扫搜索状态
    case GLB_STATUS_FSCAN:
        Arith_Status_FSCAN(img, g_GLB_stInput, g_GLB_stPara);
        break;
        // 周扫搜索状态
    case GLB_STATUS_SCAN:
        Arith_Status_SCAN(img, g_GLB_stInput, g_GLB_stPara);
        break;
        //凝视搜索状态
    case GLB_STATUS_SEARCH:
        Arith_Status_SEARCH(img, g_GLB_stInput, g_GLB_stPara);
        break;

        //单目标随动跟踪状态
    case GLB_STATUS_TRACK:
        Arith_Status_TRACK(img, g_GLB_stInput, g_GLB_stPara, g_GLB_stArithPara);
        break;

        // 待命模式
    case GLB_STATUS_WAIT:
        Arith_Status_WAIT(img, g_GLB_stInput, g_GLB_stPara);
        break;

        // 丢失重捕模式
    case GLB_STATUS_LOST:
        Arith_Status_LOST(img, g_GLB_stInput, g_GLB_stPara);
        break;

        // 未知状态
    case GLB_STATUS_UNKOWN:
        break;

        // 多目标随动跟踪状态
    case GLB_STATUS_MOTRACK:
        Arith_Status_MOTRACK(img, g_GLB_stInput, g_GLB_stPara, g_GLB_stArithPara);
        break;


    default:
        break;
    }

    return true;
}


/*************************************
* Method:                Arith_ImportExternTargets()
* Function Description:  导入外部目标列表
* CreateData:            2024/10/11
* Input Param:           TARGET_OBJECT * pTargetList:输入目标列表
* Input Param:           SINT32 nNum:输入个数
* Output Param:
* Return:                SINT32:实际导入的目标个数
* Call Relation:
* Other Description:
*************************************/
SINT32 Arith_EOController::Arith_ImportExternTargets()
{
    // 导入策略根据需求修改
    // 1. 先合并外部目标
    memcpy(g_pFrameTargetArray, g_pExternTargetArray, sizeof(TARGET_OBJECT) * g_nExternTargetNum);
    
    // 2.剩余空间合并传统检测算法的目标队列
    SINT32 nRemain = MIN(MAX(0, INPUT_OBJ_NUM - g_nExternTargetNum), g_GLB_Detectors->m_FrmObjsCnt);
    memcpy(&g_pFrameTargetArray[g_nExternTargetNum], g_GLB_Detectors->GetTargetArray(),sizeof(TARGET_OBJECT) * nRemain);

    g_TotalTargetNum = g_nExternTargetNum + nRemain;

    return g_TotalTargetNum;
}

void Arith_EOController::Arith_CleanTargetArray()
{
    // 严格按照目标个数取，避免初始化大段内存
    g_TotalTargetNum = 0;
    g_nExternTargetNum = 0;
}


/**********************************************************
 * 函数名称：Arith_SystemInit()
 * 功能描述：系统初始化函数
 * 输入参数：SINT32 nWidth   --  图像宽度
 *          SINT32 nHeight  --  图像高度
 *          GLB_SYS_MODE nSysMode -- 系统工作模式
 *          GLB_SCEN_MODE nScenMode -- 场景模式
 * 输出参数：无
 * 返 回 值：无
 * 调用关系：无
 * 其它说明：无
 **********************************************************/
void Arith_EOController::Arith_SystemInit(SINT32 nWidth, SINT32 nHeight, GLB_SYS_MODE nSysMode, GLB_SCEN_MODE nScenMode)
{
    //初始化算法输入参数
    GLB_InitInputs(nWidth, nHeight);

    //初始化算法控制命令
    CMD_CleanUpPCCommand();

    //初始化系统参数
    GLB_InitSysParameters(nWidth, nHeight, nSysMode, nScenMode);

    //初始化算法参数
    GLB_InitArithParameters();

    //初始化图像算法资源
    GLB_InitArithModule(nWidth, nHeight);

    //标记初始化完成
    g_GLB_bInitialize = TRUE;
}



////////////////////////////////////////////////////////////////////////////////
//--函数定义
////////////////////////////////////////////////////////////////////////////////
/**********************************************************
 * 函数名称：GLB_InitInputs()
 * 功能描述：系统初始化-算法输入参数
 * 输入参数：SINT32 nWidth   --  图像宽度
 *           SINT32 nHeight  --  图像高度
 * 输出参数：无
 * 返 回 值：无
 * 调用关系：无
 * 其它说明：无
 **********************************************************/
void Arith_EOController::GLB_InitInputs(SINT32 nWidth, SINT32 nHeight)
{
    //参数行
    memset(&g_GLB_stInput, 0, sizeof(GLB_INPUT));

    //图像尺寸
    g_GLB_stInput.nImageWidth = nWidth;
    g_GLB_stInput.nImageHeight = nHeight;
}

/**********************************************************
 * 函数名称：GLB_InitParameters()
 * 功能描述：系统初始化-算法控制参数
 * 输入参数：无
 * 输出参数：无
 * 返 回 值：无
 * 调用关系：无
 * 其它说明：无
 **********************************************************/
void Arith_EOController::GLB_InitSysParameters(SINT32 nWidth, SINT32 nHeight, GLB_SYS_MODE nSysMode, GLB_SCEN_MODE nScenMode)
{
    // 初始化工作模式
    g_GLB_stPara.nSysMode = nSysMode;

    // 初始化场景模式
    g_GLB_stPara.nWorkScene = nScenMode;

    // 默认算法状态
    if(nSysMode == GLB_SYS_MODE::GLB_SYS_STARE)
    {
        g_GLB_stPara.nStatus = GLB_STATUS_SEARCH;
    }
    else if(nSysMode == GLB_SYS_MODE::GLB_SYS_FSCAN)
    {
        g_GLB_stPara.nStatus = GLB_STATUS_FSCAN;
    }
    else if(nSysMode == GLB_SYS_MODE::GLB_SYS_SCAN)
    {
        g_GLB_stPara.nStatus = GLB_STATUS_SCAN;
    }

    // 管道数量
    g_GLB_stPara.nPipeMaxNum = 500;

    // 告警数量
    g_GLB_stPara.nAlarmMaxNum = 99;
    

    //雷达导引
    g_GLB_stPara.bEnRadarGuide = false;
    g_GLB_stPara.nGuideTargetNumber = 0;


    // 锁定参数初始化
    g_GLB_stPara.ptLockPos.x = nWidth / 2.0f;
    g_GLB_stPara.ptLockPos.y = nHeight / 2.0f;
    g_GLB_stPara.snLockBoxSize.w = 40;
    g_GLB_stPara.snLockBoxSize.h = 40;

    // qw 雷达导引区域大小
    g_GLB_stPara.nDetectRegionX = (SINT32)GLB_RADARGUIDE_RECT / 2;
    g_GLB_stPara.nDetectRegionY = (SINT32)GLB_RADARGUIDE_RECT / 2;

    // 初始化为人工锁定方式
    g_GLB_stPara.bSelectObjManual = true;
}


void  Arith_EOController::GLB_InitArithParameters()
{
    // 初始化跟踪参数模板
    memset(&g_GLB_TKPara_Template,0,sizeof(Tracker_Param));

    //对空参数下发
    g_GLB_TKPara_Template.skyParam.Sky_nTrackMemFrmNum = 1000;
    g_GLB_TKPara_Template.skyParam.Sky_bEnableFullImgDet = false;
    g_GLB_TKPara_Template.skyParam.Sky_bEnableTrackSA = true;
    g_GLB_TKPara_Template.skyParam.Sky_bEnableKCF = true;
    g_GLB_TKPara_Template.skyParam.Sky_bEnableMatcher = false;
    g_GLB_TKPara_Template.skyParam.nSmallObjSizeMax = 32;
    g_GLB_TKPara_Template.skyParam.Sky_bSelectObjManual = 1;
    g_GLB_TKPara_Template.skyParam.Sky_bUseAIDet = 0;
    g_GLB_TKPara_Template.skyParam.prmTSkyDet.bEnableDetcetAreaTarget = true;
    g_GLB_TKPara_Template.skyParam.prmTSkyDet.bEnableDetcetDimTarget = false;
    g_GLB_TKPara_Template.skyParam.prmTSkyDet.fSmallDetectGDK = 5;
    g_GLB_TKPara_Template.skyParam.prmTSkyDet.nGrayThresMinBright = 20;
    g_GLB_TKPara_Template.skyParam.prmTSkyDet.nGrayThresMinDark = 10;
    g_GLB_TKPara_Template.skyParam.prmTSkyDet.fAreaDetectGradDiffThre = 20;
    g_GLB_TKPara_Template.skyParam.prmTSkyDet.nDetectGrayType = 1;
    g_GLB_TKPara_Template.skyParam.prmTSkyDet.nDSmpScale = 4;


    //对地参数下发
    g_GLB_TKPara_Template.grdParam.bEnableAccuracyTrack = true;
    g_GLB_TKPara_Template.grdParam.bUseServePredict = false;
    g_GLB_TKPara_Template.grdParam.bEnableLKCorrect = true;
    g_GLB_TKPara_Template.grdParam.bEnableKCF = true;
    g_GLB_TKPara_Template.grdParam.nKcfUpdataStep = 2;
    g_GLB_TKPara_Template.grdParam.bKCFMutiScales = true;
    g_GLB_TKPara_Template.grdParam.fArrestKCFMinThre = 0.38;
    g_GLB_TKPara_Template.grdParam.fArrestKCFMaxThre = 0.68;
    g_GLB_TKPara_Template.grdParam.fKCFResthre = 0.23;
    g_GLB_TKPara_Template.grdParam.bTLDOccJudge = true;
    g_GLB_TKPara_Template.grdParam.bEnableDaSiamRPN = false;
    g_GLB_TKPara_Template.grdParam.bEnableAIOccJudge = true;
    g_GLB_TKPara_Template.grdParam.fAIOccThre = 0.85;
    g_GLB_TKPara_Template.grdParam.bEnableAIDetect = true;
    g_GLB_TKPara_Template.grdParam.bEnableArrestCorr = false;
    g_GLB_TKPara_Template.grdParam.bEnableArrestAngle = false;
    g_GLB_TKPara_Template.grdParam.bEnableAreestEsay = false;
    g_GLB_TKPara_Template.grdParam.nArrestEsayCnt = 250;
}



void Arith_EOController::GLB_InitArithModule(SINT32 nWidth, SINT32 nHeight)
{
    // 创建内部队列
    g_pFrameTargetArray = new TARGET_OBJECT[INPUT_OBJ_NUM];
    memset(g_pFrameTargetArray, 0, sizeof(TARGET_OBJECT) * INPUT_OBJ_NUM);
    g_TotalTargetNum = 0;

    // 创建外部目标队列
    g_pExternTargetArray = new TARGET_OBJECT[INPUT_OBJ_NUM];
    memset(g_pExternTargetArray, 0, sizeof(TARGET_OBJECT) * INPUT_OBJ_NUM);
    g_nExternTargetNum = 0;

    // 创建检测器
    g_GLB_Detectors = new Detectors(nWidth, nHeight);

    // 创建管道模块   
    g_GLB_PipeProc = API_MOT_PIPE::Create(g_GLB_stPara.nPipeMaxNum,g_GLB_stPara.nAlarmMaxNum);


    // 创建与管道配合的跟踪器指针数组
    g_GLB_Trackers = new Tracker_Ptr[g_GLB_stPara.nPipeMaxNum];

    for (size_t i = 0; i < g_GLB_stPara.nPipeMaxNum; i++)
    {
        g_GLB_Trackers[i] = NULL;
    }
    
    //memset(g_GLB_Trackers, 0, sizeof(Tracker_Ptr) * g_GLB_stPara.nPipeMaxNum);//多目标跟踪器模块

    // 初始化AI跟踪结果接收
    memset(&g_GLB_AITrackerInfo,0,sizeof(AIT_OUTPUT));


    // 
    g_pGrdTracker = new Tracker(nWidth,nHeight,GLB_SCEN_GROUND,g_GLB_TKPara_Template);
    g_pSkyTracker = new Tracker(nWidth,nHeight,GLB_SCEN_SKY,g_GLB_TKPara_Template);

    m_SceneType = GLB_SCEN_MODE::GLB_SCEN_SKY;          // 默认全场景的类型为对空

    //g_pSkyTracker = NULL;

}

/**********************************************************
 * 函数名称：GLB_InitOutputs()
 * 功能描述：系统初始化-算法输出结果
 * 输入参数：无
 * 输出参数：无
 * 返 回 值：无
 * 调用关系：无
 * 其它说明：无
 **********************************************************/
void Arith_EOController::GLB_InitOutputs(void)
{
    // //清空目标检测结果
    g_GLB_Detectors->ClearTargetsArray();

    // // 清空管道及告警队列
    g_GLB_PipeProc->ClearAllPipesAndAlarms();

    // //清空目标跟踪结果
    GLB_Release_Trackers(g_GLB_stPara.nPipeMaxNum);

    //初始化算法结果
    memset(&g_GLB_stOutput, 0, sizeof(GLB_OUTPUT));
}


void Arith_EOController::Arith_CMD_Lock(CENTERRECT32S LockBox)
{
    // 视场内锁定
    g_GLB_stCommand.unSetLockPosX = LockBox.cx;
    g_GLB_stCommand.unSetLockPosY = LockBox.cy;
    g_GLB_stCommand.unSetLockBoxW = LockBox.w;
    g_GLB_stCommand.unSetLockBoxH = LockBox.h;

    // 发送点选或者框选
    if (LockBox.w == 0 || LockBox.h == 0)
    {
        g_GLB_stCommand.ubCmd = LockMode::LOCK_POINT;

    }
    else
    {
        g_GLB_stCommand.ubCmd = LockMode::LOCK_RECT;
    }
}

PIPE* Arith_EOController::Arith_CMD_LockIDAndInit(GD_VIDEO_FRAME_S img, SINT32 nID)
{
    // 遍历当前告警队列，查找满足ID的目标
    PIPE* pLockPipe = g_GLB_PipeProc->getPipeByAlarmID(nID);

    if (pLockPipe == NULL)
    {
        return pLockPipe;
    }

    // 模拟点选跟踪
    g_GLB_stPara.stLockCtrl = LockMode::LOCK_POINT;
    g_GLB_stPara.ubLockID = -1;

    //实际锁定点坐标（实际锁定点X坐标、实际锁定点Y坐标）
    g_GLB_stPara.ptLockPos.x = pLockPipe->ptStopPnt.x;
    g_GLB_stPara.ptLockPos.y = pLockPipe->ptStopPnt.y;

    //锁定波门尺寸（宽度、高度）
    g_GLB_stPara.snLockBoxSize.w = 0;
    g_GLB_stPara.snLockBoxSize.h = 0;
    g_GLB_stPara.snLockBoxSize.s = 0;

    if (g_GLB_stPara.nStatus == GLB_STATUS_SEARCH)
    {
        Proc_SearchLock(img, g_GLB_stInput, g_GLB_stPara);
    }
    else if(g_GLB_stPara.nStatus == GLB_STATUS_TRACK)
    {
        // 等同于跟踪时点选，需要换批号
        Proc_SOTLock_NoRemainID(img, g_GLB_stInput, g_GLB_stPara);
    }

    return pLockPipe;

}



PIPE* Arith_EOController::Arith_CMD_LockAndInit(GD_VIDEO_FRAME_S img, CENTERRECT32S LockBox)
{
    // 视场内锁定
    if (LockBox.w == 0 || LockBox.h == 0)
    {
        g_GLB_stPara.stLockCtrl = (LockMode::LOCK_POINT);
    }
    else
    {
        g_GLB_stPara.stLockCtrl = (LockMode::LOCK_RECT);
    }

    g_GLB_stPara.ubLockID = -1;

    //实际锁定点坐标（实际锁定点X坐标、实际锁定点Y坐标）
    g_GLB_stPara.ptLockPos.x = LockBox.cx;
    g_GLB_stPara.ptLockPos.y = LockBox.cy;
    //锁定波门尺寸（宽度、高度）
    g_GLB_stPara.snLockBoxSize.w = LockBox.w;
    g_GLB_stPara.snLockBoxSize.h = LockBox.h;
    g_GLB_stPara.snLockBoxSize.s = LockBox.w * LockBox.h;


    if (g_GLB_stPara.nStatus == GLB_STATUS_SEARCH)
    {
        Proc_SearchLock(img, g_GLB_stInput, g_GLB_stPara);
    }
    else if(g_GLB_stPara.nStatus == GLB_STATUS_TRACK)
    {
        // 跟踪时框选，更接近重新锁定目标意图，用老批号
        if(g_GLB_stPara.stLockCtrl == LockMode::LOCK_RECT)
        {
            Proc_SOTLock(img, g_GLB_stInput, g_GLB_stPara);
        }
        // 跟踪时点选，接近新目标锁定，用新批号
        else if(g_GLB_stPara.stLockCtrl == LockMode::LOCK_POINT)
        {
            Proc_SOTLock_NoRemainID(img, g_GLB_stInput, g_GLB_stPara);
        }
    }

    // 
    PIPE* pLockPipe = NULL;
    if (g_GLB_stPara.nLockPipeInd > -1)
    {
        pLockPipe = &g_GLB_PipeProc->getPipeArray()[g_GLB_stPara.nLockPipeInd];
    }

    return pLockPipe;

}

void Arith_EOController::Arith_CMD_AdjustSOTRect(int dx, int dy, int dw, int dh)
{
    // 在单目标跟踪流程下，重新下发框选跟踪即可实现

    // 仅SOT模式下有效
    if (g_GLB_stPara.nStatus != GLB_STATUS_TRACK)
    {
        return;
    }

    // 获取当前跟踪位置
    if (g_GLB_stPara.nLockPipeInd > -1)
    {
        PIPE* pLockPipe = &g_GLB_PipeProc->getPipeArray()[g_GLB_stPara.nLockPipeInd];
        TARGET_OBJECT* pTarget = &pLockPipe->objHistoryList[pLockPipe->ubEnd];

        // 发送框选
        g_GLB_stCommand.ubCmd = LockMode::LOCK_RECT;
        g_GLB_stCommand.unSetLockPosX = pLockPipe->ptCurrentPnt.x + dx;
        g_GLB_stCommand.unSetLockPosY = pLockPipe->ptCurrentPnt.y + dy;
        g_GLB_stCommand.unSetLockBoxW = pTarget->snSize.w + dw;
        g_GLB_stCommand.unSetLockBoxH = pTarget->snSize.h + dh;
    }
    else
    {
        LOG_ERROR("NO TRACKING PIPE!");
    }

}


SINT32 Arith_EOController::Arith_CMD_LockMultiTarget(TargetGuide* target, int num)
{
    memcpy(g_GLB_stCommand.target, target, num * sizeof(TargetGuide));
    g_GLB_stPara.bEnRadarGuide = true;
    g_GLB_stCommand.bGuideUpDate = true;
    return num;
}

void Arith_EOController::Arith_CMD_CancleLock()
{
    g_GLB_stCommand.ubCmd = LockMode::LOCK_UNLOCK;
}

void Arith_EOController::Arith_CMD_SetScenMode(GLB_SCEN_MODE nScenMode)
{
    g_GLB_stCommand.ubScenCmd = nScenMode;
}

void Arith_EOController::Arith_CMD_SetSysMode(GLB_SYS_MODE nSysMode)
{
    g_GLB_stCommand.ubSysStatusCmd = nSysMode;
}



void Arith_EOController::Arith_SetRunTimeParam(ARIDLL_PARMA config)
{
    //1.获取当前参数，2.仅修改给定部分参数
    // 设置检测器实例参数
    Param_SkyDetect para = g_GLB_Detectors->GetParam();
    para.bEnableDetcetSmallTarget = config.PrmSkyDetect.bEnableDetcetSmallTarget;
    para.bEnableDetcetAreaTarget = config.PrmSkyDetect.bEnableDetcetAreaTarget;
    para.bEnableDetcetDimTarget = config.PrmSkyDetect.bEnableDetcetDimTarget;
    para.fSmallDetectGDK = config.PrmSkyDetect.fSmallDetectGDK;
    para.fAreaDetectGradDiffThre = config.PrmSkyDetect.fAreaDetectGradDiffThre;
    para.nGrayThresMinBright = config.PrmSkyDetect.nGrayThresMinBright;
    para.nGrayThresMinDark = config.PrmSkyDetect.nGrayThresMinDark;
    para.nDetectGrayType = config.PrmSkyDetect.nDetectGrayType;
    para.nDSmpScale = config.PrmSkyDetect.nDSmpScale;
    g_GLB_Detectors->SetParam(para);//设置参数

    // 全局检测器参数共享给局部检测器参数
    //memcpy(&config.stSkyParam.prmTSkyDet, &config.PrmSkyDetect, sizeof(Param_SkyDetect));
    

    // 设置管道实例参数
    PIPE_PARAMETERS pipePara = g_GLB_PipeProc->GetParam();
    pipePara.nPipeRadiusSearch= config.nPipeRadiusSearch;  //管道搜索范围直径 -- 搜索
    pipePara.nPipeRadiusLock =  config.nPipeRadiusLock;    //管道搜索范围直径 -- 锁定
    pipePara.nPipeRadiusTrack=  config.nPipeRadiusTrack;   //管道搜索范围直径 -- 跟踪
    pipePara.nPipeRadiusLost =  config.nPipeRadiusLost;    //管道搜索范围直径 -- 跟踪丢失
    pipePara.nAwsFrmNumScan  =  config.nAwsFrmNumScan;     //管道目标的确认帧数- 扫描模式 4
    pipePara.nAwsFrmNumStare =  config.nAwsFrmNumStare;    //管道目标的确认帧数- 凝视模式 4
    pipePara.nPipeDelMinScan =  config.nPipeDelMinScan;    //管道最少延迟删除帧数- 扫描模式 4
    pipePara.nPipeDelMinStare=  config.nPipeDelMinStare;   //管道最少延迟删除帧数- 凝视/跟踪模式 4
    pipePara.bCloseBadPointPipe = config.bCloseBadPointPipe;//关闭坏点管道
    pipePara.fFilterA = config.fA;
    pipePara.fFilterB = config.fB;

    g_GLB_PipeProc->SetParam(pipePara);

    g_GLB_stPara.snLockBoxSize.w = config.nLockPointW <= 0? g_GLB_stPara.snLockBoxSize.w: config.nLockPointW;
    g_GLB_stPara.snLockBoxSize.h = config.nLockPointH <= 0? g_GLB_stPara.snLockBoxSize.h: config.nLockPointH;

    //LOG_DEBUG("{0}, {1}, {2}", __FILE__, __FUNCTION__, __LINE__);
    // 设置跟踪器参数模板
    Param_SkyTracker* pSkyParam = &g_GLB_TKPara_Template.skyParam;//对空
    Param_GroundTracker* pGrdParam = &g_GLB_TKPara_Template.grdParam;//对地

    memcpy(pGrdParam, &config.stGrdParam,sizeof(Param_GroundTracker));
    memcpy(pSkyParam, &config.stSkyParam,sizeof(Param_SkyTracker));

    //LOG_DEBUG("{0}, {1}, {2}", __FILE__, __FUNCTION__, __LINE__);
    //LOG_DEBUG("{0}, {1}, {2}, {3}", __FILE__, __FUNCTION__, __LINE__, config.nLogLevel);

    memcpy(&g_GLB_stArithPara.stGrdParam, &config.stGrdParam, sizeof(Param_GroundTracker));
    memcpy(&g_GLB_stArithPara.stSkyParam, &config.stSkyParam, sizeof(Param_SkyTracker));
    // 非文件设置状态，设置日志输出级别
    if (false == LOG_GETFILESTATE())
    {
       LOG_SETLEVEL(config.nLogLevel);
       LOG_SETLEVEL_IN(config.nLogLevel);
       LOG_SETLEVEL_OUT(config.nLogLevel);  
    }

    g_pGrdTracker->pGroundTracker->SetParam(g_GLB_TKPara_Template.grdParam);
}

// 读取运行时跟踪参数
void Arith_EOController::Arith_GetRunTimeParam(ARIDLL_PARMA* config)
{
    Param_SkyDetect para = g_GLB_Detectors->GetParam();
    config->PrmSkyDetect.bEnableDetcetAreaTarget = para.bEnableDetcetAreaTarget;
    config->PrmSkyDetect.bEnableDetcetDimTarget = para.bEnableDetcetDimTarget;
    config->PrmSkyDetect.bEnableDetcetSmallTarget = para.bEnableDetcetSmallTarget;
    config->PrmSkyDetect.fSmallDetectGDK = para.fSmallDetectGDK;
    config->PrmSkyDetect.fAreaDetectGradDiffThre = para.fAreaDetectGradDiffThre;
    config->PrmSkyDetect.nGrayThresMinBright = para.nGrayThresMinBright;
    config->PrmSkyDetect.nGrayThresMinDark = para.nGrayThresMinDark;
    config->PrmSkyDetect.nDetectGrayType = para.nDetectGrayType;
    config->PrmSkyDetect.nDSmpScale = para.nDSmpScale;

    PIPE_PARAMETERS pipePara = g_GLB_PipeProc->GetParam();
    config->nPipeRadiusSearch = pipePara.nPipeRadiusSearch;
    config->nPipeRadiusLock = pipePara.nPipeRadiusLock;
    config->nPipeRadiusTrack = pipePara.nPipeRadiusTrack;
    config->nPipeRadiusLost = pipePara.nPipeRadiusLost;
    config->nAwsFrmNumScan = pipePara.nAwsFrmNumScan;
    config->nAwsFrmNumStare = pipePara.nAwsFrmNumStare;
    config->nPipeDelMinScan = pipePara.nPipeDelMinScan;
    config->nPipeDelMinStare = pipePara.nPipeDelMinStare;

    // 返回当前跟踪参数
    config->stGrdParam = g_GLB_TKPara_Template.grdParam;
    config->stSkyParam = g_GLB_TKPara_Template.skyParam;

}


/*************************************
* Method:                getStructFromFS()
* Function Description:  从cv文件流解析json字段进行参数赋值
* CreateData:            2024/10/30
* Input Param:           cv::FileStorage fs
* Output Param:
* Return:                ARIDLL_PARMA
* Call Relation:
* Other Description:win平台无对应参数默认返回为0，linux平台无对应参数对导致崩溃，需要在调用时加参数存在性检查
*************************************/
ARIDLL_PARMA getStructFromFS(cv::FileStorage fs)
{
    ARIDLL_PARMA config = { 0 };
    //LOG_DEBUG("{0}, {1}, {2}", __FILE__, __FUNCTION__, __LINE__);
   
    assert(!fs["bEnableDetcetAreaTarget"].empty()); fs["bEnableDetcetAreaTarget"]       >> config.PrmSkyDetect.bEnableDetcetAreaTarget;
    assert(!fs["bEnableDetcetSmallTarget"].empty());fs["bEnableDetcetSmallTarget"]      >> config.PrmSkyDetect.bEnableDetcetSmallTarget;
    assert(!fs["bEnableDetcetDimTarget"].empty());  fs["bEnableDetcetDimTarget"]        >> config.PrmSkyDetect.bEnableDetcetDimTarget;
    assert(!fs["fSmallDetectGDK"].empty());         fs["fSmallDetectGDK"]               >> config.PrmSkyDetect.fSmallDetectGDK;
    assert(!fs["fAreaDetectGradDiffThre"].empty()); fs["fAreaDetectGradDiffThre"]       >> config.PrmSkyDetect.fAreaDetectGradDiffThre;
    assert(!fs["nGrayThresMinBright"].empty());     fs["nGrayThresMinBright"]           >> config.PrmSkyDetect.nGrayThresMinBright;
    assert(!fs["nGrayThresMinDark"].empty());       fs["nGrayThresMinDark"]             >> config.PrmSkyDetect.nGrayThresMinDark;
    assert(!fs["nDetectGrayType"].empty());         fs["nDetectGrayType"]               >> config.PrmSkyDetect.nDetectGrayType;
    assert(!fs["nDSmpScale"].empty());              fs["nDSmpScale"]                    >> config.PrmSkyDetect.nDSmpScale;


    // 跟踪过程的局部检测控制参数
    fs["bEnableDetcetAreaTarget"]       >> config.stSkyParam.prmTSkyDet.bEnableDetcetAreaTarget;
    fs["bEnableDetcetSmallTarget"]      >> config.stSkyParam.prmTSkyDet.bEnableDetcetSmallTarget;
    fs["bEnableDetcetDimTarget"]        >> config.stSkyParam.prmTSkyDet.bEnableDetcetDimTarget;
    fs["fSmallDetectGDK"]               >> config.stSkyParam.prmTSkyDet.fSmallDetectGDK;
    fs["fAreaDetectGradDiffThre"]       >> config.stSkyParam.prmTSkyDet.fAreaDetectGradDiffThre;
    fs["nGrayThresMinBright"]           >> config.stSkyParam.prmTSkyDet.nGrayThresMinBright;
    fs["nGrayThresMinDark"]             >> config.stSkyParam.prmTSkyDet.nGrayThresMinDark;
    fs["nDetectGrayType"]               >> config.stSkyParam.prmTSkyDet.nDetectGrayType;
    fs["nDSmpScale"]                    >> config.stSkyParam.prmTSkyDet.nDSmpScale;


    assert(!fs["Sky_bEnableFullImgDet"].empty());   fs["Sky_bEnableFullImgDet"]         >> config.stSkyParam.Sky_bEnableFullImgDet;
    assert(!fs["Sky_bEnableTrackSA"].empty());      fs["Sky_bEnableTrackSA"]            >> config.stSkyParam.Sky_bEnableTrackSA;
    assert(!fs["Sky_bEnableKCF"].empty());          fs["Sky_bEnableKCF"]                >> config.stSkyParam.Sky_bEnableKCF;
    assert(!fs["Sky_bEnableMatcher"].empty());      fs["Sky_bEnableMatcher"]            >> config.stSkyParam.Sky_bEnableMatcher;
    assert(!fs["Sky_bSelectObjManual"].empty());    fs["Sky_bSelectObjManual"]          >> config.stSkyParam.Sky_bSelectObjManual;
    assert(!fs["Sky_bUseAIDet"].empty());           fs["Sky_bUseAIDet"]                 >> config.stSkyParam.Sky_bUseAIDet;
    assert(!fs["Sky_nTrackMemFrmNum"].empty());     fs["Sky_nTrackMemFrmNum"]           >> config.stSkyParam.Sky_nTrackMemFrmNum;
    assert(!fs["nSmallObjSizeMax"].empty());        fs["nSmallObjSizeMax"]              >> config.stSkyParam.nSmallObjSizeMax;


    assert(!fs["nPipeRadiusSearch"].empty());       fs["nPipeRadiusSearch"]             >> config.nPipeRadiusSearch;
    assert(!fs["nPipeRadiusLock"].empty());         fs["nPipeRadiusLock" ]              >> config.nPipeRadiusLock;
    assert(!fs["nPipeRadiusTrack"].empty());        fs["nPipeRadiusTrack"]              >>config.nPipeRadiusTrack;
    assert(!fs["nPipeRadiusLost"].empty());         fs["nPipeRadiusLost" ]              >>config.nPipeRadiusLost;
    assert(!fs["nAwsFrmNumScan"].empty());          fs["nAwsFrmNumScan"]                >>config.nAwsFrmNumScan;
    assert(!fs["nAwsFrmNumStare"].empty());         fs["nAwsFrmNumStare"]               >>config.nAwsFrmNumStare;
    assert(!fs["nPipeDelMinScan"].empty());         fs["nPipeDelMinScan"]               >>config.nPipeDelMinScan;
    assert(!fs["nPipeDelMinStare"].empty());        fs["nPipeDelMinStare"]              >>config.nPipeDelMinStare;
    assert(!fs["ABF_a"].empty());                   fs["ABF_a"]                         >>config.fA;
    assert(!fs["ABF_b"].empty());                   fs["ABF_b"]                         >>config.fB;
    assert(!fs["nLockPointW"].empty());             fs["nLockPointW"]                   >>config.nLockPointW;
    assert(!fs["nLockPointH"].empty());             fs["nLockPointH"]                   >>config.nLockPointH;
    assert(!fs["bCloseBadPointPipe"].empty());      fs["bCloseBadPointPipe"]            >>config.bCloseBadPointPipe;


    assert(!fs["Grd_bEnableAccuracyTrack"].empty());fs["Grd_bEnableAccuracyTrack"]      >> config.stGrdParam.bEnableAccuracyTrack;
    assert(!fs["Grd_bUseServePredict"].empty());    fs["Grd_bUseServePredict"]          >> config.stGrdParam.bUseServePredict;
    assert(!fs["Grd_bEnableLKCorrect"].empty());    fs["Grd_bEnableLKCorrect"]          >> config.stGrdParam.bEnableLKCorrect;
    assert(!fs["Grd_bEnableKCF"].empty());          fs["Grd_bEnableKCF"]                >> config.stGrdParam.bEnableKCF;
    assert(!fs["Grd_nKcfUpdataStep"].empty());      fs["Grd_nKcfUpdataStep"]            >> config.stGrdParam.nKcfUpdataStep;
    assert(!fs["Grd_bKCFMutiScales"].empty());      fs["Grd_bKCFMutiScales"]            >> config.stGrdParam.bKCFMutiScales;
    assert(!fs["Grd_fArrestKCFMinThre"].empty());   fs["Grd_fArrestKCFMinThre"]         >> config.stGrdParam.fArrestKCFMinThre;
    assert(!fs["Grd_fArrestKCFMaxThre"].empty());   fs["Grd_fArrestKCFMaxThre"]         >> config.stGrdParam.fArrestKCFMaxThre;
    assert(!fs["Grd_fKCFResthre"].empty());         fs["Grd_fKCFResthre"]               >> config.stGrdParam.fKCFResthre;
    assert(!fs["Grd_bTLDOccJudge"].empty());        fs["Grd_bTLDOccJudge"]              >> config.stGrdParam.bTLDOccJudge;
    assert(!fs["Grd_bEnableDaSiamRPN"].empty());    fs["Grd_bEnableDaSiamRPN"]          >> config.stGrdParam.bEnableDaSiamRPN;
    assert(!fs["Grd_bEnableAIOccJudge"].empty());   fs["Grd_bEnableAIOccJudge"]         >> config.stGrdParam.bEnableAIOccJudge;
    assert(!fs["Grd_fAIOccThre"].empty());   fs["Grd_fAIOccThre"]         >> config.stGrdParam.fAIOccThre;
    assert(!fs["Grd_bEnableAIDetect"].empty());   fs["Grd_bEnableAIDetect"]         >> config.stGrdParam.bEnableAIDetect;


    assert(!fs["Grd_bEnableArrestCorr"].empty());   fs["Grd_bEnableArrestCorr"]         >> config.stGrdParam.bEnableArrestCorr;
    assert(!fs["Grd_bEnableArrestAngle"].empty());  fs["Grd_bEnableArrestAngle"]        >> config.stGrdParam.bEnableArrestAngle;
    assert(!fs["Grd_bEnableAreestEsay"].empty());   fs["Grd_bEnableAreestEsay"]         >> config.stGrdParam.bEnableAreestEsay;
    assert(!fs["Grd_nArrestEsayCnt"].empty());      fs["Grd_nArrestEsayCnt"]            >> config.stGrdParam.nArrestEsayCnt;
    assert(!fs["EnableLogLevel"].empty());          fs["EnableLogLevel"]                >> config.nLogLevel;
    return config;
}



void Arith_EOController::Arith_ReadParaFile(const char* path)
{
    cv::FileStorage fs(path, cv::FileStorage::READ);

    if (!fs.isOpened())
    {
        return;
    }
    ARIDLL_PARMA config = getStructFromFS(fs);
    Arith_SetRunTimeParam(config);
}

void Arith_EOController::Arith_ExportParaFile(const char* path)
{
    cv::FileStorage fs(path, cv::FileStorage::WRITE);
    fs << "bEnableDetcetSmallTarget" << true;
    fs << "bEnableDetcetAreaTarget" << true;
    fs << "bEnableDetcetDimTarget" << false;
    fs << "fSmallDetectGDK" << 5.0f;
    fs << "fAreaDetectGradDiffThre" << 40;
    fs << "nGrayThresMinBright" << 20;
    fs << "nGrayThresMinDark" << 10;
    fs << "nDetectGrayType" << 1;
    fs << "nDSmpScale" << 4;
    fs << "Sky_bEnableFullImgDet" << false;     // 对空跟踪过程中是否开启全图小面
    fs << "Sky_bEnableTrackSA" << true;
    fs << "Sky_bEnableKCF" << true;
    fs << "Sky_bEnableMatcher" << false;
    fs << "Sky_bSelectObjManual" << true;
    fs << "Sky_bUseAIDet" << true;
    fs << "Sky_nTrackMemFrmNum" << 1000;
    fs << "nSmallObjSizeMax" << 36;


    fs << "nPipeRadiusSearch" << 40;//管道搜索范围直径 -- 搜索
    fs << "nPipeRadiusLock" << 80;//管道搜索范围直径 -- 锁定
    fs << "nPipeRadiusTrack" << 30;   //管道搜索范围直径 -- 跟踪
    fs << "nPipeRadiusLost" << 100;//管道搜索范围直径 -- 跟踪丢失
    fs << "nAwsFrmNumScan" << 3;//管道目标的确认帧数- 扫描模式 4
    fs << "nAwsFrmNumStare" << 6;//管道目标的确认帧数- 凝视模式 4
    fs << "nPipeDelMinScan" << 3;//管道最少延迟删除帧数- 扫描模式 4
    fs << "nPipeDelMinStare" << 3;//管道最少延迟删除帧数- 凝视/跟踪模式 4
    fs << "ABF_a" << 0.2f;// 轨迹滤波器AB参数
    fs << "ABF_b" << 0.04f;// 轨迹滤波器AB参数
    fs << "nLockPointW" << 40;    //点选锁定目标宽度
    fs << "nLockPointH" << 40;    //点选锁定目标高度
    fs << "bCloseBadPointPipe" << false; //关闭坏点管道

    fs << "Grd_bEnableAccuracyTrack" << true;
    fs << "Grd_bUseServePredict" << false;
    fs << "Grd_bEnableLKCorrect" << true;
    fs << "Grd_bEnableKCF" << true;
    fs << "Grd_nKcfUpdataStep" << 2;
    fs << "Grd_bKCFMutiScales" << 1;
    fs << "Grd_fArrestKCFMinThre" << 0.38;
    fs << "Grd_fArrestKCFMaxThre" << 0.68;
    fs << "Grd_fKCFResthre" << 0.2;
    fs << "Grd_bTLDOccJudge" << true;
    fs << "Grd_bEnableDaSiamRPN" << false;
    fs << "Grd_bEnableAIOccJudge" << true;
    fs << "Grd_fAIOccThre" << 0.85;
    fs << "Grd_bEnableAIDetect" << true;
    fs << "Grd_bEnableArrestCorr" << false;
    fs << "Grd_bEnableArrestAngle" << false;
    fs << "Grd_bEnableAreestEsay" << false;
    fs << "Grd_nArrestEsayCnt" << 250;
    fs << "EnableLogLevel" << 2; // 6/off

    fs.release();
}

void Arith_EOController::Arith_ReadJsonStream(const char* str)
{
    cv::FileStorage fs(str, cv::FileStorage::MEMORY);

    ARIDLL_PARMA config = getStructFromFS(fs);

    Arith_SetRunTimeParam(config);
}


SINT32  Arith_EOController::Arith_ExportDebugJson(char* str, SINT32 size, GLB_SCEN_MODE nWorkScene)
{
    memset(str, 0, size);
    std::string strTmp;
    cv::FileStorage fs(strTmp.data(), cv::FileStorage::FORMAT_JSON | cv::FileStorage::WRITE | cv::FileStorage::MEMORY);

    // 写入数据到 FileStorage
    //fs << "timestamp" << 1010;
    //fs << "version" << "V1.00";
    //fs << "XYZ" << 1;

    if (GLB_SCEN_MODE::GLB_SCEN_SKY == nWorkScene)
    {
        fs << "DownRatio" << g_GLB_stOutput.sky_TrkDownRatio;
        fs << "TrkType" << g_GLB_stOutput.sky_nObjTypeSrc;
        fs << "TrkMem" << g_GLB_stOutput.sky_TrkMemFrm;
        fs << "W" << g_GLB_stOutput.sky_nTrkW;
        fs << "H" << g_GLB_stOutput.sky_nTrkH;
        fs << "PxlsCnt" << g_GLB_stOutput.sky_nTrkPxlsCnt;
        fs << "ClsType" << g_GLB_stOutput.sky_nClsType;
        fs << "ClsSrc" << g_GLB_stOutput.sky_nClassSource;
        //fs << "Conf" << std::to_string(g_GLB_stOutput.sky_fTrkConf);
    }
    else if (GLB_SCEN_MODE::GLB_SCEN_GROUND == nWorkScene)
    {
        fs << "fKCFRes" << std::to_string(g_GLB_stOutput.fKCFRes);
        fs << "nLearnCnt" << g_GLB_stOutput.nLearnCnt;
        fs << "nTLDNum" << g_GLB_stOutput.nTLDNum;
        fs << "fMaxNNConf" << std::to_string(g_GLB_stOutput.fMaxNNConf);
        fs << "nAIJamCnt" << g_GLB_stOutput.nAIJamCnt;
        fs << "nAIChangCnt" << g_GLB_stOutput.nAIChangCnt;
        fs << "fLargeResTH" << std::to_string(g_GLB_stOutput.fLargeResTH);
        fs << "fArrKCFRes" << std::to_string(g_GLB_stOutput.fArrKCFRes);
    }
    else if (GLB_SCEN_MODE::GLB_SCEN_SEA == nWorkScene)
    {

    }

    memcpy(str, fs.releaseAndGetString().c_str(), size);

    return 0;
}


void Arith_EOController::Arith_SOT_RunTLDTracker(GD_VIDEO_FRAME_S img)
{
    // 管道队列
    PIPE* pPipeArray = g_GLB_PipeProc->getPipeArray();
    SINT32 nPipeArrayLen = g_GLB_stPara.nPipeMaxNum;
    if (g_GLB_stPara.nLockPipeInd == -1)
    {
        return;
    }
    
    // 寻找被锁定的单目标管道
    PIPE* pTrackPipe = &pPipeArray[g_GLB_stPara.nLockPipeInd];
    Tracker_Ptr sotracker = g_GLB_Trackers[g_GLB_stPara.nLockPipeInd];

    if (sotracker == NULL)
    {
        return;
    }

    API_TLD* pTLDMod = sotracker->GetpTLDTracker();

    // TLD 初始化和解锁在框架内部完成，这里只跑最耗时的检测和学习过程。
    if (pTLDMod != NULL)
    {
        // 执行TLD流程
        pTLDMod->TLD_Run(img, &sotracker->ObjStatus,TrackEvent());
    }
    

    return;
}

OBJECTSTATUS Arith_EOController::Arith_SOT_GetTrackerStatus()
{
    OBJECTSTATUS obj = {0};
    // 对空和对地模式分别返回
    if (GLB_SCEN_SKY == m_SceneType)
    {
        return g_pSkyTracker->pSkyTracker->ObjStatus;
    }

    if (GLB_SCEN_GROUND == m_SceneType)
    {
        return g_pGrdTracker->pGroundTracker->ObjStatus;
    }

    return obj;
    
}



void  Arith_CalcTargetSysInfo(TARGET_OBJECT* pTargetObj, SINT32 nFrmNum, GLB_INPUT g_Input)
{
    // 补全目标信息，如帧编号、角度、检测时间等.
    for (int i = 0; i < nFrmNum; i++)
    {
        TARGET_OBJECT* tTarget = &pTargetObj[i];
        // 帧编号
        tTarget->unFrmID = g_Input.unFrmId;

        //// 转惯性系
        Pole polenue = getStablePoleFromImagePos(tTarget->pfCenPos, g_Input.stCamera, g_Input.servoInfo, g_Input.afPlatformRPY, g_Input.setupErr);
        // 目标角度赋值为大地坐标系
        tTarget->afAngle.fAz = DEGLIM360((FLOAT32)polenue.beta);
        tTarget->afAngle.fPt = (FLOAT32)polenue.alpha;      

        // 下视场景目标3d坐标计算，非下视返回空
        tTarget->pos3d = getXYZFromPole_withHeight(polenue,g_Input.nElevationDiff);
    }
}



void Arith_EOController::GLB_Release_Trackers(SINT32 nPipeNum)
{

    // 单目标跟踪时，由于使用了
    if (GLB_STATUS_TRACK == g_GLB_stPara.nStatus)
    {
        if (g_pSkyTracker && g_pSkyTracker->m_LockingPipe)
        {
            g_pSkyTracker->Cancle();
        }
        if (g_pGrdTracker && g_pGrdTracker->m_LockingPipe)
        {
            g_pGrdTracker->Cancle();
        }
        /*if (g_pSkyTracker)
        {
            g_pSkyTracker->Cancle();
        }
        if (g_pGrdTracker)
        {
            g_pGrdTracker->Cancle();
        }*/

		PIPE* pTrackPipe = &g_GLB_PipeProc->getPipeArray()[g_GLB_stPara.nLockPipeInd];
		g_GLB_PipeProc->DelPipe(pTrackPipe);
    }

    if (GLB_STATUS_MOTRACK == g_GLB_stPara.nStatus)
    {
        // 依次解锁
        for (size_t i = 0; i < nPipeNum; i++)
        {
            if (g_GLB_Trackers[i] != NULL)
            {
                g_GLB_Trackers[i]->Cancle();
                delete g_GLB_Trackers[i];
                g_GLB_Trackers[i] = NULL;

                // 删除跟踪管道
                PIPE* pTrackPipe = &g_GLB_PipeProc->getPipeArray()[i];
                g_GLB_PipeProc->DelPipe(pTrackPipe);

            }
        }
    }    
}