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();
    pBkgMonitor = new BkgMonitor();
    mTargetFusion = { 0 };
    ObjStatus = { 0 };
    m_LockingPipe = nullptr;
    g_GLB_nTrackDecisionCnt = 0;
    m_bSetMemoryTrack = false;

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


    bsvmInitDone = FALSE;
    pipeAlarmInfo = new PipeMainAlarmInfo;
    memset(pipeAlarmInfo, 0, sizeof(PipeMainAlarmInfo));

    // 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 ((model = svm_load_model(svmModelName)) == 0)
    {
        bsvmInitDone = FALSE;
    }
    else
    {
        x = (struct svm_node*)malloc(MAIN_ALARM_DEPTH_MAX * 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);
}

SkyTracker::SkyTracker(SINT32 nWidth, SINT32 nHeight, Param_SkyTracker para)
{
    pSATracker = new SA_Tracker(nWidth, nHeight);
    pKCFTracker = API_KCF_Tracker::Create();
    pBkgMonitor = new BkgMonitor();

    mTargetFusion = { 0 };
    ObjStatus = { 0 };
    m_LockingPipe = nullptr;
    g_GLB_nTrackDecisionCnt = 0;
    m_bSetMemoryTrack = false;

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

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

    bsvmInitDone = FALSE;

    pipeAlarmInfo = new PipeMainAlarmInfo;
    memset(pipeAlarmInfo, 0, sizeof(PipeMainAlarmInfo));

    // 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 ((model = svm_load_model(svmModelName)) == 0)
    {
        LOG_WARN("svm_load_model Failed");
        bsvmInitDone = FALSE;
    }
    else
    {
        x = (struct svm_node*)malloc(MAIN_ALARM_DEPTH_MAX * 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);
}

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

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


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
    // 面目标初始化KCF
    if (pSATracker && pSATracker->m_SizeMode == SizeType::AreaTarget)
    {
        mTrakingPara.Sky_bEnableKCF = true;
    }
    else
    {
        mTrakingPara.Sky_bEnableKCF = false;
    }

    // 初始化Matcher模板匹配信息
    RECT32S stTargetRect = { 0 };
    TARGET_OBJECT* pTrackingTarget = &pLockPipe->objHistoryList[pLockPipe->ubEnd];
    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);  // 设置模板


    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));

    return true;
}

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

    SINT32 nWidth = img.u32Width;
    SINT32 nHeight = img.u32Height;

    m_TSky_Output.m_nTargetNum = 0;                     // 局部目标检测结果重置为0
    m_TSky_Output.m_nMatcherNum = 0;                    // 模板匹配结果重置为0

    // 将当前帧找到目标置为空
    memset(&mTargetFusion, 0, sizeof(TARGET_OBJECT));

    // 获取外部传入参数
    //memcpy(&mTrakingPara, &g_stArithPara->stSkyParam, sizeof(Param_SkyTracker));
    mTrakingPara.Sky_bEnableMatcher = g_stArithPara->stSkyParam.Sky_bEnableMatcher;
    mTrakingPara.Sky_bUseAIDet = g_stArithPara->stSkyParam.Sky_bUseAIDet;

    //执行小面目标跟踪器
    if (pSATracker && mTrakingPara.Sky_bEnableTrackSA)
    {
        // 执行对空跟踪
        pSATracker->Track(img, nWidth, nHeight, p_GLB_Input, g_GLB_PipeProc);
        memcpy(&m_TSky_Output.mTrakingPara_Output, &mTrakingPara, sizeof(Param_SkyTracker));
        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);
        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.
    }

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


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

        if(bsvmInitDone)
        {
            RecordAlarmInfo(m_LockingPipe);
            AlarmClassVerify(m_LockingPipe);
        }

        if (mTrakingPara.Sky_bEnableTrackSA)
        {
            // 背景监控
            pBkgMonitor->BKM_BkgMonitor(img, nWidth, nHeight, pSATracker->m_GrayMode, pSATracker->m_TSA_Input.crCenterRect, &ObjStatus);

            // SA跟踪器更新目标表观特性模型
            pSATracker->TO_RecordObjFeatures(&mTargetFusion, p_GLB_Input);
        }

    }
    else
    {
        // 跟踪失败，解锁
        Cancle();
        // 返回跟踪状态
        return bTrackStatus;
    }

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


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));                    // 对空决策结果清空

    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 PARA)
{
    
}


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



CENTERRECT SkyTracker::getSATracker_SrBox()
{
    return pSATracker->m_TSA_Input.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;
}


void SkyTracker::RecordAlarmInfo(PIPE* pPipe)
{
    if (FALSE == pipeAlarmInfo->bOccupy)
    {
        pipeAlarmInfo->bOccupy = TRUE;
    }
    pipeAlarmInfo->nAlarmBatchID = pPipe->nAlarmBatchID_1;
    pipeAlarmInfo->frmAlarmInfo[pipeAlarmInfo->ubEnd].unFrmId = pPipe->objHistoryList[pPipe->ubEnd].unFrmID;
    pipeAlarmInfo->frmAlarmInfo[pipeAlarmInfo->ubEnd].afAngle = pPipe->objHistoryList[pPipe->ubEnd].afAngle;
    pipeAlarmInfo->frmAlarmInfo[pipeAlarmInfo->ubEnd].pfCenPos = pPipe->objHistoryList[pPipe->ubEnd].pfCenPos;
    pipeAlarmInfo->frmAlarmInfo[pipeAlarmInfo->ubEnd].snSize = pPipe->objHistoryList[pPipe->ubEnd].snSize;


    pipeAlarmInfo->nCountNum = pipeAlarmInfo->nCountNum + 1 > MAIN_ALARM_DEPTH_MAX ? MAIN_ALARM_DEPTH_MAX : pipeAlarmInfo->nCountNum + 1;
    pipeAlarmInfo->ubEnd = (UBYTE8)((pipeAlarmInfo->ubEnd + 1) % MAIN_ALARM_DEPTH_MAX);

}


void SkyTracker::AlarmClassVerify(PIPE* pPipe)
{
    // 未缓存足够数据的时候跳过
    if (pipeAlarmInfo->nCountNum < MAIN_ALARM_DEPTH_MAX)
    {
        return;
    }

    // 未达到判定点跳过
    if (0 != pipeAlarmInfo->ubJudgePoint % 50)
    {
        pipeAlarmInfo->ubJudgePoint += 1;
        return;
    }

    int i = 0;
    double lower = -1.0, upper = 1.0;
    double predict_label = -100;
    // step1:轨迹稳定性判断


    // step2:基于IOU连续性范围统计判断

    // step3:基于svm抖动分类判断
    float scaled_res[MAIN_ALARM_DEPTH_MAX] = { 0 };

    unsigned char ubIndex = pipeAlarmInfo->ubEnd;

    // 数据归一化
    for (i = 0; i < MAIN_ALARM_DEPTH_MAX; ++i)
    {
        scaled_res[i] = lower + (upper - lower) * (pipeAlarmInfo->frmAlarmInfo[ubIndex].pfCenPos.y - svmParm.scaled_param[i][1]) /
            (svmParm.scaled_param[i][2] - svmParm.scaled_param[i][1]);
        ubIndex = (ubIndex + 1) % MAIN_ALARM_DEPTH_MAX;
    }

    for (i = 0; i < MAIN_ALARM_DEPTH_MAX; ++i)
    {
        x[i].index = i + 1;
        x[i].value = scaled_res[i];
    }
    x[i].index = -1;

    predict_label = svm_predict(model, x);      // 0表示无人机，1表示飞鸟
    pipeAlarmInfo->ubJudgePoint = 1;

    //pPipe->unClsType = ClassType::UAV;
    //if (ABS(predict_label - 0) < EPSILON)
    //{
    //    pPipe->unClsSubType = SubTypeUAV::FourRotorDrone;
    //}
    //else if (ABS(predict_label - 1) < EPSILON)
    //{
    //    pPipe->unClsSubType = SubTypeUAV::Bird;
    //}
}


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);

    // 面目标跟踪结果回传到检测器内部（不含KCF）
    OBJECTSTATUS* pDATTrk = pSATracker->pDAT_Module->GetTargetMatched();
    if (SizeType::AreaTarget == m_TSky_Output.m_SizeMode)
    {
        memcpy(pDATTrk, &pSATracker->m_ObjStatus, sizeof(OBJECTSTATUS));
    }
    else
    {
        memset(pDATTrk, 0, sizeof(OBJECTSTATUS));
    }


    // 若KCF和CEND都找到了目标，且此时发现长时轨迹预测偏差较大，直接清空长时轨迹
    auto 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;

    // 错误保护
    if (policy.Act_SATracker == Act::Credit &&
        policy.Act_KCFTracker == Act::Credit)
    {
        policy.Act_SATracker = Act::Reset;
    }

    // 先输出决策跟踪结果
    if (policy.Act_SATracker == Act::Credit)
    {
        // SA Tracker
        OBJECTSTATUS* pObjStatusSA = &pSATracker->m_ObjStatus;
        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));
    }
    // 未采信任何跟踪器
    else
    {
        //更新目标计数器
        ObjStatus.unTrackedCnt++;
        ObjStatus.unContiTrackedCnt = 0;
        ObjStatus.unContiLostCnt++;
        //决策跟踪器解锁
        ObjStatus.bObjMiss = true;

        // 返回跟踪失败标记
        ubSuccessFlag = FALSE;
        return ubSuccessFlag;
    }

    // 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
    }

    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;
//}