S3315_RK3588/NeoTracker/src/Arith_SkyTracker.cpp

#include "Arith_SkyTracker.h"
#include "PIPE/Arith_PIPE_utils.h"
#include "Arith_CoordModule.h"
#include "Arith_Bbox.h"
#include "Arith_timer.h"
#include <string>

SkyTracker::SkyTracker(SINT32 nWidth, SINT32 nHeight)
{
    pSATracker = new SA_Tracker(nWidth, nHeight);
    pKCFTracker = API_KCF_Tracker::Create(96, 128);  // 参数控制模板输入大小 (56, 64) (96, 128) (120, 128)
    pBkgMonitor = new BkgMonitor();                 // 跟踪目标背景监控
    pRadarInfo = new RadarInfo();                   // 雷达信息监控
    Ai_DMonitor = new AID_Monitor();                // AI Detect 管道监控模块，用于筛选AI检测的重捕候选

    mTargetFusion = { 0 };
    ObjStatus = { 0 };
    m_LockingPipe = nullptr;
    g_GLB_nTrackDecisionCnt = 0;
    m_bSetMemoryTrack = false;
    skyControlInfo.m_bMemoryTrackOcc = false;
    skyControlInfo.nTrackMemFrmNum = GLB_MEMORY_FRM_NUM;

    mTrakingPara.Sky_bEnableKCF = false;
    mTrakingPara.Sky_bEnableFullImgDet = false;
    mTrakingPara.Sky_bEnableTrackSA = false;
    mTrakingPara.nSmallObjSizeMax = 32;

    SkyTrkStatusInfo = { 0 };

    bsvmInitDone = FALSE;
    pipeClassifyInfo = new PipeMainClassifyInfo;
    memset(pipeClassifyInfo, 0, sizeof(PipeMainClassifyInfo));

    // svm 模型加载
    char svmModelName[256];
    std::string basePath = GetDynamicLibraryPath();
    snprintf(svmModelName, 256, "%s/%s", basePath.c_str(), "scaled_fly_bird_train.model");
    //std::string model_name = std::string(SOURCE_PATH) + "/NeoTracker/src/SVM/models/scaled_fly_bird_train.model";
    if ((SVM_model = svm_load_model(svmModelName)) == 0)
    {
        bsvmInitDone = FALSE;
        svm_nodeX = NULL;
    }
    else
    {
        svm_nodeX = (struct svm_node*)malloc((MAIN_ALARM_DEPTH_MAX + 1) * sizeof(struct svm_node));
        bsvmInitDone = TRUE;
    }

    m_TSky_Output.m_nTargetNum = 0;
    m_TSky_Output.mTarget_Array = new TARGET_OBJECT[DT_TARGET_MAX_NUM];

    // 模板匹配初始化
    m_TSky_Output.m_nMatcherNum = 0;
    m_TSky_Output.mMatcher_Array = new MATCHER_TARGET[DT_MATCHER_MAX_NUM];
    Matcher_Create(&matcher);

    SkyTrkState = LockStateUnknown;
}

SkyTracker::SkyTracker(SINT32 nWidth, SINT32 nHeight, Param_SkyTracker para)
{
    pSATracker = new SA_Tracker(nWidth, nHeight);
    pKCFTracker = API_KCF_Tracker::Create(96, 128); // 参数控制模板输入大小(56,64) (96,128) (120, 128)
    pBkgMonitor = new BkgMonitor();                 // 跟踪目标背景监控
    pRadarInfo = new RadarInfo();                   // 雷达信息监控
    Ai_DMonitor = new AID_Monitor();                // AI Detect 管道监控模块，用于筛选AI检测的重捕候选

    mTargetFusion = { 0 };
    ObjStatus = { 0 };
    m_LockingPipe = nullptr;
    g_GLB_nTrackDecisionCnt = 0;
    m_bSetMemoryTrack = false;
    skyControlInfo.m_bMemoryTrackOcc = false;
    skyControlInfo.nTrackMemFrmNum = GLB_MEMORY_FRM_NUM;

    // 读取跟踪参数配置
    mTrakingPara = para;

    // 从检测器同步参数到跟踪器
    pSATracker->pDST_Module->setDstParm(&para.prmTSkyDet);
    //pSATracker->pDAT_Module->setDatParm(&para.prmTSkyDet);

    bsvmInitDone = FALSE;

    SkyTrkStatusInfo = { 0 };

    pipeClassifyInfo = new PipeMainClassifyInfo;
    memset(pipeClassifyInfo, 0, sizeof(PipeMainClassifyInfo));

    // svm 模型加载
    char svmModelName[256];
    std::string basePath = GetDynamicLibraryPath();
    snprintf(svmModelName, 256, "%s/%s", basePath.c_str(), "scaled_fly_bird_train.model");
    //std::string model_name = PATH + "/NeoTracker/src/SVM/models/scaled_fly_bird_train.model";;
    if ((SVM_model = svm_load_model(svmModelName)) == 0)
    {
        LOG_WARN("svm_load_model Failed");
        svm_nodeX = NULL;
        bsvmInitDone = FALSE;
    }
    else
    {
        svm_nodeX = (struct svm_node*)malloc((MAIN_ALARM_DEPTH_MAX + 1) * sizeof(struct svm_node));
        bsvmInitDone = TRUE;
    }
    m_TSky_Output.m_nTargetNum = 0;
    m_TSky_Output.mTarget_Array = new TARGET_OBJECT[DT_TARGET_MAX_NUM];

    // 模板匹配初始化
    m_TSky_Output.m_nMatcherNum = 0;
    m_TSky_Output.mMatcher_Array = new MATCHER_TARGET[DT_MATCHER_MAX_NUM];
    Matcher_Create(&matcher);

    SkyTrkState = LockStateUnknown;
}

SkyTracker::~SkyTracker()
{
    if (pSATracker)
    {
        delete pSATracker;
        pSATracker = NULL;
    }
    if (pKCFTracker)
    {
        API_KCF_Tracker::Destroy(pKCFTracker);
        pKCFTracker = NULL;
    }

    if (pBkgMonitor)
    {
        delete pBkgMonitor;
        pBkgMonitor = NULL;
    }

    if (pRadarInfo)
    {
        delete pRadarInfo;
        pRadarInfo = NULL;
    }

    if (m_TSky_Output.mTarget_Array)
    {
        delete[]m_TSky_Output.mTarget_Array;
        m_TSky_Output.mTarget_Array = NULL;
    }

    if (m_TSky_Output.mMatcher_Array)
    {
        delete[]m_TSky_Output.mMatcher_Array;
        m_TSky_Output.mMatcher_Array = NULL;
    }

    if (matcher)
    {
        if (matcher->g_Matcher_Image)
        {
            delete[]matcher->g_Matcher_Image;
            matcher->g_Matcher_Image = NULL;
        }
        delete matcher;
        matcher = NULL;
    }

    if (NULL != pipeClassifyInfo)
    {
        delete pipeClassifyInfo;
        pipeClassifyInfo = NULL;
    }

    if (Ai_DMonitor)
    {
        delete Ai_DMonitor;
        Ai_DMonitor = NULL;
    }

    if (NULL != svm_nodeX)
    {
        free(svm_nodeX);
        svm_nodeX = NULL;
    }

    if (SVM_model)
    {
        svm_free_and_destroy_model(&SVM_model);
    }
}


bool SkyTracker::Init(GD_VIDEO_FRAME_S img, PIPE* pLockPipe, GLB_INPUT* p_GLB_Input)
{
    m_LockingPipe = pLockPipe;

    // 锁定管道传指针
    if (pSATracker)
    {
        mTrakingPara.Sky_bEnableTrackSA = true;
        pSATracker->Init((UINT16*)img.u64VirAddr[0], img.u32Width, img.u32Height, pLockPipe, p_GLB_Input);
    }
    pKCFTracker->KCF_InitObjectParameters(img, pLockPipe, p_GLB_Input);//无条件初始化KCF参数，防止后续出现除0

    TARGET_OBJECT* pTrackingTarget = &pLockPipe->objHistoryList[pLockPipe->ubEnd];
    // 面目标初始化KCF
    if (pSATracker && pSATracker->m_SizeMode == SizeType::AreaTarget)
    {
        mTrakingPara.Sky_bEnableKCF = true;
        // 从管道拷贝尺寸信息到面目标匹配输出，用于控制跟踪锁定后的分割降采样倍率
        OBJECTSTATUS* m_DatTrkTarget = pSATracker->pDAT_Module->GetTargetMatched();
        memset(m_DatTrkTarget, 0, sizeof(OBJECTSTATUS));
        m_DatTrkTarget->sfSize.w = (FLOAT32)pTrackingTarget->snSize.w;
        m_DatTrkTarget->sfSize.h = (FLOAT32)pTrackingTarget->snSize.h;
        m_DatTrkTarget->sfSize.s = (FLOAT32)pTrackingTarget->unObjPxlsCnt;
    }
    else
    {
        mTrakingPara.Sky_bEnableKCF = false;
    }

    // 初始化Matcher模板匹配信息
    RECT32S stTargetRect = { 0 };
    stTargetRect.w = pTrackingTarget->snSize.w;
    stTargetRect.h = pTrackingTarget->snSize.h;
    stTargetRect.x = SINT32(pTrackingTarget->pfCenPos.x - stTargetRect.w / 2);
    stTargetRect.y = SINT32(pTrackingTarget->pfCenPos.y - stTargetRect.h / 2);
    //templateMatching::Matcher_SetTemplate(matcher, img, stTargetRect);  // 设置模板  --3315会崩溃，先注释


    m_TSky_Output.m_SizeMode = SizeType::unKnown;       // 输出目标尺寸类型为未知
    m_TSky_Output.m_nTargetNum = 0;                     // 局部目标检测结果重置为0
    m_TSky_Output.m_nMatcherNum = 0;                    // 模板匹配结果重置为0
    m_TSky_Output.findState = Finded_FAILED;            // 小面匹配状态
    m_TSky_Output.m_bComplexEnv = FALSE;                // 复杂背景标记
    m_TSky_Output.m_bInterferenceMem = FALSE;           // 过竿遮挡标记
    m_TSky_Output.unSimTargetNum = 0;                   // 相似目标个数
    m_TSky_Output.m_bSimInflunce = FALSE;               // 相似干扰标记


    // 跟踪阶段局部区域单帧目标检测结果
    memset(&m_TSky_Output.mTrakingPara_Output, 0, sizeof(Param_SkyTracker));
    memset(&m_TSky_Output.ObjectStatusTST, 0, sizeof(OBJECTSTATUS));
    memset(&m_TSky_Output.ObjectStatusCEND, 0, sizeof(OBJECTSTATUS));
    memset(&m_TSky_Output.ObjectStatusKCF, 0, sizeof(OBJECTSTATUS));
    memset(&m_TSky_Output.ObjectStatusDesc, 0, sizeof(OBJECTSTATUS));

    return true;
}

void Tracker_JammPipe(PIPE* pLockingPipe, PIPE* pPipeArray, SINT32 nMaxPipeNum)
{
	for (int i = 0; i < nMaxPipeNum; i++)
	{
		// 管道
		PIPE* pPipe = &pPipeArray[i];
		if (!pPipe->bOccupy)
		{
			continue;
		}
		//跟踪器丢失时或干扰状态不判断
		if (pLockingPipe->bLost || pLockingPipe->unSimTargetNum > 0)
		{
			continue;
		}
		FLOAT32 Dis = ABS(pPipe->ptCurrentPnt.x - pLockingPipe->ptCurrentPnt.x) + ABS(pPipe->ptCurrentPnt.y - pLockingPipe->ptCurrentPnt.y);
		if (pLockingPipe->nPipeID == pPipe->nPipeID || Dis < MAX(pLockingPipe->ObjectFilter.sfSize.w, pLockingPipe->ObjectFilter.sfSize.h) * 1.5)
		{
			continue;
		}
		pPipe->bJammPipe = true;
	}
}



TrackUnlockState SkyTracker::Track(GD_VIDEO_FRAME_S img, GLB_INPUT* p_GLB_Input, API_MOT_PIPE* g_GLB_PipeProc, ARIDLL_PARMA* g_stArithPara, GLB_SCEN_MODE m_type)
{
    // 跟踪成功状态
    bool bTrackStatus = false;

    SkyTrkState = Locked_Tracking;                      // 对空(决策)跟踪器的跟踪状态
    m_TSky_Output.m_nTargetNum = 0;                     // 局部目标检测结果重置为0
    m_TSky_Output.m_nMatcherNum = 0;                    // 模板匹配结果重置为0

	// 管道队列
	PIPE* pPipeArray = g_GLB_PipeProc->getPipeArray();
	SINT32 nPipeArrayLen = g_GLB_PipeProc->PIPE_GetMaxPipeNum();

    Tracker_JammPipe(m_LockingPipe, pPipeArray, nPipeArrayLen);

    // 将当前帧找到目标置为空
    //memset(&mTargetFusion, 0, sizeof(TARGET_OBJECT));
    // 获取外部传入参数
    SetSkyParam(&g_stArithPara->stSkyParam);

    //判断当前跟踪目标相对于天地线的位置，决定是否对跟踪阶段的  小、面目标检测进行抑制
    skyControlInfo.bTrackLowSkyline = FALSE; //FALSE， TRUE;

    //执行小面目标跟踪器
    if (pSATracker && mTrakingPara.Sky_bEnableTrackSA)
    {
        memcpy(&m_TSky_Output.mTrakingPara_Output, &mTrakingPara, sizeof(Param_SkyTracker));
        // 执行对空跟踪
        pSATracker->Track(img, p_GLB_Input, g_GLB_PipeProc, &skyControlInfo, &m_TSky_Output);

        if (SizeType::SmallTarget == pSATracker->m_SizeMode || SizeType::DimTarget == pSATracker->m_SizeMode)
        {
            m_TSky_Output.m_SizeMode = SizeType::SmallTarget;
            memcpy(&m_TSky_Output.ObjectStatusTST, &pSATracker->m_ObjStatus, sizeof(OBJECTSTATUS));        // 导出小目标跟踪结果
        }
        else if (SizeType::AreaTarget == pSATracker->m_SizeMode)
        {
            m_TSky_Output.m_SizeMode = SizeType::AreaTarget;
            memcpy(&m_TSky_Output.ObjectStatusCEND, &pSATracker->m_ObjStatus, sizeof(OBJECTSTATUS));        // 导出质心跟踪结果

            DAT_PARAMETERS* pDAT_stPara = pSATracker->pDAT_Module->GetDatParm();
            // 参数对外输出(仅作为输出)
            m_TSky_Output.mTrakingPara_Output.prmTSkyDet.nDSmpScale = pDAT_stPara->nDSmpScale;
            m_TSky_Output.mTrakingPara_Output.prmTSkyDet.fAreaDetectGradDiffThre = pDAT_stPara->nGradThresMin;
            m_TSky_Output.mTrakingPara_Output.prmTSkyDet.nGrayThresMinBright = pDAT_stPara->nGrayThresMinBright;
            m_TSky_Output.mTrakingPara_Output.prmTSkyDet.nGrayThresMinDark = pDAT_stPara->nGrayThresMinDark;
        }
        m_TSky_Output.m_nTargetNum = pSATracker->m_TSA_output.m_nTargetNum;
        memcpy(m_TSky_Output.mTarget_Array, pSATracker->m_TSA_output.mTarget_Array, sizeof(TARGET_OBJECT) * m_TSky_Output.m_nTargetNum);
    }

    // 执行KCF跟踪器
    if (pKCFTracker && mTrakingPara.Sky_bEnableKCF)
    {
        pKCFTracker->KCF_Run(img, p_GLB_Input, &skyControlInfo);
        memcpy(&m_TSky_Output.ObjectStatusKCF, pKCFTracker->GetTrackeStatus(), sizeof(OBJECTSTATUS));
    }

    // 执行快速模板匹配
    if (matcher && mTrakingPara.Sky_bEnableMatcher)
    {
        //time_checker time1;
        //time1.TimeStart();

        templateMatching::Matcher_Process(matcher, img, &pSATracker->m_ObjStatus, &m_TSky_Output);

        //time1.TimeStop();

        //int a = time1.timeDistance();       // x86debug 3~5ms
        //LOG_INFO("Matcher_Process:", time1.timeDistance());
        
        //m_TSky_Output.ObjectStatusMatcher.
    }

    //AI识别监控
    if (mTrakingPara.Sky_bUseAIDet)
    {
        Ai_DMonitor->Process(img, p_GLB_Input, &ObjStatus, m_LockingPipe, g_GLB_PipeProc);
    }

    // 跟踪决策及模型更新
    bTrackStatus = TrackDecision(img, p_GLB_Input);


    // 跟踪成功
    if (bTrackStatus)
    {
        // 自动切换跟踪器
        SetTrackModeAuto(img, p_GLB_Input);

        // 小面跟踪目标及背景监控
        if (pSATracker && mTrakingPara.Sky_bEnableTrackSA)
        {
            // 面目标的情况下
            if (SizeType::AreaTarget == pSATracker->m_SizeMode)
            {
                // 计算跟踪面目标质心
                calculateCentroid(img, &ObjStatus, m_LockingPipe->ObjectFilter.fGray);

                // 面目标跟踪错误判定相关信息统计，AI识别率+位移信息统计
                SkyTrackFalse_Process(img, p_GLB_Input, &ObjStatus, m_LockingPipe);
            }
            else if (SizeType::SmallTarget == pSATracker->m_SizeMode || SizeType::DimTarget == pSATracker->m_SizeMode)
            {
                // 如果是小目标，需要单独计算的内容

            }

            // 对空目标类别判定处理
            SkyClassify_Process(img, &ObjStatus, m_LockingPipe, pSATracker->m_SizeMode);

            // 背景监控
            pBkgMonitor->BKM_BkgMonitor(img, p_GLB_Input, pSATracker->m_GrayMode, pSATracker->m_TSA_Input.crCenterRect, &ObjStatus);
            skyControlInfo.m_bMemoryTrackOcc = pBkgMonitor->g_GLB_bInterferenceMem;    // 过竿判定遮挡状态传递

            // SA跟踪器更新目标表观特性模型
            pSATracker->TO_RecordObjFeatures(&mTargetFusion, p_GLB_Input);
            m_TSky_Output.m_bComplexEnv = pBkgMonitor->g_GLB_bComplexEnv;               // 复杂背景透传到输出
            m_TSky_Output.m_bInterferenceMem = pBkgMonitor->g_GLB_bInterferenceMem;     // 过竿遮挡标记透传到输出

            // 跟踪器局部的检测器结果清空
            memset(pSATracker->m_Target_Array, 0, sizeof(TARGET_OBJECT) * pSATracker->m_nTargetNum);
            pSATracker->m_nTargetNum = 0;
        }
        //// KCF特性监控
        //if (pKCFTracker && mTrakingPara.Sky_bEnableKCF)
        //{
        //    // 获取KCF响应值监控
        //    KcfRespHistory* kcfStatus = &g_kcfRespVec;
        //    // 统计监控信息
        //    SINT32 nCount = MIN(KCFHistLen, kcfStatus->Cnt);

        //    PIPE_FEATURE_FLUC kcfStatus_Peak;
        //    TO_CalcStdOfArray(kcfStatus->kcf_peak, KCFHistLen, kcfStatus->ind, nCount, &kcfStatus_Peak);
        //    PIPE_FEATURE_FLUC kcfStatus_Psr;
        //    TO_CalcStdOfArray(kcfStatus->kcf_psr, KCFHistLen, kcfStatus->ind, nCount, &kcfStatus_Psr);
        //    PIPE_FEATURE_FLUC kcfStatus_apce;
        //    TO_CalcStdOfArray(kcfStatus->kcf_apce, KCFHistLen, kcfStatus->ind, nCount, &kcfStatus_apce);

        //    // 信息融合
        //    FLOAT32 Prob_Peak = EVENT_ProbCalc(kcfStatus->kcf_peak_curr, kcfStatus_Peak.fMean, kcfStatus_Peak.fStd);

        //    FLOAT32 Prob_Psr = EVENT_ProbCalc(kcfStatus->kcf_psr_curr, kcfStatus_Psr.fMean, kcfStatus_Psr.fStd);

        //    FLOAT32 Prob_Apce = EVENT_ProbCalc(kcfStatus->kcf_apce_curr, kcfStatus_apce.fMean, kcfStatus_apce.fStd);
        //}

        // 遮挡决策
        
        // 跟踪判错
    }
    else
    {
        // 跟踪失败，解锁
        Cancle();
    }

    // 返回跟踪状态
    return SkyTrkState;
}


// 对空强制记忆跟踪
TrackUnlockState SkyTracker::MemTrack(GD_VIDEO_FRAME_S img, GLB_INPUT* g_GLB_Input, API_MOT_PIPE* g_GLB_PipeProc, ARIDLL_PARMA* g_stArithPara, GLB_SCEN_MODE m_type)
{
    SkyTrkState = Locked_Losting;                      // 对空(决策)跟踪器的跟踪状态


    OBJECTSTATUS* pObjStatus = &this->ObjStatus;


    // 取管道运动模型
    FilterMeanNL stMotionMod_mean = m_LockingPipe->stMotionMod_mean;
    pObjStatus->ptPos.x = stMotionMod_mean.crnObjPrediRtLong.cx;
    pObjStatus->ptPos.y = stMotionMod_mean.crnObjPrediRtLong.cy;
    pObjStatus->sfAglSpeed.vx = m_LockingPipe->sfAglSpeed.vx;
    pObjStatus->sfAglSpeed.vy = m_LockingPipe->sfAglSpeed.vy;

    // 更新目标角度值
    pObjStatus->afAngle = stMotionMod_mean.ObjAglListsLong.arfPredict.afAngle;

    // 直接预测并更新决策跟踪器
    pObjStatus->unFrmId = g_GLB_Input->unFrmId;
    pObjStatus->unTotalCnt++;
    pObjStatus->unTrackedCnt++;
    pObjStatus->unContiTrackedCnt++;
    pObjStatus->unContiLostCnt++;
    pObjStatus->bObjMiss = FALSE;
    //更新目标丢失/跟踪失败标志
    pObjStatus->bObjLost = TRUE;

    //更新目标位置
    POINT32F pfPosPrePre = pObjStatus->ptPosPre;
    pObjStatus->ptPosPre = pObjStatus->ptPos;

    pObjStatus->ptPosFilter.x = (pObjStatus->ptPosPre.x + pObjStatus->sfSpeed.vx + pObjStatus->ptPos.x) / 2;
    pObjStatus->ptPosFilter.y = (pObjStatus->ptPosPre.y + pObjStatus->sfSpeed.vy + pObjStatus->ptPos.y) / 2;


    //更新目标尺寸
    pObjStatus->sfSize.s = pObjStatus->sfSize.w * pObjStatus->sfSize.h;
    pObjStatus->fObjPxlsCnt = pObjStatus->sfSize.s;

    //更新目标速度
    pObjStatus->sfSpeed.vx = (pObjStatus->ptPos.x - pfPosPrePre.x) / 2;
    pObjStatus->sfSpeed.vy = (pObjStatus->ptPos.y - pfPosPrePre.y) / 2;

    // 结果输出
    memcpy(&m_TSky_Output.ObjectStatusDesc, pObjStatus, sizeof(OBJECTSTATUS));

	return SkyTrkState;
}


void SkyTracker::Cancle()
{
	PIPE* pPipe = m_LockingPipe;
    
    if (pPipe)
    {
        pPipe->bTrackingPipe = false;
    }

    pKCFTracker->KCF_CleanUpObjectTracker();    // 清空KCF跟踪结果

    pBkgMonitor->BKM_CleanUpResult();           // 清空背景监控结果

    memset(&pSATracker->m_ObjStatus, 0, sizeof(OBJECTSTATUS));      // 小面跟踪结果清空
    memset(&ObjStatus, 0, sizeof(OBJECTSTATUS));                    // 对空决策结果清空

    // 类别判定监控信息清空
    memset(pipeClassifyInfo, 0, sizeof(PipeMainClassifyInfo));

    // 主跟踪目标状态信息清空
    memset(&SkyTrkStatusInfo, 0, sizeof(PipeMainStatusInfo));

    m_TSky_Output.m_SizeMode = SizeType::unKnown;       // 输出目标尺寸类型为未知
    m_TSky_Output.m_nTargetNum = 0;                     // 局部目标检测结果重置为0
    m_TSky_Output.m_nMatcherNum = 0;                    // 模板匹配结果重置为0

    // 跟踪阶段局部区域单帧目标检测结果
    memset(&m_TSky_Output.mTrakingPara_Output, 0, sizeof(Param_SkyTracker));
    memset(&m_TSky_Output.ObjectStatusTST, 0, sizeof(OBJECTSTATUS));
    memset(&m_TSky_Output.ObjectStatusCEND, 0, sizeof(OBJECTSTATUS));
    memset(&m_TSky_Output.ObjectStatusKCF, 0, sizeof(OBJECTSTATUS));
    memset(&m_TSky_Output.ObjectStatusDesc, 0, sizeof(OBJECTSTATUS));

}

void SkyTracker::SetMemTrack(bool bMemFlag)
{
    m_bSetMemoryTrack = bMemFlag;
}

void SkyTracker::SetSkyParam(Param_SkyTracker * stSkyParam)
{
    mTrakingPara.Sky_bEnableMatcher = stSkyParam->Sky_bEnableMatcher;
    mTrakingPara.Sky_bUseAIDet = stSkyParam->Sky_bUseAIDet;
    mTrakingPara.Sky_nUseAIDetFeq = stSkyParam->Sky_nUseAIDetFeq;
    mTrakingPara.Sky_nTrackMemFrmNum = stSkyParam->Sky_nTrackMemFrmNum;

    //mTrakingPara.Sky_bEnableTrackSA = stSkyParam->Sky_bEnableTrackSA;
    //mTrakingPara.Sky_bEnableKCF = stSkyParam->Sky_bEnableKCF;             // 内部自动调整关闭，因为清空了kcf信息，强制打开可能会导致程序崩溃，不建议设置kcf算法开关

    pSATracker->m_TSA_Param.Sky_bUseAIDet = stSkyParam->Sky_bUseAIDet;
    pSATracker->m_TSA_Param.nUseAIDetFeq = stSkyParam->Sky_nUseAIDetFeq;

    skyControlInfo.nTrackMemFrmNum = mTrakingPara.Sky_nTrackMemFrmNum;
}


Param_SkyTracker SkyTracker::GetSkyParam()
{
    return mTrakingPara;
}



CENTERRECT SkyTracker::getSATracker_SrBox()
{
    return pSATracker->m_TSA_output.crCenterRect;
}

CENTERRECT SkyTracker::getKCFTracker_SrBox()
{
    return pKCFTracker->KCF_GetSrBox();
}

SizeType SkyTracker::getSATracker_SizeType()
{
    return pSATracker->m_SizeMode;
}

TARGET_OBJECT* SkyTracker::getTrackerSATargetList(SINT32& num)
{
    num = MAX(pSATracker->m_nTargetNum, 0);
    return pSATracker->m_Target_Array;
}




bool SkyTracker::TrackDecision(GD_VIDEO_FRAME_S img, GLB_INPUT* p_GLB_Input)
{
    UBYTE8 ubSuccessFlag = FALSE;

    // 决策计算
    DecPolicy policy = Decision(img);

    // 执行决策结果
    ubSuccessFlag = ApplyDecision(policy, img, p_GLB_Input);

    // 跟踪错误判断,算法内部判断主动解锁
    if (SkyTrkStatusInfo.nAbnormalCnt > 10)
    {
        SkyTrkState = Unlock_ArithTrackFalse;
        ubSuccessFlag = FALSE;
    }

    // 若KCF和CEND都找到了目标，且此时发现长时轨迹预测偏差较大，直接清空长时轨迹
    FilterMeanNL * pFilter = &m_LockingPipe->stMotionMod_mean;
    if (mTrakingPara.Sky_bEnableKCF && mTrakingPara.Sky_bEnableTrackSA)
    {
        if (pKCFTracker->GetTrackeStatus()->fConfidence > 0.5 && pSATracker->m_ObjStatus.fConfidence > 0.4)
        {
            if (pFilter->nObjTrackLostCntLong > 0)
            {
                pFilter->ObjAglListsLong.nCnt = 0;
                pFilter->ObjAglListsLong.nEnd = 0;
                //g_GLB_stOutput.ObjAglListsLong.nStep = GLB_FRM_FREQ / 4;
                //g_GLB_stOutput.ObjAglListsLong.nListSize = GLB_OBJTRACK_LONG;
                //g_GLB_stOutput.ObjAglListsLong.parHistoryList = (ANGLE_R*)DSP_GLB_pObjAglListsLong;
                memset(pFilter->ObjAglListsLong.parHistoryList, 0, sizeof(ANGLE_R) * GLB_OBJTRACK_LEN);
                memset(&pFilter->ObjAglListsLong.arfFilter, 0, sizeof(ANGLE_R));
            }
        }
    }

    memcpy(&m_TSky_Output.ObjectStatusDesc, &ObjStatus, sizeof(OBJECTSTATUS));     // 对空跟踪决策结果透传

    return ubSuccessFlag;
}

DecPolicy SkyTracker::Decision(GD_VIDEO_FRAME_S img)
{
    // 默认全部保持，不做处理
    DecPolicy policy = { Act::Remain };
    // 仅运行检测跟踪器的决策流程，包含小目标和面目标逻辑一致
    if (mTrakingPara.Sky_bEnableTrackSA && !mTrakingPara.Sky_bEnableKCF)
    {
        policy.Act_SATracker = Act::Credit;//取SA
    }
    // 同时开启管道跟踪器与KCF，表明当前为面目标融合跟踪
    else if (mTrakingPara.Sky_bEnableTrackSA && mTrakingPara.Sky_bEnableKCF)
    {
        policy = TO_TrackDecisionOf_KCF_CEND(img);
    }
    // 仅开启KCF
    else if (!mTrakingPara.Sky_bEnableTrackSA && mTrakingPara.Sky_bEnableKCF)
    {
        policy.Act_KCFTracker = Act::Credit;//取KCF
    }
    // 跟踪器全关
    else
    {
        //pass
    }

    return policy;
}

bool SkyTracker::ApplyDecision(DecPolicy policy, GD_VIDEO_FRAME_S img, GLB_INPUT* p_GLB_Input)
{
    UBYTE8 ubSuccessFlag = FALSE;

    UINT16* pSrc = (UINT16*)img.u64VirAddr[0];
    SINT32 nWidth = img.u32Width;
    SINT32 nHeight = img.u32Height;

    // SA Tracker
    OBJECTSTATUS* pObjStatusSA = &pSATracker->m_ObjStatus;  // 小面跟踪决策输出
    // 错误保护
    if (policy.Act_SATracker == Act::Credit && policy.Act_KCFTracker == Act::Credit)
    {
        policy.Act_SATracker = Act::Reset;
    }

    // 先输出决策跟踪结果
    if (policy.Act_SATracker == Act::Credit)
    {
        memcpy(&ObjStatus, pObjStatusSA, sizeof(OBJECTSTATUS));
        // 将SA的匹配目标直接给mTargetFusion
        mTargetFusion = pSATracker->m_MatchedTarget;
    }
    else if (policy.Act_KCFTracker == Act::Credit)
    {
        // KCF 模型更新
        if (pKCFTracker->GetTrackeStatus()->unContiLostCnt < 1)
        {
            //20171218，更新kcf模板
            pKCFTracker->KCF_ModelUpdate(img);
        }
        // 直接取KCF跟踪结果
        memcpy(&ObjStatus, pKCFTracker->GetTrackeStatus(), sizeof(OBJECTSTATUS));

        // 质心中心定位结果拷贝到输出
        //ObjStatus.ptCentroid.x = pObjStatusSA->ptCentroid.x;
        //ObjStatus.ptCentroid.y = pObjStatusSA->ptCentroid.y;

        // AI尺寸结果拷贝到KCF对应位置
        ObjStatus.snAIDetSize.w = pObjStatusSA->snAIDetSize.w;
        ObjStatus.snAIDetSize.h = pObjStatusSA->snAIDetSize.h;
        ObjStatus.snAIDetSize.s = pObjStatusSA->snAIDetSize.s;
    }
    // 未采信任何跟踪器
    else
    {
        TrackUnlockState KCFTrkState = pKCFTracker->GetKcfTrkStatus();      // KCF的跟踪状态
        TrackUnlockState SATrkState = pSATracker->SATrkState;               // SATrack的跟踪状态
        if (Unlock_MemTimeOver == KCFTrkState && Unlock_MemTimeOver == SATrkState)
        {
            SkyTrkState = Unlock_MemTimeOver;
        }
        else
        {
            SkyTrkState = Unlock_MemOutFOV;
        }
        //更新目标计数器
        ObjStatus.unTrackedCnt++;
        ObjStatus.unContiTrackedCnt = 0;
        ObjStatus.unContiLostCnt++;
        //决策跟踪器解锁
        ObjStatus.bObjMiss = true;

        // 返回跟踪失败标记
        ubSuccessFlag = FALSE;
        return ubSuccessFlag;
    }
    //////////////////////////////////////////////////////////////////////////
    // 类别输出决策
    //////////////////////////////////////////////////////////////////////////
    SkyClassify_Desci(p_GLB_Input, pObjStatusSA);

    // 输出送显
    m_TSky_Output.ObjectStatusDesc.unClsType = ObjStatus.unClsType;
    m_TSky_Output.ObjectStatusDesc.emClsSrc = ObjStatus.emClsSrc;

    //////////////////////////////////////////////////////////////////////////
    // SA执行决策重置
    if (policy.Act_SATracker == Act::Reset)
    {
        pSATracker->Reset(&ObjStatus, true, img, nWidth, nHeight, p_GLB_Input);
    }
    else if (policy.Act_SATracker == Act::Adjust)
    {
        pSATracker->Reset(&ObjStatus, false, img, nWidth, nHeight, p_GLB_Input);
    }
    else
    {
        //pass
    }

    // KCF执行决策重置
    if (policy.Act_KCFTracker == Act::Reset)
    {
        pKCFTracker->KCF_Reset(&ObjStatus, true, img, p_GLB_Input);
    }
    else if (policy.Act_KCFTracker == Act::Adjust)
    {
        pKCFTracker->KCF_Reset(&ObjStatus, false, img, p_GLB_Input);
    }
    else
    {
        //pass
    }

    //// AID monitor 监控提供最佳重捕信息后，对KCF和小面进行重启
    //MASTERAIDMONITOR* p_masterMonitor = Ai_DMonitor->getAIDMasterInfo();
    //if (p_masterMonitor->nBestArrestId >= 0)
    //{
    //    NEIBOR* pNeibor = &p_masterMonitor->nNeiborInfo[p_masterMonitor->nBestArrestId];
    //    ObjStatus.ptPos.x = pNeibor->rsCurrentBox.x + pNeibor->rsCurrentBox.w / 2;
    //    ObjStatus.ptPos.y = pNeibor->rsCurrentBox.y + pNeibor->rsCurrentBox.h / 2;
    //    ObjStatus.sfSize.w = pNeibor->rsCurrentBox.w;
    //    ObjStatus.sfSize.h = pNeibor->rsCurrentBox.h;

    //    // 初始化目标响应及相似度信息
    //    pKCFTracker->KCF_Reset(&ObjStatus, true, img, p_GLB_Input);
    //    pSATracker->Reset(&ObjStatus, true, img, nWidth, nHeight, p_GLB_Input);

    //    // 重捕成功后参数重置
    //    Ai_DMonitor->initPara();
    //}

    policy.Act_KCFTracker = Act::Remain;        // 重置状态恢复
    policy.Act_SATracker = Act::Remain;         // 重置状态恢复

    ubSuccessFlag = !ObjStatus.bObjMiss;

    return ubSuccessFlag;
}



void SkyTracker::SetTrackModeAuto(GD_VIDEO_FRAME_S img, GLB_INPUT* p_GLB_Input)
{
    UBYTE8* pSmall2Area_Target = (UBYTE8*)pSATracker->TO_pSmall2Area_Target_counter;
    OBJECTSTATUS* pObjStatus = &ObjStatus;//上一帧跟踪结果

    UINT16* pSrc = (UINT16*)img.u64VirAddr[0];
    SINT32 nWidth = img.u32Width;
    SINT32 nHeight = img.u32Height;

    SINT16 i = 0;
    SINT16 nCount = 0;
    // 待融合跟踪器结果
    OBJECTSTATUS* pObjStatusKCF = pKCFTracker->GetTrackeStatus();
    OBJECTSTATUS* pObjStatusSA = &pSATracker->m_ObjStatus;

    CENTERRECT32F crfCandiRect;
    FLOAT32 fObjWHRatio = pObjStatus->sfSize.w / pObjStatus->sfSize.h;
    SINT32 nNewPipeId = -1;
    //POINT32F ptfObjPos;
    //SIZE32F sfObjSize;

    // 强制记忆跟踪期间，不切换目标跟踪器类型
    //if (g_GLB_Force_MemTrack || g_GLB_bInterferenceMem || g_GLB_MainHControl || (g_GLB_TargetMissileCrose_EO == GLB_MT_CROSS))
    //{
    //	return;
    //}

    //// 复杂场景丢失目标，也不切换//20230604
    if (pBkgMonitor->g_GLB_bComplexEnv)
    {
        return;
    }

    // 如果小面目标跟踪未开，直接返回
    if (!mTrakingPara.Sky_bEnableTrackSA)
    {
        return;
    }

    //+++++++++++++++++++++++++++++++++++++++++++++++++++++++
    //小目标跟踪切面目标跟踪
    if (pSATracker->m_SizeMode == SmallTarget)
    {
        //若目标尺寸大于阈值，则开启KCF跟踪、关闭小目标跟踪
        if ((pObjStatus->fObjPxlsCnt >= mTrakingPara.nSmallObjSizeMax
            || pObjStatus->sfSize.w >= 7 || pObjStatus->sfSize.h >= 7)
            && ((pObjStatusSA->unContiLostCnt > 0) ||
                (pObjStatus->fObjPxlsCnt >= mTrakingPara.nSmallObjSizeMax * 2.0)))
        {
            pSmall2Area_Target[pSATracker->g_GLB_S2ATagCnt] = 1;
        }
        else
        {
            pSmall2Area_Target[pSATracker->g_GLB_S2ATagCnt] = 0;
        }
        pSATracker->g_GLB_S2ATagCnt = (pSATracker->g_GLB_S2ATagCnt + 1) % GLB_TRACK_SMALL2FACE_CNT;

        //统计变大次数，若超过阈值，则切换算法
        nCount = 0;
        for (i = 0; i < GLB_TRACK_SMALL2FACE_CNT; i++)
        {
            nCount += pSmall2Area_Target[i];
        }

        // 如果TST即将超过记忆帧数还没有发现目标(即将解锁)，此时直接切换到面目标试试
        BBOOL bLostTST = FALSE;
        BBOOL bFindAreaTarget = FALSE;

#if 1
        if (pObjStatusSA->unContiLostCnt > 10 &&
            !(this->m_bSetMemoryTrack))
        {
            bLostTST = true;
        }

        RECT32S rc = { 0 };
        rc.x = SINT32(ObjStatus.ptPos.x - ObjStatus.sfSize.w / 2);
        rc.y = SINT32(ObjStatus.ptPos.y - ObjStatus.sfSize.h / 2);
        rc.w = SINT32(ObjStatus.sfSize.w);
        rc.h = SINT32(ObjStatus.sfSize.h);
        // 判断CEND单一目标跟踪，表明无干扰场景
        SINT32 nCrossTarget = pSATracker->getInterfereAreaTargteNum(rc);


        // 如果面目标检测器检测到1个目标，且小目标跟踪器丢失目标，可能是切换的最佳时机
        if (nCrossTarget > 0 && (ObjStatus.unContiLostCnt > 0) || (pObjStatus->sfSize.w >= 7 || pObjStatus->sfSize.h >= 7))
        {
            // 面目标检测到目标，需要更新特性。因为TST模板大小限制，可能丢失目标尺度
            TARGET_OBJECT* ptTargetArr = (TARGET_OBJECT*)pSATracker->m_Target_Array;
            TARGET_OBJECT* ptTarget = NULL;

            // 查找离跟踪中心最近的面目标
            FLOAT32 nDistMin = FLOAT32(nWidth * nHeight);
            SINT32 nIndex = -1;
            for (SINT32 i = 0; i < pSATracker->m_nTargetNum; i++)
            {
                FLOAT32 dist = ABS(ptTargetArr[i].pfCenPos.x - pObjStatusSA->ptPos.x) *
                    ABS(ptTargetArr[i].pfCenPos.y - pObjStatusSA->ptPos.y);
                if (dist < nDistMin)
                {
                    nDistMin = dist;
                    nIndex = i;
                }
            }

            if (nIndex >= 0)
            {

                ptTarget = &ptTargetArr[nIndex];
                if (ABS(ptTarget->pfCenPos.x - pObjStatusSA->ptPos.x) <= 3 &&
                    ABS(ptTarget->pfCenPos.y - pObjStatusSA->ptPos.y) <= 3 &&
                    (ptTarget->snSize.w >= 6 || ptTarget->snSize.h >= 6)
                    )
                {
                    bFindAreaTarget = true;//标记面目标跟踪成功
                    // 重新设置跟踪初始化值
                    pObjStatus->unContiLostCnt = 0;
                    pObjStatus->fObjPxlsCnt = FLOAT32(MAX((UINT32)mTrakingPara.nSmallObjSizeMax, ptTarget->unObjPxlsCnt));
                    pObjStatus->sfSize.w = FLOAT32(MAX(8, ptTarget->snSize.w));
                    pObjStatus->sfSize.h = FLOAT32(MAX(8, ptTarget->snSize.h));
                    //// 重置跟踪管道特性，防止被拒绝
                    //if (g_GLB_stOutput.nPriorPipeId >= 0)
                    //{
                    //	PIPE* pPipe = &pPipeArray[g_GLB_stOutput.nPriorPipeId];
                    //	memset(pPipe, 0, sizeof(PIPE));
                    //	PIPE_AddObjToSpecifiedEmptyPipe(pPipe, g_GLB_stOutput.nPriorPipeId, ptTarget);
                    //}

                }

            }


        }
#endif       

        if (nCount >= GLB_TRACK_SMALL2FACE_THRES && bFindAreaTarget) //以338A对空抗干扰数据--双机小目标交错为例
        {
            crfCandiRect.cx = pObjStatus->ptPos.x;
            crfCandiRect.cy = pObjStatus->ptPos.y;
            crfCandiRect.w = pObjStatus->sfSize.w;
            crfCandiRect.h = pObjStatus->sfSize.h;
            crfCandiRect.s = crfCandiRect.w * crfCandiRect.h;

            pKCFTracker->KCF_InitObjectParameters(img, m_LockingPipe, p_GLB_Input);
            mTrakingPara.Sky_bEnableKCF = true;

            // SA跟踪器置于面目标
            pSATracker->m_SizeMode = SizeType::AreaTarget;

            //小目标和面目标相互切换计数器清零
            pSATracker->g_GLB_S2ATagCnt = 0;
            memset(pSmall2Area_Target, 0, sizeof(UBYTE8) * GLB_TRACK_SMALL2FACE_CNT);
        }
    }

    //+++++++++++++++++++++++++++++++++++++++++++++++++++++++
    //正常(中/大)目标跟踪
    if (pSATracker->m_SizeMode == AreaTarget)
    {
        //若目标尺寸小于阈值，则开启小目标跟踪、关闭KCF跟踪
        SINT32 nFaceObjPxlsMin = MAX(mTrakingPara.nSmallObjSizeMax, 25/*pSATracker->*/);
        if ((pObjStatus->fObjPxlsCnt < nFaceObjPxlsMin - 8 || (pObjStatus->sfSize.w < 5 && pObjStatus->sfSize.h < 5))
            && ((pObjStatusSA->unContiLostCnt > 0) || pObjStatus->fObjPxlsCnt < nFaceObjPxlsMin))
        {
            pSmall2Area_Target[pSATracker->g_GLB_S2ATagCnt] = 1;
        }
        else
        {
            pSmall2Area_Target[pSATracker->g_GLB_S2ATagCnt] = 0;
        }
        pSATracker->g_GLB_S2ATagCnt = (pSATracker->g_GLB_S2ATagCnt + 1) % GLB_TRACK_SMALL2FACE_CNT;

        //统计变大次数，若超过阈值，则切换算法
        nCount = 0;
        for (i = 0; i < GLB_TRACK_SMALL2FACE_CNT; i++)
        {
            nCount += pSmall2Area_Target[i];
        }


        // S726属于逼近型跟踪器，正常跟踪不会出现面目标转小目标，将GLB_TRACK_SMALL2FACE_THRES 调整为7          by wcw04046 @ 2021/12/01
        if (nCount >= GLB_TRACK_SMALL2FACE_THRES)
        {

            //初始化TST跟踪算法
            //pSATracker->Init(pSrc, nWidth, nHeight, pxlType, m_LockingPipe, g_GLB_Input);
            pKCFTracker->KCF_CleanUpObjectTracker();

            //清空CEND、KCF跟踪算法
            memset(pObjStatusSA, 0, sizeof(OBJECTSTATUS));
            memset(pObjStatusKCF, 0, sizeof(OBJECTSTATUS));

            //小目标和面目标相互切换计数器清零
            pSATracker->g_GLB_S2ATagCnt = 0;
            memset(pSmall2Area_Target, 0, sizeof(UBYTE8) * GLB_TRACK_SMALL2FACE_CNT);

            pSATracker->m_SizeMode = SmallTarget;//切到小目标模式

            mTrakingPara.Sky_bEnableKCF = false;//关闭KCF
        }

    }

}

POINT16S* SkyTracker::GetDST_MaxPoint_Sky()
{
    return pSATracker->getDSTmodule()->getMaxPoint();
}




DecPolicy SkyTracker::TO_TrackDecisionOf_KCF_CEND(GD_VIDEO_FRAME_S img)
{
    DecPolicy policy = { Act::Remain };

    OBJECTSTATUS* pObjStatus = &ObjStatus;
    OBJECTSTATUS* pObjStatusKCF = pKCFTracker->GetTrackeStatus();
    OBJECTSTATUS* pObjStatusCEND = &pSATracker->m_ObjStatus;

    BBOOL   bSameObjCendKCF = false;//CEND与KCF结果的是否为同一个目标
    FLOAT32 fSizeSameCendKCF = 0.0f; //CEND与KCF的目标尺寸重合率
    FLOAT32 fWHDiffCendKCF = 0.0f; //CEND与KCF的目标宽高差异
    SIZE32F snObjRadius = { 0 };    //目标半径
    FLOAT32 fObjWHRatio = 0.0;    //目标宽高比
    SINT32  i = 0;
    SINT32  nBkgAbnormal = 0;



    //20171206，统计背景区域异常块个数
    //for (i = 0; i < GLB_OBJ_BKG_NUM; i++)
    //{
    //	if (g_GLB_bObj8BkgStatus[i])
    //	{
    //		nBkgAbnormal++;
    //	}
    //}

    //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    //上一帧目标结果：尺寸半径、目标矩形、宽高比
    snObjRadius.w = pObjStatus->sfSize.w / 2.0f;
    snObjRadius.h = pObjStatus->sfSize.h / 2.0f;
    fObjWHRatio = pObjStatus->sfSize.w / pObjStatus->sfSize.h;

    //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    //判断CEND与KCF结果的是否为同一个目标，计算重合率、相似度
    bSameObjCendKCF = TO_IsCoincideObj(pObjStatusCEND->ptPos, pObjStatusCEND->sfSize,
        pObjStatusKCF->ptPos, pObjStatusKCF->sfSize, &fSizeSameCendKCF, &fWHDiffCendKCF);


    // 判断CEND单一目标跟踪，表明无干扰场景
    RECT32S rc = { 0 };
    rc.x = SINT32(ObjStatus.ptPos.x - ObjStatus.sfSize.w / 2);
    rc.y = SINT32(ObjStatus.ptPos.y - ObjStatus.sfSize.h / 2);
    rc.w = SINT32(ObjStatus.sfSize.w);
    rc.h = SINT32(ObjStatus.sfSize.h);

    SINT32 nCrossTarget = pSATracker->getInterfereAreaTargteNum(rc);

    // 先处理有跟踪器解锁情况（两个跟踪器都解锁）
    if (pObjStatusCEND->bObjMiss && pObjStatusKCF->bObjMiss)
    {
        // 直接退出，谁也不信
        return policy;
    }

    if (pObjStatusCEND->bObjMiss)
    {
        // 采信KCF
        policy.Act_KCFTracker = Act::Credit;
        policy.Act_SATracker = Act::Reset;
        return policy;
    }
    else if (pObjStatusKCF->bObjMiss)
    {
        //采信SA
        policy.Act_SATracker = Act::Credit;
        policy.Act_KCFTracker = Act::Reset;
        return policy;
    }

    // 以下处理都未跟丢情形

    // STATUS-1:均稳跟
    if (pObjStatusCEND->unContiLostCnt == 0 && pObjStatusKCF->unContiLostCnt == 0)
    {
        // 跟踪相同位置，且简单场景下，取CEND
        if (!pBkgMonitor->g_GLB_bComplexEnv && bSameObjCendKCF)
        {
            //采信SA，但不重置KCF，只调整位置
            policy.Act_SATracker = Act::Credit;
            policy.Act_KCFTracker = Act::Adjust;
            g_GLB_nTrackDecisionCnt++;
            // 1.附近没有目标且跟踪偏离，隔5帧重置一次KCF（更苛刻条件下）
            // 2.CEND检出稳定的尺寸条件下，每20帧重置1次KCF
            if (
                ((1 == nCrossTarget && fSizeSameCendKCF < 0.7f && g_GLB_nTrackDecisionCnt > 5)
                    || (pObjStatusCEND->fConfidence > 0.8f && nBkgAbnormal <= 2
                        && g_GLB_nTrackDecisionCnt > 20 && pObjStatusCEND->sfSize.s < 300))
                )
            {
                policy.Act_KCFTracker = Act::Reset;
            }

            if (g_GLB_nTrackDecisionCnt > 20)
            {
                g_GLB_nTrackDecisionCnt = 0;
            }

        }
        // 其余情况取KCF，并重置CEND
        else
        {
            policy.Act_SATracker = Act::Adjust;
            policy.Act_KCFTracker = Act::Credit;
            g_GLB_nTrackDecisionCnt = 0;
        }
    }

    // STATUS-2:SA记忆
    if (pObjStatusCEND->unContiLostCnt > 0 && pObjStatusKCF->unContiLostCnt == 0)
    {
        //SA对学习依赖不高，随时重置无影响
        policy.Act_SATracker = Act::Adjust;
        policy.Act_KCFTracker = Act::Credit;
    }

    // STATUS-3:KCF记忆
    if (pObjStatusKCF->unContiLostCnt > 0 && pObjStatusCEND->unContiLostCnt == 0)
    {
        policy.Act_SATracker = Act::Credit;
        policy.Act_KCFTracker = Act::Adjust;
        // KCF不轻易重置
        if (pObjStatusKCF->unContiLostCnt > 5 && pObjStatusCEND->unContiTrackedCnt > 5)
        {
            policy.Act_KCFTracker = Act::Reset;
        }
    }

    // STATUS-4:均记忆，取KCF
    if (pObjStatusKCF->unContiLostCnt > 0 && pObjStatusCEND->unContiLostCnt > 0)
    {
        policy.Act_SATracker = Act::Reset;
        policy.Act_KCFTracker = Act::Credit;
    }


    return policy;
}



//
///*
//* 将跟踪器结果输出为管道目标信息，不要丢特征，否则可能导致SA跟踪器特征突变一直无法跟踪
//*/
//bool SkyTracker::UpdateTracker2Pipe(GLB_INPUT* g_GLB_Input, PIPE* pPipeOut)
//{
//	PIPE* pPipe = pPipeOut;
//
//	// 总帧数+1
//	pPipe->unTotalCnt++;
//
//	// 目标丢失
//	if (ObjStatus.unContiLostCnt > 0)
//	{
//		//标记管道丢失
//		pPipe->bLost = true;
//		pPipe->unLostCnt++;
//		pPipe->unContinueExistCnt = 0;
//		////当管道目标超过N次（连续）没被检测到时，清空管道。
//		//if (pPipe->unLostCnt >= (UINT32)(nPipeFOVLostCnt))
//		//{
//		//	//标记跟踪失败
//		//	ubSuccessFlag = FALSE;
//		//	return ubSuccessFlag;
//		//}
//	}
//	// 目标更新
//	else
//	{
//		//获取在当前帧中查找到的最相似目标
//		//KCF获得的目标只有位置和尺度信息
//		TARGET_OBJECT Target = { 0 };
//		Target.unFrmID = g_GLB_Input->unFrmId;
//		Target.bObject = true;
//		Target.pfCenPos = ObjStatus.ptPos;
//		//ptTarget.pnMaxPos = ptTarget.pfCenPos;
//		Target.afAngle = ObjStatus.afAngle;
//		Target.fMatchConf = ObjStatus.fConfidence;
//
//		// 尺度，重要，KCF和SA之间除位置外唯一的切换特征
//		Target.snSize.w = ObjStatus.sfSize.w;
//		Target.snSize.h = ObjStatus.sfSize.h;
//		Target.snSize.s = ObjStatus.sfSize.s;
//		Target.nObjTypeSrc = ObjStatus.nObjTypeSrc;
//		Target.unObjPxlsCnt = ObjStatus.fObjPxlsCnt;
//
//		// 如果对空小面目标跟踪器已经检出目标，且是融合决策目标，直接加入管道
//		if (mTargetFusion.bObject)
//		{
//			Target = mTargetFusion;
//		}
//
//		// 将跟踪决策目标加入管道
//		PIPE_AddObjToOccupiedPipe(pPipe, pPipe->nPipeID, &Target, 0, g_GLB_Input);
//	}
//
//	return true;
//}

//20180305，统计数组的标准差
void SkyTracker::CalcStdOfArrayStatus(FLOAT32* pfArray, SINT32 nTotalLen, SINT32 nEnd,
    SINT32 nCountLen, PIPE_FEATURE_FLUC* pstPipeFeature)
{
    //统计极值、均值、方差
    FLOAT32 fMinValue = 1e6;
    FLOAT32 fMaxValue = -1e6;
    DOUBLE64 dMean = 0.0f;
    DOUBLE64 dVariance = 0.0f;	//方差
    SINT32 i = 0;
    FLOAT32 fValue = 0.0f;
    //FLOAT32 fStd = 0.0f;	//标准差
    SINT32 nIndex = 0;		//数组下标
    FLOAT32 fNormalizedArray[TRACK_STATUS_DEPTH_MAX] = { 0 };	//归一化后的数组
    //FLOAT32 fRange = 0.0f;	//数组值域范围

    //防错，如果统计长度大于总数组长度，则直接返回-1（异常）
    if (nCountLen > TRACK_STATUS_DEPTH_MAX)
    {
        return;
    }

    for (i = 0; i < nCountLen; i++)
    {
        nIndex = (nEnd - i + nTotalLen) % nTotalLen;
        fValue = pfArray[nIndex];

        //有效值存入归一化数组
        fNormalizedArray[i] = fValue;

        //最大最小值统计
        fMinValue = MIN(fMinValue, fValue);
        fMaxValue = MAX(fMaxValue, fValue);
    }

    //归一化数组并统计均值和方差累加
    for (i = 0; i < nCountLen; i++)
    {
        fValue = fNormalizedArray[i];

        //背景灰度及灰度平方累加
        dMean += (DOUBLE64)fValue;
        dVariance += (DOUBLE64)fValue * fValue;
    }

    //计算均值、方差
    dMean = dMean / (DOUBLE64)(MAX(nCountLen, 1));
    dVariance = dVariance / (DOUBLE64)(MAX(nCountLen, 1));
    dVariance -= dMean * dMean;
    pstPipeFeature->fMean = (FLOAT32)dMean;
    pstPipeFeature->fStd = (FLOAT32)sqrt(dVariance);
    pstPipeFeature->fMin = fMinValue;
    pstPipeFeature->fMax = fMaxValue;

    return;
}

void SkyTracker::SkyTrackFalse_Process(GD_VIDEO_FRAME_S img, GLB_INPUT* p_GLB_Input, OBJECTSTATUS* ObjStatus, PIPE* m_LockingPipe)
{
    if (FALSE == SkyTrkStatusInfo.bOccupy)
    {
        SkyTrkStatusInfo.bOccupy = TRUE;
    }

    SkyTrkStatusInfo.nCountNum += 1;

    // 角度位置信息
    if (1 == SkyTrkStatusInfo.nCountNum)
    {
        SkyTrkStatusInfo.fPosAngle = ObjStatus->afAngle;
    }

    FLOAT32 disX = ABS(DEGLIM(SkyTrkStatusInfo.fPosAngle.fAz - ObjStatus->afAngle.fAz)) / p_GLB_Input->stCamera.fAglReso;
    FLOAT32 disY = ABS(DEGLIM(SkyTrkStatusInfo.fPosAngle.fPt - ObjStatus->afAngle.fPt)) / p_GLB_Input->stCamera.fAglReso;
    SkyTrkStatusInfo.fMoveDis[SkyTrkStatusInfo.ubEnd] = sqrt(POW2(disX) + POW2(disY));

    if (SkyTrkStatusInfo.nCountNum > 2 * p_GLB_Input->unFreq)   // 单次计数满更新数据
    {
        SkyTrkStatusInfo.nAIDetNum = 0;
        SkyTrkStatusInfo.ubEnd = (++SkyTrkStatusInfo.ubEnd) % TRACK_STATUS_DEPTH_MAX;
        SkyTrkStatusInfo.nCountNum = SkyTrkStatusInfo.nCountNum % (2 * p_GLB_Input->unFreq);
        SkyTrkStatusInfo.nSavedNum = (++SkyTrkStatusInfo.nSavedNum) % TRACK_STATUS_DEPTH_MAX;
    }

    // 识别信息统计
    if (ObjStatus->snAIDetSize.w > 0)
    {
        SkyTrkStatusInfo.nAIDetNum += 1;
        SkyTrkStatusInfo.fAIDetRatio[SkyTrkStatusInfo.ubEnd] = 1.0f * SkyTrkStatusInfo.nAIDetNum / SkyTrkStatusInfo.nCountNum;
    }
    
    PIPE_FEATURE_FLUC DetFeatureB = { 0 };

    PIPE_FEATURE_FLUC MoveFeatureB = { 0 };

    // 累计存满6次，开始分析当前跟踪目标是否出错
    if (SkyTrkStatusInfo.nSavedNum > 3 && SkyTrkStatusInfo.nCountNum > p_GLB_Input->unFreq)
    {
        // 识别率变化
        CalcStdOfArrayStatus(SkyTrkStatusInfo.fAIDetRatio, TRACK_STATUS_DEPTH_MAX, (SkyTrkStatusInfo.ubEnd - 2) % TRACK_STATUS_DEPTH_MAX,
            SkyTrkStatusInfo.nSavedNum - 1, &DetFeatureB);


        // 位移量变化
        CalcStdOfArrayStatus(SkyTrkStatusInfo.fMoveDis, TRACK_STATUS_DEPTH_MAX, (SkyTrkStatusInfo.ubEnd - 2) % TRACK_STATUS_DEPTH_MAX,
            SkyTrkStatusInfo.nSavedNum - 1, &MoveFeatureB);


        // 跟踪异常判断条件
        if (DetFeatureB.fMean > 0.65 && SkyTrkStatusInfo.fAIDetRatio[SkyTrkStatusInfo.ubEnd] <= 0.05 &&
            MoveFeatureB.fMean > 100 && SkyTrkStatusInfo.fMoveDis[SkyTrkStatusInfo.ubEnd] <= 20)
        {
            SkyTrkStatusInfo.nAbnormalCnt += 1;
        }
    }
}