S3315_RK3588/NeoTracker/src/Arith_TrackSAObj.cpp

#include "Arith_TrackSAObj.h"
#include "Arith_CoordModule.h"
#include "Arith_ImgOperate.h"
#include "PIPE/Arith_PIPE_utils.h"
#include "Arith_Detector.h"
#include "Arith_DetectAreaObj.h"
#include "Arith_Bbox.h"
// 定义跟踪器最大检测个数
#define TK_TARGET_MAX_NUM 10  


//#define GLB_TRACK_SMALL2FACE_CNT    5   //小目标和面目标相互切换数组总长度
//#define GLB_TRACK_SMALL2FACE_THRES  3   //小目标和面目标相互切换计数阈值
//#define GLB_SMALL2FACE_SIZE_THRES   36 //49//64  //小目标转面目标尺寸上限                qw 20220104 


SA_Tracker::SA_Tracker(int nWidth, int nHeight)
{
	pDST_Module = API_DetectSmallObj::Create(nWidth, nHeight);
	pDAT_Module = API_DetectAreaObj::Create(nWidth, nHeight);


	// 最大处理TK_TARGET_MAX_NUM个目标
	//m_Target_Array = new TARGET_OBJECT[TK_TARGET_MAX_NUM];
    // 将跟踪器的本地队列指向小目标队列，复用内存
    m_Target_Array = pDST_Module->GetTargetArray();


    m_LockingPipe = nullptr;

    m_ObjStatus = { 0 };
    m_SizeMode = SizeType::SmallTarget;
    m_GrayMode = GrayType::BrightTarget;

    m_TSA_Input = { 0 };
    m_TSA_Param = { 0 };
    m_TSA_output = { 0 };

    // 波门影响的检测效果，应该为每个目标设置合适的波门
    m_TSA_Param.nPipeRadiusTrack = GLB_PIPE_RADIUS_TRACK;      // 小面正常跟踪过程中的波门
    m_TSA_Param.nPipeRadiusLost = GLB_PIPE_RADIUS_LOST;        // 小面跟踪丢失时的波门
    m_TSA_Param.nPipeFOVLostCnt = GLB_MEMORY_FRM_NUM;       // 小面目标跟踪默认记忆帧数
    m_TSA_Param.nUseAIDetFeq = 1;              // 使用AI信息进行修正的频率,默认为1,每帧修正1次

    g_GLB_MonitorStatus = { 0 };
    g_GLB_MonitorStatus.bMonitorVaildFlag = TRUE;

    nTrackTargetID = -1;
    m_nTargetNum = 0;

    m_MatchTarget1 = { 0 };
    m_MatchTarget2 = { 0 };
    m_MatchedTarget = { 0 };

    // 跟踪阶段局部区域单帧目标检测结果
    m_TSA_output.m_nTargetNum = 0;
    m_TSA_output.mTarget_Array = new TARGET_OBJECT[DT_TARGET_MAX_NUM];

    //跟踪算法：小目标与面目标相互切换计数
    memset(TO_pSmall2Area_Target_counter, 0, GLB_TRACK_SMALL2FACE_CNT * sizeof(UBYTE8));
    g_GLB_S2ATagCnt = 0;   //小目标和面目标相互切换计数器

    nSAUseAIDetFeq = 1;       // 默认AI信息每帧用1次

    //跟踪算法：统计跟踪目标周围相似目标个数
    ubSimTarget_End = 0;        // 索引初始化为0
    CountSim2To0 = 0;           // 2变成0的次数置为0
    bSim2To0_T = FALSE;         // 2变0, 0连续持续T帧的标志位为假
    nSimStateLifeTime = 0;      // 生命周期默认为0帧
    memset(nSimTargetNum_Counter, -1, GLB_SIMOBJ_VALID_CNT * sizeof(SINT32));

    SATrkState = LockStateUnknown;      // 小面记忆跟踪状态跟踪

    
}


SA_Tracker::~SA_Tracker()
{
    if (pDST_Module)
    {
        delete pDST_Module;
    }
    if (pDAT_Module)
    {
        delete pDAT_Module;
    }
	if (m_TSA_output.mTarget_Array)
	{
        delete[] m_TSA_output.mTarget_Array;
	}

    pDST_Module = NULL;
    pDAT_Module = NULL;
}



// 使用管道初始化小面目标跟踪器，返回成功。
bool SA_Tracker::Init(UINT16* pSrc, SINT32 nWidth, SINT32 nHeight, PIPE* pTrackingPipe, GLB_INPUT* p_GLB_Input)
{
    m_LockingPipe = pTrackingPipe;

    m_nTargetNum = 0;

	// 标记为跟踪管道
    m_LockingPipe->bTrackingPipe = true;
    m_LockingPipe->bTracking = false;
    SINT32 nEnd = m_LockingPipe->ubEnd;
    SINT32 nObjSize = (SINT32)(m_LockingPipe->ObjectFilter.fPxlsCnt);
    TARGET_OBJECT* pTrackingTarget = &m_LockingPipe->objHistoryList[nEnd];
	// 计算目标尺度类型，决定初始跟踪参数，初始化不允许弱小目标，虚警无法控制
    if (nObjSize <= GLB_SMALL2FACE_SIZE_THRES)
    {
        m_SizeMode = SizeType::SmallTarget;
    }
    else
    {
        m_SizeMode = SizeType::AreaTarget;
    }
    // 计算目标灰度类型
    if (pTrackingTarget->nObjTypeGray == GLB_OBJ_GRAY_BRIGHT)
    {
        m_GrayMode = GrayType::BrightTarget;
    }
    else if (pTrackingTarget->nObjTypeGray == GLB_OBJ_GRAY_DARK)
    {
        m_GrayMode = GrayType::DarkTarget;
    }

    //初始化跟踪输出的目标状态
    OBJECTSTATUS* pObjStatus = &m_ObjStatus;
    pObjStatus->bObjAssigned = true;

    pObjStatus->unTotalCnt = 1;
    pObjStatus->unTrackedCnt = 1;
    pObjStatus->unContiTrackedCnt = 1;
    pObjStatus->unContiLostCnt = 0;
    pObjStatus->bObjMiss = false;

    // 

    pObjStatus->ptPosPre.x = m_LockingPipe->ptCurrentPnt.x;
    pObjStatus->ptPosPre.y = m_LockingPipe->ptCurrentPnt.y;
    pObjStatus->ptPos.x = pObjStatus->ptPosPre.x;
    pObjStatus->ptPos.y = pObjStatus->ptPosPre.y;

    pObjStatus->ptPosFilter.x = pObjStatus->ptPosPre.x;
    pObjStatus->ptPosFilter.y = pObjStatus->ptPosPre.y;
    pObjStatus->ptPosBorn = pObjStatus->ptPos;
    pObjStatus->sfSize.w = pTrackingTarget->snSize.w;
    pObjStatus->sfSize.h = pTrackingTarget->snSize.h;
    pObjStatus->sfSize.s = pObjStatus->sfSize.w * pObjStatus->sfSize.h;
    pObjStatus->sfSizeBorn = pObjStatus->sfSize;
    pObjStatus->fObjPxlsCnt = (FLOAT32)pTrackingTarget->unObjPxlsCnt;

    //更新目标速度、角速度、置信度
    pObjStatus->sfSpeed.vx = 0.0f;
    pObjStatus->sfSpeed.vy = 0.0f;
    pObjStatus->sfAglSpeed.vx = 0.0f;
    pObjStatus->sfAglSpeed.vy = 0.0f;
    pObjStatus->fConfidence = 1.0f;


    //更新目标角度
    Pole pole = getStablePoleFromImagePos(pTrackingTarget->pfCenPos,
        p_GLB_Input->stCamera, p_GLB_Input->servoInfo, p_GLB_Input->afPlatformRPY, p_GLB_Input->setupErr);

    pObjStatus->afAngle.fAz = (FLOAT32)pole.beta;
    pObjStatus->afAngle.fPt = (FLOAT32)pole.alpha;

    pObjStatus->afAngleBorn = pObjStatus->afAngle;

    // 表观模型初始化
    TO_CleanUpObjFeatures();
    TARGET_OBJECT* pLockObj = &pTrackingPipe->objHistoryList[pTrackingPipe->ubEnd];
    TO_RecordObjFeatures(pLockObj, p_GLB_Input);

    nSimStateLifeTime = MAX(p_GLB_Input->unFreq * 8, GLB_SIM_INFLUENCE_FRM);

	return true;
}

void SA_Tracker::GetTrackState(GLB_INPUT* p_GLB_Input)
{
    UINT32 unTotalCnt = m_LockingPipe->unTotalCnt;
    if (unTotalCnt < p_GLB_Input->unFreq * 2)
    {
        m_LockingPipe->ubEventStatus = PIPE_EVENT_JUST_LOCK;    // 锁定后3s范围内，处于初始锁定状态
    }
    else
    {
        m_LockingPipe->ubEventStatus = PIPE_EVENT_STALE_TRACK;
    }
}

int SA_Tracker::Track(GD_VIDEO_FRAME_S img, GLB_INPUT* p_GLB_Input, API_MOT_PIPE* g_GLB_PipeProc, SkyControlInfo* pSkyControlInfo, TSky_Output *m_TSky_Output)
{
    SINT32 nTrackTargetID1 = -1;
    SINT32 nTrackTargetID2 = -1;
    SINT32 nWidth = img.u32Width;
    SINT32 nHeight = img.u32Height;
    DAT_PARAMETERS* pDAT_stPara = pDAT_Module->GetDatParm();
    SINT32 nObjCombineDist = pDAT_stPara->nObjCombineDist;
    PIPE* m_PipeArray = g_GLB_PipeProc->getPipeArray();
    SINT32 nRealPipeNum = g_GLB_PipeProc->PIPE_GetAlarmNum();
    SINT32 m_nMaxPipeNum = g_GLB_PipeProc->PIPE_GetMaxPipeNum();
    SINT32 nPipeRadiusTrack = g_GLB_PipeProc->GetParam().nPipeRadiusTrack;
    SINT32 nPipeRaduisLost = g_GLB_PipeProc->GetParam().nPipeRadiusLost;
    PIPE_PARAMETERS MOT_PARA = g_GLB_PipeProc->GetParam();
    nTrackTargetID = -1;

    // 同步管道参数
    m_TSA_Param.nPipeRadiusTrack = nPipeRadiusTrack;
    m_TSA_Param.nPipeRadiusLost = nPipeRaduisLost;

    //调整搜索区域大小，为分块宽高的整数倍
    SetAutoSearchZone(nWidth, nHeight, p_GLB_Input);

    //确定跟踪目标状态
    GetTrackState(p_GLB_Input);

    if (!pSkyControlInfo->bTrackLowSkyline)
    {
        SARegionDet(img, p_GLB_Input, nPipeRadiusTrack, m_TSky_Output);
    }

    //////////////////////////////////////////////////////////////////////////
    ///////////////////// 相似目标处理 ///////////////////////////////////////
    ProcessSimInflunce(p_GLB_Input, &nObjCombineDist, m_TSky_Output);


    //////////////////////////////////////////////////////////////////////////
    ///////////////////// 管道匹配 ///////////////////////////////////////////
    bool bEnablebrid = 0;//g_GLB_PipeProc->GetParam().bOutMainPipeShadow;  //复用变量进行测试
    if (bEnablebrid)
    {
        if (!m_LockingPipe->blookout)
        {
            nTrackTargetID1 = FindMatchOneshot(m_LockingPipe, m_Target_Array, m_nTargetNum, m_TSA_Param.nPipeRadiusTrack, p_GLB_Input->stCamera.fAglReso);
        }
        else
        { 
            nTrackTargetID2 = FindMatchTracklet(nWidth, nHeight, m_LockingPipe, m_PipeArray, nRealPipeNum, m_nMaxPipeNum,&m_TSA_Param, p_GLB_Input->stCamera.fAglReso);
        }
        if (-1 != nTrackTargetID1 || -1 != nTrackTargetID2)
        {
            if (nTrackTargetID1 != -1)
            {
                memcpy(&m_MatchedTarget, &m_Target_Array[nTrackTargetID1], sizeof(TARGET_OBJECT));
            }
            else
            {
                memcpy(&m_MatchedTarget, &m_PipeArray[nTrackTargetID2].objHistoryList[m_PipeArray[nTrackTargetID2].ubEnd], sizeof(TARGET_OBJECT));
            }
        } 
        else
        {
            memset(&m_MatchedTarget, 0, sizeof(TARGET_OBJECT));
        }
    }
    else
    {
        if (GLB_VIDEO_TYPE::GLB_VIDEO_VL == p_GLB_Input->unVideoSrc)
        {
            // 第一次查找：在波门范围查找全图管道目标
            nTrackTargetID2 = FindMatchPipe(nWidth, nHeight, m_LockingPipe, m_PipeArray, nRealPipeNum, m_nMaxPipeNum,
                                            &m_TSA_Param, p_GLB_Input->stCamera.fAglReso);
            // 将搜索到的匹配目标输出到跟踪器，使用AI识别进行匹配关联对识别结果的同步性要求较高
            if (nTrackTargetID2 != -1)
            {
                memcpy(&m_MatchTarget2, &m_PipeArray[nTrackTargetID2].objHistoryList[m_PipeArray[nTrackTargetID2].ubEnd], sizeof(TARGET_OBJECT));
            } else
            {
                memset(&m_MatchTarget2, 0, sizeof(TARGET_OBJECT));
            }

            // 第二次查找：在波门范围查找区域小面检测目标
            nTrackTargetID1 = FindMatchTarget(m_LockingPipe, m_Target_Array, m_nTargetNum, m_TSA_Param.nPipeRadiusTrack, p_GLB_Input);
            if (-1 != nTrackTargetID1)
            {
                memcpy(&m_MatchTarget1, &m_Target_Array[nTrackTargetID1], sizeof(TARGET_OBJECT));
            }
            else
            {
                memset(&m_MatchTarget1, 0, sizeof(TARGET_OBJECT));
            }
        }
        else
        {
            // 第一次查找：在波门范围查找区域小面检测目标
            nTrackTargetID1 = FindMatchTarget(m_LockingPipe, m_Target_Array, m_nTargetNum, m_TSA_Param.nPipeRadiusTrack, p_GLB_Input);

            if (nTrackTargetID1 != -1)
            {
                memcpy(&m_MatchTarget1, &m_Target_Array[nTrackTargetID1], sizeof(TARGET_OBJECT));
            }
            else
            {
                memset(&m_MatchTarget1, 0, sizeof(TARGET_OBJECT));
            }
        
            // 第二次查找：在波门范围查找全图管道目标
            // 将搜索到的匹配目标输出到跟踪器，使用AI识别进行匹配关联对识别结果的同步性要求较高
            nTrackTargetID2 = FindMatchPipe(nWidth, nHeight, m_LockingPipe, m_PipeArray, nRealPipeNum, m_nMaxPipeNum,
                &m_TSA_Param, p_GLB_Input->stCamera.fAglReso);
            if (-1 != nTrackTargetID2)
            {
                memcpy(&m_MatchTarget2, &m_PipeArray[nTrackTargetID2].objHistoryList[m_PipeArray[nTrackTargetID2].ubEnd], sizeof(TARGET_OBJECT));
            }
            else
            {
                memset(&m_MatchTarget2, 0, sizeof(TARGET_OBJECT));
            }
        }

        //////////////////////////////////////////////////////////////////////////
        // 决策,判定当前是取小面匹配输出，还是取AI识别管道输出，还是使用AI识别对跟踪器重启
        if (-1 != nTrackTargetID1 && -1 != nTrackTargetID2)
        {//小面输出以及AI识别均有匹配
            if (TRUE == pSkyControlInfo->m_bMemoryTrackOcc)
            {
                m_MatchedTarget.pfCenPos = m_MatchTarget2.pfCenPos;
            }
            else
            {
                memcpy(&m_MatchedTarget, &m_MatchTarget1, sizeof(TARGET_OBJECT));
            }
            if (ObjSrc::Arith_AI == m_MatchTarget2.nObjTypeSrc)
            {
                m_MatchedTarget.fDetConf = m_MatchTarget2.fDetConf;
                m_MatchedTarget.unClsType = m_MatchTarget2.unClsType;
                m_MatchedTarget.emClsSrc = m_MatchTarget2.emClsSrc;
                m_MatchedTarget.snAIDetSize = m_MatchTarget2.snSize;
            }
            m_MatchedTarget.bObject = true;
            m_TSky_Output->findState = FindState::Finded_SA_AI;
        }
        else if (-1 != nTrackTargetID1)
        {//仅有小面匹配到目标
            memcpy(&m_MatchedTarget, &m_MatchTarget1, sizeof(TARGET_OBJECT));
            m_MatchedTarget.bObject = true;
            m_TSky_Output->findState = FindState::Finded_SA;
            // 基于过竿遮挡判断，强制记忆(退出干扰过慢的问题)
            if (TRUE == pSkyControlInfo->m_bMemoryTrackOcc)
            {
                m_MatchedTarget.bObject = false;
            }
        }
        else if (-1 != nTrackTargetID2)
        {//仅有AI识别匹配到目标，类别复制，统计计数进行位置修正
            if (ObjSrc::Arith_AI == m_MatchTarget2.nObjTypeSrc)
            {
                m_MatchedTarget.fDetConf = m_MatchTarget2.fDetConf;
                m_MatchedTarget.unClsType = m_MatchTarget2.unClsType;
                m_MatchedTarget.emClsSrc = m_MatchTarget2.emClsSrc;
                m_MatchedTarget.snAIDetSize = m_MatchTarget2.snSize;
                m_MatchedTarget.unObjPxlsCnt = m_MatchTarget2.snSize.s;

                // 低空目标小面不参与匹配，需要从AI匹配结果获取匹配尺寸
                if (pSkyControlInfo->bTrackLowSkyline)
                {
                    m_MatchedTarget.snSize = m_MatchTarget2.snSize;
                    m_MatchedTarget.mrnRect = m_MatchTarget2.mrnRect;
                }
            }
            m_MatchedTarget.pfCenPos = m_MatchTarget2.pfCenPos;
            m_MatchedTarget.bObject = true;
            m_TSky_Output->findState = FindState::Finded_AI;
        }
        else
        {//二者都未匹配到目标,标识目标未找到
            m_MatchedTarget.bObject = false;
            m_TSky_Output->findState = FindState::Finded_FAILED;
        }

    }
    // 将匹配目标的特征更新到跟踪器
    UpdateObject2Tracker(&m_MatchedTarget, p_GLB_Input, pSkyControlInfo->nTrackMemFrmNum);

    // 局部区域的单帧检测结果拷贝到输出
    m_TSA_output.m_nTargetNum = m_nTargetNum;
    memcpy(m_TSA_output.mTarget_Array, m_Target_Array, sizeof(TARGET_OBJECT)* m_nTargetNum);

    return 0;
}

RECT32S SA_Tracker::getTrackRect()
{
    RECT32S bbox = { 0 };
    bbox.x = SINT32(m_ObjStatus.ptPos.x - m_ObjStatus.sfSize.w / 2);
    bbox.y = SINT32(m_ObjStatus.ptPos.y - m_ObjStatus.sfSize.h / 2);
    bbox.w = (SINT32)m_ObjStatus.sfSize.w;
    bbox.h = (SINT32)m_ObjStatus.sfSize.h;

    return bbox;
}

SINT32 SA_Tracker::getInterfereAreaTargteNum(RECT32S bbox)
{
    SINT32 nNum = 0;
    SINT32 nX = bbox.x + bbox.w / 2;
    SINT32 nY = bbox.y + bbox.h / 2;
    for (size_t i = 0; i < pDAT_Module->GetTargetNum(); i++)
    {
       TARGET_OBJECT* pObj =  &pDAT_Module->GetTargetArray()[i];

       if (ABS(pObj->pfCenPos.x - nX) < MAX(50, bbox.w * 3) &&
           ABS(pObj->pfCenPos.y - nY) < MAX(50, bbox.h * 3))
       {
           nNum++;
       }

    }
    return nNum;
}

API_DetectSmallObj* SA_Tracker::getDSTmodule()
{
    return pDST_Module;
}




void SA_Tracker::SetAutoSearchZone(SINT32 nWidth, SINT32 nHeight, GLB_INPUT* p_GLB_Input)
{
#if 1
    SINT32  nEnd = 0;    //所跟踪管道的当前目标数组队列末尾
    SINT32  nPreStep = 1;    //管道队列中最后一次保存的目标，与当前帧的帧间隔
    POINT32F ptPrePoint = { 0,0 };//所跟踪管道的上一帧中心坐标
    CENTERRECT crCenterRect;        //搜索区域中心矩形
    MINMAXRECT mmCenterRect;        //搜索区域四角矩形

    POINT32F PipeCurrentPnt = { 0 };//跟踪管道当前帧位置预测
    PIPE* pPipe = m_LockingPipe;
    //上一帧跟踪结果
    OBJECTSTATUS* pObjStatus = &this->m_ObjStatus;
    POINT32F ptCurrentPnt = { 0 };  //波门参考中心点位置

    //SINT32 nSizePadRadius = (SINT32)(pObjStatus->sfSizeFilter.w + pObjStatus->sfSizeFilter.h) / 2;
    SINT32 nPipeRadiusTrack = m_TSA_Param.nPipeRadiusTrack;
    SINT32 nPipeRadiusLost = m_TSA_Param.nPipeRadiusLost;
    //根据目标速度预测目标在当前帧的坐标位置

    // 伺服太晃，直接使用短时预测。
    nEnd = m_LockingPipe->ubEnd;

    // 取长时预测点作为管道当前位置预测
    if (PIPE_EVENT_JUST_LOCK == m_LockingPipe->ubEventStatus)
    {
        ptCurrentPnt.x = pObjStatus->ptPos.x + pPipe->stMotionMod_mean.ObjAglListsNear.arfSpeed.afAngle.fAz / (p_GLB_Input->stCamera.fAglReso + FEPSILON);
        ptCurrentPnt.y = pObjStatus->ptPos.y + pPipe->stMotionMod_mean.ObjAglListsNear.arfSpeed.afAngle.fPt / (p_GLB_Input->stCamera.fAglReso + FEPSILON);
    }
    else
    {
        ptCurrentPnt.x = pPipe->ptCurrentPnt.x;
        ptCurrentPnt.y = pPipe->ptCurrentPnt.y;
    }


    //判断管道目标是否超出视场
    //if (IMGO_IsPoint16SOutImg(nWidth, nHeight, pPipe->ptCurrentPnt))
    if(IMGO_IsPoint32FOutImg(nWidth, nHeight, ptCurrentPnt))
    {
        pPipe->bInsideFOV = false;
        memset(&m_TSA_Input.crCenterRect, 0, sizeof(CENTERRECT));
        memset(&m_TSA_Input.mmCenterRect, 0, sizeof(MINMAXRECT));
        m_TSA_Input.nBlkWidth = 0;
        m_TSA_Input.nBlkHeight = 0;
        m_TSA_Input.nBlkNumW = 0;
        m_TSA_Input.nBlkNumH = 0;
        m_TSA_Input.nBlkNum = 0;
        return;
    }

    //+++++++++++++++++++++++++++++++++++++++++++++++++++++++
    //获取目标大小，调整分块大小
    //N = 1 : 16, W = fix((N + 1) / 2) * 4
    //N = 1  2  3  4   5   6   7   8
    //W = 4  4  8  8  12  12  16  16
    //N = 1 : 16, W = fix((N + 2) / 3) * 3
    //N = 1  2  3  4   5   6   7   8
    //W = 4  4  4  8   8   8   12  12
    SINT32 nObjW = (SINT32)pPipe->ObjectFilter.sfSize.w;
    SINT32 nObjH = (SINT32)pPipe->ObjectFilter.sfSize.h;
    SINT32 nObjSize = MAX(nObjW, nObjH);
    SINT32 nBlkWidth = MAX(4, ((nObjSize + 2) / 3) * 4);
    SINT32 nBlkHeight = nBlkWidth;

    //+++++++++++++++++++++++++++++++++++++++++++++++++++++++
    //获取目标等效像方速度
    FLOAT32 fObjVx = FLOAT32(fabs(pPipe->sfAglSpeed.vx) / p_GLB_Input->stCamera.fAglReso);
    FLOAT32 fObjVy = FLOAT32(fabs(pPipe->sfAglSpeed.vy) / p_GLB_Input->stCamera.fAglReso);
    FLOAT32 fObjSpeed = (FLOAT32)(sqrt((double)(fObjVx * fObjVx + fObjVy * fObjVy)) + 0.5f);

    //更新管道半径
    //20150416: 管道半径取管道速度的1.5~2倍，但以可调参数为下限
    SIZE32S snPipeRadius;
    if (pPipe->bLost)
    {
        snPipeRadius.w = MIN((SINT32)(fObjVx * pPipe->unLostCnt), nPipeRadiusLost);
        snPipeRadius.h = MIN((SINT32)(fObjVy * pPipe->unLostCnt), nPipeRadiusLost);
        snPipeRadius.w = MAX(snPipeRadius.w, nPipeRadiusTrack);
        snPipeRadius.h = MAX(snPipeRadius.h, nPipeRadiusTrack);
    }
    else
    {
        snPipeRadius.w = MAX((SINT32)(fObjVx * 2), nPipeRadiusTrack);
        snPipeRadius.h = MAX((SINT32)(fObjVy * 2), nPipeRadiusTrack);
    }

    //+++++++++++++++++++++++++++++++++++++++++++++++++++++++
    //根据管道半径，修正分块大小：
    //保证nBlkNumW<=20, nBlkNumH<=16，防止分块极值点坐标溢出，及保证计算量
    SIZE32S snPipeRange;
    snPipeRange.w = snPipeRadius.w * 2 + 1;
    snPipeRange.h = snPipeRadius.h * 2 + 1;
    snPipeRange.s = snPipeRange.w * snPipeRange.h;

    auto DST_BLK_NUM = nWidth/16 * nHeight/16;
    SINT32 nBlkWidthMin = MAX((SINT32)(sqrt(((double)snPipeRange.s) / DST_BLK_NUM) + 2), 2);
    nBlkWidth = MAX(nBlkWidthMin, MIN(nBlkWidth, 16));
    nBlkHeight = nBlkWidth;

    //+++++++++++++++++++++++++++++++++++++++++++++++++++++++
    //根据目标大小及速度，调整管道半径和分块大小
    //20150416: 管道半径取管道速度的1.5~2倍，但以可调参数为下限
    //20150514: 预测时扩大管道半径
    if (pPipe->bLost)
    {
        m_TSA_Param.nPipeRadius = (SINT32)(fObjSpeed * pPipe->unLostCnt);
    }
    else
    {
        m_TSA_Param.nPipeRadius = (SINT32)(fObjSpeed * 2.0f);
    }
    m_TSA_Param.nPipeRadius = MAX(nPipeRadiusTrack, MIN(m_TSA_Param.nPipeRadius, nPipeRadiusLost));
    m_TSA_Input.nBlkWidth = nBlkWidth;
    m_TSA_Input.nBlkHeight = nBlkHeight;
    m_TSA_Param.nObjCombineDist = (nBlkWidth >> 1);

    //+++++++++++++++++++++++++++++++++++++++++++++++++++++++
    //计算搜索区域中心矩形
    snPipeRange.w = (snPipeRange.w + nBlkWidth - 1) / nBlkWidth * nBlkWidth;
    snPipeRange.h = (snPipeRange.h + nBlkHeight - 1) / nBlkHeight * nBlkHeight;
    snPipeRadius.w = snPipeRange.w >> 1;
    snPipeRadius.h = snPipeRange.h >> 1;


    //+++++++++++++++++++++++++++++++++++++++++++++++++++++++
    //计算搜索区域左、右、上、下边界
    mmCenterRect.minX = ptCurrentPnt.x - snPipeRadius.w;
    mmCenterRect.maxX = ptCurrentPnt.x + snPipeRadius.w - 1;
    mmCenterRect.minY = ptCurrentPnt.y - snPipeRadius.h;
    mmCenterRect.maxY = ptCurrentPnt.y + snPipeRadius.h - 1;

    //20150513: 利用长短轨迹预测位置，调整当前帧目标预测位置
    //if (g_GLB_stOutput.bObjPredictAbnormal && g_GLB_stPara.bEnableServoTrackPredi)
    //{
    //    //计算长短轨迹预测位置外接矩形
    //    MINMAXRECT32S mrnObjTrackNear, mrnObjTrackLong, mrnObjTrackNLOuter;
    //    mrnObjTrackNear = g_GLB_stOutput.mrnObjPrediRtNear;
    //    mrnObjTrackLong = g_GLB_stOutput.mrnObjPrediRtLong;
    //    mrnObjTrackNLOuter.minX = MIN(mrnObjTrackNear.minX, mrnObjTrackLong.minX);
    //    mrnObjTrackNLOuter.maxX = MAX(mrnObjTrackNear.maxX, mrnObjTrackLong.maxX);
    //    mrnObjTrackNLOuter.minY = MIN(mrnObjTrackNear.minY, mrnObjTrackLong.minY);
    //    mrnObjTrackNLOuter.maxY = MAX(mrnObjTrackNear.maxY, mrnObjTrackLong.maxY);

    //    //更新搜索区域矩形
    //    mmCenterRect.minX = MIN(mmCenterRect.minX, mrnObjTrackNLOuter.minX);
    //    mmCenterRect.maxX = MAX(mmCenterRect.maxX, mrnObjTrackNLOuter.maxX);
    //    mmCenterRect.minY = MIN(mmCenterRect.minY, mrnObjTrackNLOuter.minY);
    //    mmCenterRect.maxY = MAX(mmCenterRect.maxY, mrnObjTrackNLOuter.maxY);

    //    //更新分块大小
    //    snPipeRange.w = mmCenterRect.maxX - mmCenterRect.minX + 1;
    //    snPipeRange.h = mmCenterRect.maxY - mmCenterRect.minY + 1;
    //    snPipeRange.s = snPipeRange.w * snPipeRange.h;
    //    nBlkWidthMin = MAX((SINT32)(sqrt(((double)snPipeRange.s) / DST_BLK_NUM) + 2), 2);
    //    nBlkWidth = MAX(nBlkWidthMin, MIN(nBlkWidth, DST_BLK_SIZE_W_16));
    //    nBlkHeight = nBlkWidth;
    //    g_TST_stInput.nBlkWidth = nBlkWidth;
    //    g_TST_stInput.nBlkHeight = nBlkHeight;
    //    g_TST_stInput.nObjCombineDist = (nBlkWidth >> 1) + 1;
    //}

    //搜索区域边界限制，防止超出图像边界
    mmCenterRect.minX = MAX(0, MIN(mmCenterRect.minX, nWidth - 1));
    mmCenterRect.maxX = MAX(0, MIN(mmCenterRect.maxX, nWidth - 1));
    mmCenterRect.minY = MAX(0, MIN(mmCenterRect.minY, nHeight - 1));
    mmCenterRect.maxY = MAX(0, MIN(mmCenterRect.maxY, nHeight - 1));
    m_TSA_Input.mmCenterRect = mmCenterRect;

    //20180123，根据搜索区域大小更新分块大小，避免搜索区域小时分块数量少，极值点过少
    snPipeRange.w = mmCenterRect.maxX - mmCenterRect.minX + 1;
    snPipeRange.h = mmCenterRect.maxY - mmCenterRect.minY + 1;
    snPipeRange.s = snPipeRange.w * snPipeRange.h;
    nBlkWidth = MAX((SINT32)(sqrt(((double)snPipeRange.s) / DST_BLK_NUM) + 2), 2);
    nBlkWidth = MAX(2, MIN(nBlkWidth, 16));
    nBlkHeight = nBlkWidth;
    m_TSA_Input.nBlkWidth = nBlkWidth;
    m_TSA_Input.nBlkHeight = nBlkHeight;
    m_TSA_Param.nObjCombineDist = (nBlkWidth >> 1) + 1;


    //计算搜索区域中心矩形
    crCenterRect.cx = (mmCenterRect.minX + mmCenterRect.maxX) >> 1;
    crCenterRect.cy = (mmCenterRect.minY + mmCenterRect.maxY) >> 1;
    crCenterRect.w = mmCenterRect.maxX - mmCenterRect.minX + 1;
    crCenterRect.h = mmCenterRect.maxY - mmCenterRect.minY + 1;


    crCenterRect.w = MIN(crCenterRect.w, 480);
    crCenterRect.h = MIN(crCenterRect.h, 480);

    crCenterRect.s = crCenterRect.w * crCenterRect.h;
    m_TSA_Input.crCenterRect = crCenterRect;

    //+++++++++++++++++++++++++++++++++++++++++++++++++++++++
    //计算分块个数  //20150424: 目标较小但记忆跟踪
    m_TSA_Input.nBlkNumH = m_TSA_Input.crCenterRect.h / m_TSA_Input.nBlkHeight;
    m_TSA_Input.nBlkNumW = m_TSA_Input.crCenterRect.w / m_TSA_Input.nBlkWidth;
    m_TSA_Input.nBlkNum = m_TSA_Input.nBlkNumH * m_TSA_Input.nBlkNumW;


#endif
}

void SA_Tracker::SARegionDet(GD_VIDEO_FRAME_S img, GLB_INPUT* p_GLB_Input, SINT32 nPipeRadiusTrack, TSky_Output* m_TSky_Output)
{
    SINT32 nWidth = img.u32Width;
    SINT32 nHeight = img.u32Height;
    DAT_PARAMETERS* pDAT_stPara = pDAT_Module->GetDatParm();
    SINT32 nObjCombineDist = pDAT_stPara->nObjCombineDist;
    FilterMeanNL stMotionMod_mean = m_LockingPipe->stMotionMod_mean;    // 使用运动模型
    TARGET_OBJECT* trackTarget = &m_LockingPipe->objHistoryList[m_LockingPipe->ubEnd];//跟踪最近的目标

    //////////////////////////////////////////////////////////////////////////
    //////////////////////////小面目标检测////////////////////////////////////
    BBOOL bEnableAreaObjDetect = FALSE;
    float fObjAglSpeed = ABS(m_LockingPipe->sfAglSpeed.vx) + ABS(m_LockingPipe->sfAglSpeed.vy);
    //// 像素数超过面目标检出下限或者速度较快，开启面目标检测算法
    //if ((m_ObjStatus.fObjPxlsCnt > DAT_TARGET_PXLS_MIN) || (fObjAglSpeed > 0.01f))
    //{
    //    bEnableAreaObjDetect = true;
    //}
    //else
    //{
    //    bEnableAreaObjDetect = false;
    //}
    bEnableAreaObjDetect = true;
    // 跟踪阶段，合并距离关键参数
    SINT32 nCombinDist = m_TSA_Input.nBlkWidth / 2 + 1;
    m_TSA_output.crCenterRect = m_TSA_Input.crCenterRect;
    // 小目标跟踪模式
    if (m_SizeMode == SizeType::SmallTarget)
    {
        // 小目标跟踪模块使用上面计算分块方案
        DST_INPUT trackInput = { 0 };
        trackInput.crCenterRect = m_TSA_Input.crCenterRect;
        trackInput.mmCenterRect = m_TSA_Input.mmCenterRect;
        trackInput.nBlkHeight = m_TSA_Input.nBlkHeight;
        trackInput.nBlkWidth = m_TSA_Input.nBlkWidth;
        trackInput.nBlkNumW = m_TSA_Input.nBlkNumW;
        trackInput.nBlkNumH = m_TSA_Input.nBlkNumH;
        trackInput.nBlkNum = m_TSA_Input.nBlkNum;
        pDST_Module->SetSearchBLK(trackInput);

        //查找分块极大值、统计当前帧目标
        //MSSu, 20150606: 根据分块大小，修改极值点合并距离
        //MSSu, 20150606: 小目标跟踪阶段，根据目标大小设置合并距离阈值

        // 跟踪阶段，强制缩小检测器合并距离提升分辨能力
        nCombinDist = (MAX(1, m_TSA_Input.nBlkWidth / 2 + 1), 9);

        pDST_Module->SetCombinDist(nCombinDist);

        // 首次挑选使用自适应SNR阈值，尽量避免低SNR阈值下虚警
        if (trackTarget->fSNR > 9)
        {
            pDST_Module->SetDetSnr(MAX(trackTarget->fSNR * 0.5, 4.0f));
        }

        SINT32 nSmallTargetNum = pDST_Module->Detect(img, m_TSA_Input.crCenterRect, GLB_STATUS_TRACK);
        // 小目标可以开面目标检测
        if (bEnableAreaObjDetect)
        {
            pDAT_Module->Detect(img, m_TSA_Input.crCenterRect, 0, GLB_STATUS_TRACK);
            m_TSA_output.crCenterRect = pDAT_Module->getDAT_stOutput()->crCenterRect;
        }
    }

    // 面目标跟踪模式
    if (m_SizeMode == SizeType::AreaTarget)
    {
        SINT32 nAreaTargetNum = pDAT_Module->Detect(img, m_TSA_Input.crCenterRect, nObjCombineDist, GLB_STATUS_TRACK);
        m_TSA_output.crCenterRect = pDAT_Module->getDAT_stOutput()->crCenterRect;
        m_TSky_Output->mTrakingPara_Output.nAreaCombineDist = nObjCombineDist;      // 面目标合并距离调试输出
    }

    // 将小目标队列拷贝进输出队列
    SINT32 nSmallTargetNum = pDST_Module->GetTargetNum();
    memcpy(m_Target_Array, pDST_Module->GetTargetArray(), nSmallTargetNum * sizeof(TARGET_OBJECT));

    //拷贝面目标检测队列，截断拷贝
    SINT32 nCpNum = MIN(DT_TARGET_MAX_NUM - nSmallTargetNum, pDAT_Module->GetTargetNum());
    memcpy(&m_Target_Array[nSmallTargetNum], pDAT_Module->GetTargetArray(), nCpNum * sizeof(TARGET_OBJECT));

    //小、面目标原地合并
    m_nTargetNum = MergeSmallAndAreaTarget(m_Target_Array, nSmallTargetNum, nCpNum, nCombinDist, GLB_STATUS::GLB_STATUS_TRACK);
    // 小面目标检测信息使用后重置，为了提高效率这里只重置个数
    //pDST_Module->SetTargetNum(0);
    //pDAT_Module->SetTargetNum(0);


    //////////////////////////////////////////////////////////////////////////
    /////////////////////////执行位移强限制///////////////////////////////////
    SINT32 nInvalidTargetCnt = 0;	//单帧小目标检测的无效目标，指不满足帧间位移强逻辑的目标
    if (m_nTargetNum > 0)
    {
        //遍历单帧检测到的目标，进行位移强逻辑判断
        for (int i = 0; i < m_nTargetNum; i++)
        {
            OBJECTSTATUS* pObjStatus = &m_ObjStatus;

            FLOAT32 fAzSpeed = pObjStatus->sfAglSpeed.vx / p_GLB_Input->stCamera.fAglReso; //目标运动的方位角速度(单位：像素)
            FLOAT32 fPtSpeed = pObjStatus->sfAglSpeed.vy / p_GLB_Input->stCamera.fAglReso; //目标运动的方位角速度(单位：像素)
            //强逻辑限制原则应该为限制不可能或极小概率事件，因此，阈值不应设置的过于严格，取所有可能性的上限
            int nPredictDiffThres = MAX(20, nPipeRadiusTrack);
            int nLastDiffThres = MAX(12, nPipeRadiusTrack);
            //指向当前目标
            TARGET_OBJECT* ptTarget = &m_Target_Array[i];
            //20161130，用预测位置位置，防止伺服或者炮塔突然抖动频繁进入记忆跟踪
            //20161201，用上一帧位置，防止图像平稳过程中因为目标变速用预测位置频繁进记忆跟踪
            if ((m_ObjStatus.unContiLostCnt < 50) &&
                (ABS(ptTarget->pfCenPos.x - nWidth / 2) < 50 && ABS(ptTarget->pfCenPos.y - nHeight / 2) < 50)//目标在视场边缘跟踪时伺服通常比较快，容易跟丢
                // 修改防跳动的阈值，管道丢失几帧时也不能跳，原来条件太严，无法解决晃动后丢失被噪点干扰的问题  
                && (ABS(ptTarget->pfCenPos.x - stMotionMod_mean.crnObjPrediRtLong.cx) > MAX(ABS(fAzSpeed) * 3, nPredictDiffThres)
                    || ABS(ptTarget->pfCenPos.y - stMotionMod_mean.crnObjPrediRtLong.cy) > MAX(ABS(fPtSpeed) * 3, nPredictDiffThres))
                && (ABS(ptTarget->pfCenPos.x - pObjStatus->ptPos.x) > MAX(ABS(fAzSpeed) * 2, nLastDiffThres)
                    || ABS(ptTarget->pfCenPos.y - pObjStatus->ptPos.y) > MAX(ABS(fPtSpeed) * 2, nLastDiffThres)))
            {
                nInvalidTargetCnt++;
            }
        }

        //若所有目标均不满足强逻辑，清空检测结果
        if (nInvalidTargetCnt == m_nTargetNum && PIPE_EVENT_JUST_LOCK != m_LockingPipe->ubEventStatus)
        {
            memset(m_Target_Array, 0, sizeof(TARGET_OBJECT) * m_nTargetNum);
            m_nTargetNum = 0;
        }
    }

    // 对空小目标进入二次检测跟踪
    if ((0 == m_nTargetNum)
        && (m_TSA_Param.bEnableSecDetect || m_SizeMode == SizeType::SmallTarget) /*&& (g_GLB_stPara->nWorkScene == GLB_WATCH_SKY)*/)//对空模式下才使用二次检测
    {
        //MSSu, 20150415: 小目标跟踪管道处理时，二次检测gdk阈值下降(但不能低于3)，并关闭部分去虚警条件
        TARGET_OBJECT tTargetRe = { 0 };
        BBOOL bPipeReObjFound = pDST_Module->DST_PipeTargetReDetect(img, nWidth, nHeight,
            m_LockingPipe->ptCurrentPnt, m_LockingPipe->sfAglSpeed,
            m_TSA_Input.mmCenterRect,
            &tTargetRe, &m_LockingPipe->nInTargetID_1, m_Target_Array, &m_nTargetNum);

        // 还没有检出，执行第三次超级弱小目标检测
        if (!bPipeReObjFound)
        {
            TARGET_OBJECT tTargetSe = { 0 };
            BBOOL bPipeSeObjFound = pDST_Module->DST_PipeTargetRReDetect(img, nWidth, nHeight,
                m_LockingPipe->ptCurrentPnt, m_LockingPipe->sfAglSpeed,
                m_TSA_Input.mmCenterRect,
                &tTargetSe, &m_LockingPipe->nInTargetID_1, m_Target_Array, &m_nTargetNum);
            //int a = 0;
        }
    }
    //////////////////////////////////////////////////////////////////////////
    ////////////////补全目标信息，如帧编号、角度、检测时间等./////////////////
    for (int i = 0; i < m_nTargetNum; i++)
    {
        TARGET_OBJECT* ptTarget = &m_Target_Array[i];
        // 帧编号
        ptTarget->unFrmID = p_GLB_Input->unFrmId;
        // 计算目标的方位角、俯仰角
        //// 转惯性系
        Pole poletmp = getStablePoleFromImagePos(ptTarget->pfCenPos, p_GLB_Input->stCamera, p_GLB_Input->servoInfo, p_GLB_Input->afPlatformRPY, p_GLB_Input->setupErr);

        // 目标角度赋值为大地坐标系
        ptTarget->afAngle.fAz = (FLOAT32)poletmp.beta;
        ptTarget->afAngle.fPt = (FLOAT32)poletmp.alpha;
    }
}


void SA_Tracker::ProcessSimInflunce(GLB_INPUT* p_GLB_Input, SINT32 *nObjCombineDist, TSky_Output* m_TSky_Output)
{
    m_TSky_Output->unSimTargetNum = m_LockingPipe->unSimTargetNum;
    //2变成0出现次数小于3，持续判断
    if (CountSim2To0 < 3)
    {
        CountSim2To0 = count2To0Transitions(nSimTargetNum_Counter, GLB_SIMOBJ_VALID_CNT);
    }
    //2变成0，0持续小于6帧，持续判断
    if (FALSE == bSim2To0_T)
    {
        bSim2To0_T = checkXToYCondition(nSimTargetNum_Counter, GLB_SIMOBJ_VALID_CNT, 2, 0, 6);
    }

    //上述条件满足，说明相似目标可能被错误合并，收缩合并距离;同时关闭AI信息使用，AI在具有相似目标存在时会导致关联出错
    if (CountSim2To0 > 3 || TRUE == bSim2To0_T)
    {
        *nObjCombineDist = 2;

        m_TSA_Param.Sky_bUseAIDet = FALSE;

        nSimStateLifeTime--;      // 生命周期递减

        m_TSky_Output->m_bSimInflunce = TRUE;
        //检测退出条件
        BBOOL bSim2To1 = checkXToYCondition(nSimTargetNum_Counter, GLB_SIMOBJ_VALID_CNT, 2, 1, 6);      // 2变成1，连续出现6次
        SINT32 count2Freq = countNumberFreq(nSimTargetNum_Counter, GLB_SIMOBJ_VALID_CNT, 2);            // 2出现次数为0
        // 两个检测条件，1个生命周期条件，退出相似干扰状态
        if (TRUE == bSim2To1 || 0 == count2Freq || nSimStateLifeTime <= 0)
        {
            CountSim2To0 = 0;
            bSim2To0_T = FALSE;
            m_TSA_Param.Sky_bUseAIDet = TRUE;
            m_TSky_Output->m_bSimInflunce = FALSE;
            nSimStateLifeTime = MAX(p_GLB_Input->unFreq * 8, GLB_SIM_INFLUENCE_FRM);      // 生命周期重置
        }
    }
}



void SA_Tracker::SetTrackModeAuto()
{
    // 更新目标尺度类型
    //BBOOL bEnableAreaObjDetect = false;     //单独定义面目标检测开关
    //if ((g_GLB_stOutput.ObjectStatus.fObjPxlsCnt > DAT_TARGET_PXLS_MIN)
    //    || (fObjAglSpeed > 0.01f))
    //{
    //    bEnableAreaObjDetect = true;
    //}
    //else
    //{
    //    bEnableAreaObjDetect = false;
    //}


    // 更新目标灰度类型




}

FLOAT32 pdf(FLOAT32 fmean, FLOAT32 fstd, FLOAT32 x)
{
    FLOAT32 f = FLOAT32(exp(-(x - fmean) * (x - fmean) / (2 * fstd * fstd)));
    return f;
}


#if 1
// 表观相似度计算
FLOAT32 SA_Tracker::Similarity_Apparent(PIPE* pPipe, TARGET_OBJECT* ptTarget)
{
    FLOAT32 fSimilarity = 0.0f;
    // 取跟踪特性监控
    MonitorStatus* Monitor = &g_GLB_MonitorStatus;

    if (ObjSrc::Arith_AI == pPipe->nObjTypeSrc || ObjSrc::Arith_AI == ptTarget->nObjTypeSrc)
    {// 有来自AI的框，都使用IOU进行匹配(处理AI与传统框尺寸衔接问题)
        RECT32S PipeBox;
        PipeBox.w = pPipe->objHistoryList[pPipe->ubEnd].snSize.w;
        PipeBox.h = pPipe->objHistoryList[pPipe->ubEnd].snSize.h;
        PipeBox.x = pPipe->objHistoryList[pPipe->ubEnd].pfCenPos.x - PipeBox.w / 2;
        PipeBox.y = pPipe->objHistoryList[pPipe->ubEnd].pfCenPos.y - PipeBox.h / 2;

        RECT32S ptTargetBox;
        ptTargetBox.w = ptTarget->snSize.w;
        ptTargetBox.h = ptTarget->snSize.h;
        ptTargetBox.x = ptTarget->pfCenPos.x - ptTarget->snSize.w / 2;
        ptTargetBox.y = ptTarget->pfCenPos.y - ptTarget->snSize.h / 2;

        fSimilarity = IoUA32S(&ptTargetBox, &PipeBox);
    }
    else
    {
        if (ptTarget->nObjTypeSrc == ObjSrc::Arith_DST || ptTarget->nObjTypeSrc == ObjSrc::Arith_DAT)
        {
            // 融合权重，跟踪前期无先验，使用手工权值
            if (m_SizeMode == AreaTarget)
            {
                Monitor->fGrayWeight = 0.4f; //灰度权重
                Monitor->fPxlsWeight = 0.3f;//尺寸权重
                Monitor->fWHRWeight = 0.3f;//宽高比权重
                Monitor->fSNRWeight = 0.0f;//信噪比权重
                Monitor->fBKGMeanWeight = 0.0f;//背景特征权重
            }
            else
            {
                Monitor->fGrayWeight = 0.3f; //灰度权重
                Monitor->fPxlsWeight = 0.2f;//尺寸权重
                Monitor->fWHRWeight = 0.0f;//宽高比权重
                Monitor->fSNRWeight = 0.3f;//信噪比权重
                Monitor->fBKGMeanWeight = 0.2f;//背景特征权重
            }
        }
        else
        {
            Monitor->fPxlsWeight = 0.5f;//尺寸权重
            Monitor->fWHRWeight = 0.5f;//宽高比权重
            //Monitor->fConfWeight = 0.2;//置信度权重
        }
        //// 稳跟后根据波动情况自适应权重
        //if (Monitor->bMonitorVaildFlag)
        //{
        //    FLOAT32 fGrayVariation = Monitor->TargetGray_sta.fMean / (Monitor->TargetGray_sta.fStd);
        //    FLOAT32 fWHRatioVariation = Monitor->TargetWHRatio_sta.fMean / Monitor->TargetWHRatio_sta.fStd;
        //    FLOAT32 fPxlsVariation = Monitor->TargetPxls_sta.fMean / Monitor->TargetPxls_sta.fStd;
        //    FLOAT32 fSNRVariation = Monitor->TargetSNR_sta.fMean / Monitor->TargetSNR_sta.fStd;
        //    FLOAT32 fBKGVariation = Monitor->TargetBKGMean_sta.fMean / Monitor->TargetBKGMean_sta.fStd;


        //    FLOAT32 VariationSum = fGrayVariation + fWHRatioVariation + fPxlsVariation + fSNRVariation + fBKGVariation;

        //    // 权值计算
        //    FLOAT32 fGrayWeight = fGrayVariation / VariationSum;
        //    FLOAT32 fWHRatioWeight = fWHRatioVariation / VariationSum;
        //    FLOAT32 fPxlsWeight = fPxlsVariation / VariationSum;
        //    FLOAT32 fSNRWeight = fSNRVariation / VariationSum;
        //    FLOAT32 fBKGWeight = fBKGVariation / VariationSum;


        //    // 保存当前帧权值
        //    Monitor->fGrayWeight = fGrayWeight;
        //    Monitor->fWHRWeight = fWHRatioWeight;
        //    Monitor->fPxlsWeight = fPxlsWeight;
        //    Monitor->fSNRWeight = fSNRWeight;//信噪比权重
        //    Monitor->fBKGMeanWeight = fBKGWeight;//背景特征权重
        //}

        //FLOAT32 all = Monitor->fGrayWeight + Monitor->fSNRWeight + Monitor->fBKGMeanWeight + Monitor->fWHRWeight + Monitor->fPxlsWeight;


        // 开始计算相似度
        if (ptTarget->nObjTypeSrc == ObjSrc::Arith_DST || ptTarget->nObjTypeSrc == ObjSrc::Arith_DAT)
        {
            FLOAT32 fStd_fixGrayModel = MAX(Monitor->TargetGray_sta.fMax - Monitor->TargetGray_sta.fMin, 300.0f) / 3;
            FLOAT32 fPdf_gray = pdf(Monitor->TargetGray_sta.fMean, fStd_fixGrayModel, ptTarget->pxObjGray);

            FLOAT32 fStd_fixWHModel = MAX(Monitor->TargetWHRatio_sta.fMax - Monitor->TargetWHRatio_sta.fMin, 1.0f) / 3;//小目标宽高比不稳定，给1的跳动值
            FLOAT32 fPdf_WH = pdf(Monitor->TargetWHRatio_sta.fMean, fStd_fixWHModel, ptTarget->snSize.w * 1.0f / ptTarget->snSize.h);

            FLOAT32 fStd_fixPxlModel = MAX(Monitor->TargetPxls_sta.fMax - Monitor->TargetPxls_sta.fMin, Monitor->TargetPxls_sta.fMean * 0.3f) / 3.0f;
            FLOAT32 fPdf_Pxl = pdf(Monitor->TargetPxls_sta.fMean, fStd_fixPxlModel, (FLOAT32)ptTarget->unObjPxlsCnt);

            FLOAT32 fStd_fixSNRModel = MAX(Monitor->TargetSNR_sta.fMax - Monitor->TargetSNR_sta.fMin, Monitor->TargetSNR_sta.fMean * 0.5f) / 3.0f;
            FLOAT32 fPdf_Snr = pdf(Monitor->TargetSNR_sta.fMean, fStd_fixSNRModel, ptTarget->fSNR);

            FLOAT32 fStd_fixBKGMeanModel = MAX(Monitor->TargetBKGMean_sta.fMax - Monitor->TargetBKGMean_sta.fMin, 100.0f) / 3.0f;
            FLOAT32 fPdf_BKGMean = pdf(Monitor->TargetBKGMean_sta.fMean, fStd_fixSNRModel, ptTarget->fBGMean);


            fSimilarity = fPdf_gray * Monitor->fGrayWeight + fPdf_WH * Monitor->fWHRWeight +
                fPdf_Pxl * Monitor->fPxlsWeight + fPdf_Snr * Monitor->fSNRWeight + fPdf_BKGMean * Monitor->fBKGMeanWeight;
        }
        else
        {
            FLOAT32 fStd_fixWHModel = MAX(Monitor->TargetWHRatio_sta.fMax - Monitor->TargetWHRatio_sta.fMin, 1.0f) / 3;//小目标宽高比不稳定，给1的跳动值
            FLOAT32 fPdf_WH = pdf(Monitor->TargetWHRatio_sta.fMean, fStd_fixWHModel, ptTarget->snSize.w * 1.0f / ptTarget->snSize.h);

            FLOAT32 fStd_fixPxlModel = MAX(Monitor->TargetPxls_sta.fMax - Monitor->TargetPxls_sta.fMin, Monitor->TargetPxls_sta.fMean * 0.3f) / 3.0f;
            FLOAT32 fPdf_Pxl = pdf(Monitor->TargetPxls_sta.fMean, fStd_fixPxlModel, (FLOAT32)ptTarget->unObjPxlsCnt);

            FLOAT32 fStd_fixConfModel = MAX(Monitor->TargetConfRatio_sta.fMax - Monitor->TargetConfRatio_sta.fMin, 1.0f) / 3;

            fSimilarity = fPdf_WH * Monitor->fWHRWeight + fPdf_Pxl * Monitor->fPxlsWeight;
        }
    }


    return fSimilarity;
}


// 方向余弦相似度
FLOAT32  SA_Tracker::cos_sim(POINT32F v1, POINT32F v2)
{
    FLOAT32 fcosSim = FLOAT32((v1.x * v2.x + v1.y * v2.y) / sqrt((v1.x * v1.x + v1.y * v1.y) * (v2.x * v2.x + v2.y * v2.y)));
    return (fcosSim + 1) / 2.0f;
}

void SA_Tracker::SetParam(Param_SkyDetect para)
{
    pDST_Module->setDstDetState(para.bEnableDetcetSmallTarget);       // 小目标检测开关设置
    pDST_Module->setDstParm(&para);                     // 小目标检测参数设置

    pDAT_Module->setDatDetState(para.bEnableDetcetAreaTarget);       // 面目标检测开关设置
    pDAT_Module->setDatParm(&para);                     // 面目标检测参数设置
}

// 运动相似度
FLOAT32 SA_Tracker::Similarity_Move(PIPE* pPipe, TARGET_OBJECT* ptTarget, FLOAT32 fAglReso)
{
    // 当前帧预测位置
    CENTERRECT LongPredictPos = pPipe->stMotionMod_mean.crnObjPrediRtLong;
    CENTERRECT ShortPredictPos = pPipe->stMotionMod_mean.crnObjPrediRtNear;

    // 目标大小
    SINT32 nTargetSize = SINT32(pPipe->ObjectFilter.sfSize.w * pPipe->ObjectFilter.sfSize.h);

    // 位置惩罚区间,根据目标大小和速度，类似波门取值方法



    // 长时速度
    SPEED32F fSpeedLong;
    SPEED32F fAglSpeedLong = pPipe->sfAglSpeed;
    fSpeedLong.vx = fAglSpeedLong.vx / fAglReso;
    fSpeedLong.vy = fAglSpeedLong.vy / fAglReso;

    // 考虑到测量误差，误差在一定小范围内得分一致为1
    //POINT16S ptLong = m_LockingPipe->ptCurrentPnt_Long;
    //POINT16S ptNear = m_LockingPipe->ptCurrentPnt_Near;
    //SINT32 nPipeRadiusTrack = m_TSA_Param.nPipeRadiusTrack;
    SINT32 nPipeRadiusLost = m_TSA_Param.nPipeRadiusLost;

    SIZE16S padSize = { 0 };


    padSize.w = MAX(ABS(fSpeedLong.vx * 3) + pPipe->objHistoryList[pPipe->ubEnd].snSize.w, 20);
    padSize.h = MAX(ABS(fSpeedLong.vy * 3) + pPipe->objHistoryList[pPipe->ubEnd].snSize.h, 20);

    SINT32 nPipeRadiusTrack = 50 + MAX(pPipe->ObjectFilter.sfSize.w, pPipe->ObjectFilter.sfSize.h);

    if (pPipe->bLost)
    {
        padSize.w = MIN(MAX(nPipeRadiusTrack, ABS(fSpeedLong.vx * pPipe->unLostCnt * 0.5)), nPipeRadiusLost);
        padSize.h = MIN(MAX(nPipeRadiusTrack, ABS(fSpeedLong.vy * pPipe->unLostCnt * 0.5)), nPipeRadiusLost);
    }
    FLOAT32 pX = MIN(ABS(ptTarget->pfCenPos.x - LongPredictPos.cx), ABS(ptTarget->pfCenPos.x - ShortPredictPos.cx));
    FLOAT32 pY = MIN(ABS(ptTarget->pfCenPos.y - LongPredictPos.cy), ABS(ptTarget->pfCenPos.y - ShortPredictPos.cy));
    FLOAT32 pDistScore = 1 - MIN(1, sqrt(pX * pX + pY * pY) / MAX(padSize.w, padSize.h));



    //if (pPipe->bLost)
    //{
    //    padSize.w = nPipeRadiusTrack;
    //    padSize.h = nPipeRadiusTrack;
    //}
    //else
    //{
    //    padSize.w = SINT16(MAX(fSpeedLong.vx * 5, MAX(sqrt(nTargetSize) * 3, nPipeRadiusTrack * 0.3)));
    //    padSize.h = SINT16(MAX(fSpeedLong.vy * 5, MAX(sqrt(nTargetSize) * 3, nPipeRadiusTrack * 0.3)));
    //}



    //// 计算距离得分
    //FLOAT32 pX = MIN(1, (ABS(ptTarget->pfCenPos.x - predictPos.x) * 2 / padSize.w));
    //FLOAT32 pY = MIN(1, (ABS(ptTarget->pfCenPos.y - predictPos.y) * 2 / padSize.h));
    //// 误差经验值修正:8个像素内包含测量误差以及行车抖动误差，加入误差容忍系数，提高得分平滑性。
    //if (ABS(ptTarget->pfCenPos.x - ptNear.x) < 8)
    //{
    //    pX *= 0.2f;
    //}
    //if (ABS(ptTarget->pfCenPos.y - ptNear.y) < 8)
    //{
    //    pY *= 0.2f;
    //}
    //// 位置综合得分
    //FLOAT32 pDistScore = (1 - pX) * (1 - pY);

    // 计算方向得分，以长时滤波器为准
    // 目标运动不明显，直接取位置得分
    if (ABS(fSpeedLong.vx) < 0.1 && ABS(fSpeedLong.vy) < 0.1)
    {
        return pDistScore;
    }

    POINT32F v1 = { 0 }; POINT32F v2 = { 0 };
    v1.x = fSpeedLong.vx;
    v1.y = fSpeedLong.vy;

    SINT32 nStep = MIN(pPipe->unExistCnt, GLB_PIPE_DEPTH_MAX);
    SINT32 nFirstPos = (pPipe->ubEnd - nStep + 1 + GLB_PIPE_DEPTH_MAX) % GLB_PIPE_DEPTH_MAX;

    ANGLE32F firsAgl = pPipe->objHistoryList[nFirstPos].afAngle;


    v2.x = (ptTarget->afAngle.fAz - firsAgl.fAz) / fAglReso;
    v2.y = (ptTarget->afAngle.fPt - firsAgl.fPt) / fAglReso;
    FLOAT32 pDirScore = cos_sim(v1, v2);

    // 经验修正，如果有一个速度矢量模差异过大，表明方向相似度极低，反之如果都很小，则不需要计算cossim
    FLOAT32 norm_v2 = ABS(v2.x) + ABS(v2.y);
    FLOAT32 norm_v1 = ABS(v1.x) + ABS(v1.y);
    if (norm_v1 < 1 && norm_v2 < 1)
    {
        pDirScore = 1;
    }

    // 速度方向得分与位置得分加权，其中位置得分更具有代表性
    FLOAT32 score = pDirScore * 0.3f + pDistScore * 0.7f;


    return score;
}

// 运动相似度
FLOAT32 SA_Tracker::Similarity_Move(PIPE* pPipe, PIPE* pPipe_Temp, FLOAT32 fAglReso)
{
    // 当前帧预测位置
    CENTERRECT LongPredictPos = pPipe->stMotionMod_mean.crnObjPrediRtLong;
    CENTERRECT ShortPredictPos = pPipe->stMotionMod_mean.crnObjPrediRtNear;
    POINT32F predictPos = pPipe->ptCurrentPnt;
    // 目标大小
    SINT32 nTargetSize = SINT32(pPipe->ObjectFilter.sfSize.w * pPipe->ObjectFilter.sfSize.h);
    TARGET_OBJECT* ptTarget = &pPipe_Temp->objHistoryList[pPipe_Temp->ubEnd];
    // 位置惩罚区间,根据目标大小和速度，类似波门取值方法
    // 长时速度
    SPEED32F fSpeedLong;
    SPEED32F fAglSpeedLong = pPipe->sfAglSpeed;
    fSpeedLong.vx = fAglSpeedLong.vx / fAglReso;
    fSpeedLong.vy = fAglSpeedLong.vy / fAglReso;
    SINT32 nPipeRadiusLost = m_TSA_Param.nPipeRadiusLost;

    SIZE16S padSize = { 0 };


    padSize.w = MAX(ABS(fSpeedLong.vx * 3) + pPipe->objHistoryList[pPipe->ubEnd].snSize.w, 8);
    padSize.h = MAX(ABS(fSpeedLong.vy * 3) + pPipe->objHistoryList[pPipe->ubEnd].snSize.h, 8);

    SINT32 nPipeRadiusTrack = 10 + MAX(pPipe->ObjectFilter.sfSize.w, pPipe->ObjectFilter.sfSize.h);

    if (pPipe->bLost)
    {
        padSize.w = MIN(MAX(nPipeRadiusTrack, ABS(fSpeedLong.vx * pPipe->unLostCnt * 0.5)), nPipeRadiusLost);
        padSize.h = MIN(MAX(nPipeRadiusTrack, ABS(fSpeedLong.vy * pPipe->unLostCnt * 0.5)), nPipeRadiusLost);
    }
    FLOAT32 pX = MIN(ABS(ptTarget->pfCenPos.x - LongPredictPos.cx), ABS(ptTarget->pfCenPos.x - ShortPredictPos.cx));
    FLOAT32 pY = MIN(ABS(ptTarget->pfCenPos.y - LongPredictPos.cy), ABS(ptTarget->pfCenPos.y - ShortPredictPos.cy));
    FLOAT32 pDistScore = 1 - MIN(1, sqrt(pX * pX + pY * pY) / MAX(padSize.w, padSize.h));

    // 计算方向得分，以长时滤波器为准
    // 目标运动不明显，直接取位置得分
    if (ABS(fSpeedLong.vx) < 0.1 && ABS(fSpeedLong.vy) < 0.1 || pDistScore<0.2)
    {
        return pDistScore;
    }

    POINT32F v1 = { 0 }; POINT32F v2 = { 0 };
    v1.x = fSpeedLong.vx;
    v1.y = fSpeedLong.vy;

    SINT32 nStep = MIN(pPipe->unExistCnt, GLB_PIPE_DEPTH_MAX);
    SINT32 nFirstPos = (pPipe->ubEnd - nStep + 1 + GLB_PIPE_DEPTH_MAX) % GLB_PIPE_DEPTH_MAX;

    ANGLE32F firsAgl = pPipe->objHistoryList[nFirstPos].afAngle;


    v2.x = (ptTarget->afAngle.fAz - firsAgl.fAz) / fAglReso;
    v2.y = (ptTarget->afAngle.fPt - firsAgl.fPt) / fAglReso;
    FLOAT32 pDirScore = cos_sim(v1, v2);

    // 经验修正，如果有一个速度矢量模差异过大，表明方向相似度极低，反之如果都很小，则不需要计算cossim
    //FLOAT32 norm_v2 = ABS(v2.x) + ABS(v2.y);
    //FLOAT32 norm_v1 = ABS(v1.x) + ABS(v1.y);
    //if (norm_v1 < 1 && norm_v2 < 1)
    //{
    //    pDirScore = 1;
    //}

    // 速度方向得分与位置得分加权，其中位置得分更具有代表性
    FLOAT32 score = pDirScore * 0.3f + pDistScore * 0.7f;


    return score;
}


// 基于特征加权融合的相似度计算，提取最优跟踪目标
SINT32 SA_Tracker::FindMatchTarget(PIPE* pPipe, TARGET_OBJECT* ptTargetArray, SINT32 nFrmObjsCnt, SINT32 nPipeRadius, GLB_INPUT* p_GLB_Input)
{
    //局部变量
    FLOAT32 fSim = 0.0f; //目标相似度
    FLOAT32 fSimMax = -1.0f;//目标相似度最大值
    FLOAT32 fSimThres = 0.5f; //相似度阈值
    SINT32  nFrmsStep = 1;    //帧间隔
    TARGET_OBJECT* ptMainTarget = NULL; //主管道目标
    TARGET_OBJECT* ptTarget = NULL; //候选目标
    BYTE8   nSimTargetId = -1;   //与管道差异最小的目标编号
    TARGET_OBJECT* ptTargetTempA = NULL;//临时目标A
    TARGET_OBJECT* ptTargetTempB = NULL;//临时目标B

    FLOAT32 fMoveModelWeight = 0.5;//运动模型权值
    FLOAT32 fApparentModelWeight = 0.5;//表观模型权值

    FLOAT32 fSScaleChangeLowThres = 0.1;        // 小目标尺寸变小阈值
    FLOAT32 fSScaleChangeHighThres = 12.f;      // 小目标尺寸变大阈值
    FLOAT32 fAScaleChangeLowThres = 0.25;       // 面目标尺寸变小阈值
    FLOAT32 fAScaleChangeHighThres = 4.f;       // 面目标尺寸变大阈值
    SINT32  nPredictDiffThresX = 8;            //  与预测位置像素差阈值
    SINT32  nPredictDiffThresY = 8;            //  与预测位置像素差阈值
    SINT32  nLastDiffThresX = 6;               //  与上一帧的像素差阈值
    SINT32  nLastDiffThresY = 6;               //  与上一帧的像素差阈值

    FLOAT32 fMoveThres = 0.5f;                  //运动相似度阈值  
    FLOAT32 fMoveSim_Target = 0.f;                  //候选目标运动相似度
    FLOAT32 fAglReso = p_GLB_Input->stCamera.fAglReso;

    pPipe->unSimTargetNum = 0;                 //清空相似目标个数
    
    //若当前帧目标个数为0，则直接返回查找失败标志(差异度最小目标管道编号为-1)
    if (0 == nFrmObjsCnt)
    {
        return -1;
    }


    // 小目标主要使用位置信息
    if (SizeType::SmallTarget == m_SizeMode)
    {
        fMoveModelWeight = 0.7f;
        fApparentModelWeight = 0.3f;
        fMoveThres = 0.7f;
    }
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++
    //若长短轨迹预测异常，则直接取距离搜索区域中心最近的目标
    if (m_LockingPipe->stMotionMod_mean.bObjPredictAbnormal
        && m_LockingPipe->unLostCnt > 2)
    {
        fMoveModelWeight = 1.0;
        fApparentModelWeight = 0.0;
    }

    // 使用运动模型
    FilterMeanNL stMotionMod_mean = m_LockingPipe->stMotionMod_mean;


    //锁定后的50帧优先使用位置信息，降低锁定后立即丢失
    //if(g_GLB_stOutput.ObjectStatus.unTotalCnt < 50)
    //{
    //    fMoveModelWeight = 1.0;
    //    fApparentModelWeight = 0.0;
    //}


    //统计所有当前帧目标与管道目标的灰度、与中心距离、信噪比差异
    for (int i = 0; i < nFrmObjsCnt; i++)
    {
        ptTarget = &ptTargetArray[i];
        if (!ptTarget->bObject)
        {
            continue;
        }

        // 计算表观相似度
        FLOAT32 fAppSim = Similarity_Apparent(pPipe, ptTarget);

        // 计算运动相似度
        FLOAT32 fMoveSim = Similarity_Move(pPipe, ptTarget, fAglReso);

        fSim = fAppSim * fApparentModelWeight + fMoveSim * fMoveModelWeight;

        ptTarget->fMatchConf = fSim;

        SINT32  nDSmpScale = pDAT_Module->GetDatParm()->nDSmpScale;
        //LSBAO, 20160705: 若尺寸宽高方向同时变化超过2个像素或者宽高单方向变化超过3个像素，则认为目标大小突变 -- 丢失超过1S不判断
        if (PIPE_EVENT_JUST_LOCK != m_LockingPipe->ubEventStatus)
        {
            if (((ABS(ptTarget->snSize.h - pPipe->ObjectFilter.sfSize.h) > 2 * nDSmpScale && ABS(ptTarget->snSize.w - pPipe->ObjectFilter.sfSize.w) > 2 * nDSmpScale) ||
                ABS(ptTarget->snSize.h - pPipe->ObjectFilter.sfSize.h) > 3 * nDSmpScale ||
                ABS(ptTarget->snSize.w - pPipe->ObjectFilter.sfSize.w) > 3 * nDSmpScale) && m_LockingPipe->unLostCnt < GLB_FRM_FREQ)
            {
                //continue;
            }
        }

        FLOAT32 fSimDis = sqrt(POW2(ptTarget->pfCenPos.x - pPipe->ptCurrentPnt.x)
             + POW2(ptTarget->pfCenPos.y - pPipe->ptCurrentPnt.y));
        // 加外观和距离评价相似目标
        if ((fSim > fSimThres && fMoveSim > fMoveThres) || 
            (fAppSim > 0.25 && fSimDis < 20 + MAX(pPipe->ObjectFilter.sfSize.w, pPipe->ObjectFilter.sfSize.h)))
        {
            pPipe->unSimTargetNum++;
        }

        if (fSim > fSimMax)
        {
            fSimMax = fSim;
            nSimTargetId = i;
            fMoveSim_Target = fMoveSim;
        }
    }
    //循环记录每帧相似目标个数
    nSimTargetNum_Counter[ubSimTarget_End] = pPipe->unSimTargetNum;
    ubSimTarget_End = (UBYTE8)((ubSimTarget_End + 1) % GLB_SIMOBJ_VALID_CNT);

    if (pPipe->unSimTargetNum > 1)
    {
        pPipe->blookout = TRUE;
    }
    // 单一目标条件判断：当只有一个目标时，关闭大部分条件尽力跟踪，比较符合人眼规律。
    // 由于跟踪时谨慎合并目标，很容易导致临界目标被DST和DAT同时检出，这样其实是一个目标，但数量上不满足单一目标条件，
    BBOOL bSingleTarget = (nFrmObjsCnt == 1);
    if (nFrmObjsCnt == 2)
    {
        TARGET_OBJECT* ptTarget1 = &ptTargetArray[0];
        TARGET_OBJECT* ptTarget2 = &ptTargetArray[1];

        if (ABS(ptTarget1->pfCenPos.x - ptTarget2->pfCenPos.x) <= 3 &&
            ABS(ptTarget1->pfCenPos.y - ptTarget2->pfCenPos.y) <= 3)
        {
            bSingleTarget = TRUE;
        }
    }

    // 焦距关联阈值(按照0.01度器件精度)
    SINT32 nFThreMin = 0.01 / fAglReso * 2;
    SINT32 nMoveMin = 0.01 / fAglReso * 4;
    

    // 阈值调整
    if (pPipe->unLostCnt < 2)
    {
        fSScaleChangeLowThres = 0.3;
        fSScaleChangeHighThres = 3.f;
        fAScaleChangeLowThres = 0.5;
        fAScaleChangeHighThres = 2.0;
        nLastDiffThresX = MAX(nMoveMin, 6);
        nLastDiffThresY = MAX(nMoveMin, 6);
        nPredictDiffThresX = MAX(nFThreMin, 6);
        nPredictDiffThresY = MAX(nFThreMin, 6);
    }
    else if (pPipe->unLostCnt < 20)
    {
        fSScaleChangeLowThres = 0.3;
        fSScaleChangeHighThres = 3.f;
        fAScaleChangeLowThres = 0.5;
        fAScaleChangeHighThres = 2.0;
        nLastDiffThresX = MAX(nMoveMin,50);
        nLastDiffThresY = MAX(nMoveMin, 50);
        nPredictDiffThresX = MAX(nFThreMin, 25);
        nPredictDiffThresY = MAX(nFThreMin, 25);
    }
    else if (pPipe->unLostCnt < 50)
    {
        fSScaleChangeLowThres = 0.2;
        fSScaleChangeHighThres = 5.f;
        fAScaleChangeLowThres = 0.33;
        fAScaleChangeHighThres = 3.0;
        nLastDiffThresX = MAX(nMoveMin, 70);
        nLastDiffThresY = MAX(nMoveMin, 70);
        nPredictDiffThresX = MAX(nFThreMin, 36);
        nPredictDiffThresY = MAX(nFThreMin, 36);
    }
    else if (pPipe->unLostCnt < 100)
    {
        fSScaleChangeLowThres = 0.15;
        fSScaleChangeHighThres = 8.f;
        fAScaleChangeLowThres = 0.33;
        fAScaleChangeHighThres = 3.0;
        nLastDiffThresX = MAX(nMoveMin, 120);
        nLastDiffThresY = MAX(nMoveMin, 120);
        nPredictDiffThresX = MAX(nFThreMin, 48);
        nPredictDiffThresY = MAX(nFThreMin, 48);
    }
    else
    {
        fSScaleChangeLowThres = 0.1;
        fSScaleChangeHighThres = 12.f;
        fAScaleChangeLowThres = 0.25;
        fAScaleChangeHighThres = 4.0;
        nLastDiffThresX = MAX(nMoveMin, 48);
        nLastDiffThresY = MAX(nMoveMin, 48);
        nPredictDiffThresX = MAX(nFThreMin, 60);
        nPredictDiffThresY = MAX(nFThreMin, 60);
    }


    //// 小目标特殊处理
    if (pPipe->ObjectFilter.fPxlsCnt < 9)
    {
        fSScaleChangeLowThres = 0.3;
        fSScaleChangeHighThres = 3.f;
        fAScaleChangeLowThres = 0.5;
        fAScaleChangeHighThres = 2.0;
        nLastDiffThresX = MAX(nMoveMin, 4);
        nLastDiffThresY = MAX(nMoveMin, 4);
        nPredictDiffThresX = MAX(nFThreMin, 6);
        nPredictDiffThresY = MAX(nFThreMin, 6);
    }


    SINT32 nEnd = pPipe->ubEnd;
    // 尺寸变化强逻辑防跳变//           by wcw04046 @ 2020/06/22
    if (nSimTargetId != -1 && m_ObjStatus.unContiLostCnt < 200)
    {
        FLOAT32 fSizeChange = 0.0f;
        FLOAT32 fPixChange = 0.0f;
        ptMainTarget = &pPipe->objHistoryList[nEnd];
        ptTarget = &ptTargetArray[nSimTargetId];

        fSizeChange = (FLOAT32)(ptTarget->snSize.s) / MAX(ptMainTarget->snSize.s, 0.001f);
        fPixChange = (FLOAT32)(ptTarget->unObjPxlsCnt) / MAX(ptMainTarget->unObjPxlsCnt, 0.001f);

        // 大于DAT_TARGET_MIN的目标可能是临界目标
        if (ptTarget->unObjPxlsCnt < 9 || ptMainTarget->unObjPxlsCnt < 9)
        {
            if ((ptTarget->unObjPxlsCnt == 1)
                && (fSScaleChangeHighThres < fPixChange || fSScaleChangeLowThres > fPixChange))
            {
                //fSimMax = -1;
            }
        }
        else if (fSScaleChangeHighThres < fSizeChange || fSScaleChangeLowThres > fSizeChange)//均为面目标的情形
        {
            fSimMax = -1;
        }
        else
        {

        }

        // 目标运动距离强逻辑：用预测位置和上一帧的位置，防止跟踪跳转到错误的目标
        if (nSimTargetId != -1)
        {
            FLOAT32 fAzSpeed = m_ObjStatus.sfAglSpeed.vx / fAglReso;
            FLOAT32 fPtSpeed = m_ObjStatus.sfAglSpeed.vy / fAglReso;

            //// 没得选了
            //if (m_ObjStatus.unContiLostCnt >= 3 && bSingleTarget)
            //{
            //    nPredictDiffThresX = MAX(70, nPipeRadius * 2);
            //    nPredictDiffThresY = MAX(70, nPipeRadius * 2);
            //    nLastDiffThresX = nPipeRadius * 2;//波门范围直接取
            //    nLastDiffThresY = nPipeRadius * 2;//波门范围直接取
            //}

            nPredictDiffThresX = MAX(ptTarget->snSize.w / 2, nPredictDiffThresX);
            nPredictDiffThresY = MAX(ptTarget->snSize.h / 2, nPredictDiffThresY);

            nLastDiffThresX = MAX(ptTarget->snSize.w / 2, nLastDiffThresX);
            nLastDiffThresY = MAX(ptTarget->snSize.h / 2, nLastDiffThresY);


            // 非初始锁定阶段执行运动距离强逻辑
            //小目标情况下，周围可能检测出干扰的相似目标（真正的目标无法检出），需要强逻辑限定
            //面目标情况下，单个目标允许大范围关联，关闭距离限定；非单个目标限定关联距离
            if (/*PIPE_EVENT_JUST_LOCK != m_LockingPipe->ubEventStatus && */
                m_LockingPipe->stMotionMod_mean.bTrackStable &&
                 m_SizeMode <= SizeType::SmallTarget || (m_SizeMode >= SizeType::MiddleTarget && !bSingleTarget))
            {
                if ((ABS(ptTarget->pfCenPos.x - stMotionMod_mean.crnObjPrediRtLong.cx) > MAX(ABS(fAzSpeed) * 2, nPredictDiffThresX)
                    || ABS(ptTarget->pfCenPos.y - stMotionMod_mean.crnObjPrediRtLong.cy) > MAX(ABS(fPtSpeed) * 2, nPredictDiffThresY))
                    && (ABS(ptTarget->pfCenPos.x - m_ObjStatus.ptPos.x) > MAX(ABS(fAzSpeed) * 2, nLastDiffThresX)
                        || ABS(ptTarget->pfCenPos.y - m_ObjStatus.ptPos.y) > MAX(ABS(fPtSpeed) * 2, nLastDiffThresY)))
                {
                    fSimMax = -1;
                }
            }
        }
    }
    ////+++++++++++++++++++++++++++++++++++++++++++++++++++++++
    //FLOAT32 ratioX = 0.f;
    //FLOAT32 ratioY = 0.f;
    //// 最值中心点变化强逻辑防跳变,不适合均匀面的目标//           by yll08400 @ 2024/02/10
    //if (nSimTargetId != -1 && SizeType::AreaTarget == m_SizeMode && GLB_VIDEO_TYPE::GLB_VIDEO_VL != p_GLB_Input->unVideoSrc)
    //{
    //    FLOAT32 leftXGap = ptTarget->pnMaxPos.x - ptTarget->mrnRect.minX;
    //    FLOAT32 rightXGap = ABS(ptTarget->pnMaxPos.x - ptTarget->mrnRect.maxX);
    //    FLOAT32 topYGap = ptTarget->pnMaxPos.y - ptTarget->mrnRect.minY;
    //    FLOAT32 bottomYGap = ABS(ptTarget->pnMaxPos.y - ptTarget->mrnRect.maxY);
    //    ratioX =  MAX(leftXGap, rightXGap) / (MIN(leftXGap, rightXGap) + 1.f);
    //    ratioY = MAX(topYGap, bottomYGap) / (MIN(topYGap, bottomYGap) + 1.f);
    //    if (ratioX + ratioY > 6)
    //    {
    //        fSimMax = -1;
    //    }
    //}

    //+++++++++++++++++++++++++++++++++++++++++++++++++++++++
    // 跟踪阈值动态调整
    FLOAT32 fThres = fSimThres;
    FLOAT32 ContiLostThres = 1.f - MIN(float(m_ObjStatus.unContiLostCnt) / GLB_FRM_FREQ * 2.0f, 0.5);     //丢失帧比例
    if (bSingleTarget)
    {
        fThres = fSimThres * ContiLostThres * 0.5;//降低底线的跟踪
        fMoveThres = fMoveThres * ContiLostThres * 0.5;
    }
    else
    {
        fThres = fSimThres * ContiLostThres;
        fMoveThres = fMoveThres * ContiLostThres;

        // 跟踪前10帧防掉
        if (m_ObjStatus.unTotalCnt < 10)
        {
            fThres = fSimThres * 0.1f; //没有底线的跟踪
        }

    }
    // 警戒周边干扰时，调整阈值
    if (pPipe->blookout)
    {
        fThres = 0.8 * ContiLostThres;
        fMoveThres = 0.8 * 1.f - ContiLostThres;
    }


    // 单一目标初始锁定，不做阈值限制
    if (bSingleTarget && PIPE_EVENT_JUST_LOCK == m_LockingPipe->ubEventStatus)
    {
        fThres = 0;
    }

    //若最大相似度不满足阈值，则认为未查找到管道目标，返回-1
    if (fSimMax < fThres /*||(SmallTarget == m_SizeMode && fMoveSim_Target < fMoveThres)*/)
    {
         nSimTargetId = -1;
    }

    if (-1 != nSimTargetId)
    {
        //记录管道目标跟踪相似度
        pPipe->fConfidence = fSimMax;
        pPipe->blookout = FALSE;
    }
    else
    {
        pPipe->fConfidence = -1;
    }
    return nSimTargetId;
}

// 基于特征加权融合的相似度计算，提取最优跟踪目标
SINT32 SA_Tracker::FindMatchPipe(SINT32 nWidth, SINT32 nHeight, PIPE* pLockingPipe, PIPE* m_PipeArray, SINT32 nRealPipeNum, 
                                    SINT32 nMaxPipeNum, TSA_Parameters* pTSA_Param, FLOAT32 fAglReso)
{
    //局部变量
    FLOAT32 fSim = 0.0f; //目标相似度
    FLOAT32 fSimMax = -1.0f;//目标相似度最大值
    FLOAT32 fSimThres = 0.5f; //相似度阈值
    FLOAT32 fIOUThres = 0.3f; //IOU拦截阈值
    SINT32  nFrmsStep = 1;    //帧间隔
    TARGET_OBJECT* ptLockingTarget = NULL; // 主目标管道指向的目标
    TARGET_OBJECT* pCandidateTarget = NULL; //管道数组指向的目标
    TARGET_OBJECT* pBestTarget = NULL; //管道数组指向的最佳目标

    BYTE8   nSimTargetId = -1;   //与管道差异最小的目标编号
    FLOAT32 targerIOU = 0.f;     //最佳目标的iou

    SINT32 nFrmObjsCnt = 0;     // 管道框在关联区域内目标计数
    FLOAT32 fMoveModelWeight = 0.7;//运动模型权值
    FLOAT32 fPositionModelWeight = 0.3;//位置权值

    FLOAT32 fSScaleChangeLowThres = 0.1;        // 小目标尺寸变小阈值
    FLOAT32 fSScaleChangeHighThres = 12.f;      // 小目标尺寸变大阈值
    FLOAT32 fAScaleChangeLowThres = 0.25;       // 面目标尺寸变小阈值
    FLOAT32 fAScaleChangeHighThres = 4.f;       // 面目标尺寸变大阈值

    SINT32 nPipeRadius = pTSA_Param->nPipeRadiusTrack;  // 管道关联范围

    FilterMeanNL stMotionMod_mean = m_LockingPipe->stMotionMod_mean;

    //若当前帧目标个数为0，则直接返回查找失败标志(差异度最小目标管道编号为-1)
    if (0 == nRealPipeNum || false == pTSA_Param->Sky_bUseAIDet)
    {
        return -1;
    }

    //基于频率控制AI识别使用(在能保证同步性的情况下，这里不做频率限制)
    if (nSAUseAIDetFeq > 1)
    {
        nSAUseAIDetFeq -= 1;
        return -1;
    }
    else
    {
        nSAUseAIDetFeq = pTSA_Param->nUseAIDetFeq;
    }

    // +++++++++++++++++++++++++++++++++++++++++++++++++++++
    //若长短轨迹预测异常，则直接取距离搜索区域中心最近的目标
    if (stMotionMod_mean.bObjPredictAbnormal
        && m_LockingPipe->unLostCnt > 2)
    {
        fMoveModelWeight = 1.0;
        fPositionModelWeight = 0.0;
    }

    //锁定后的50帧优先使用位置信息，降低锁定后立即丢失
    //if(g_GLB_stOutput.ObjectStatus.unTotalCnt < 50)
    //{
    //    fMoveModelWeight = 1.0;
    //    fApparentModelWeight = 0.0;
    //}

    RECT32S LockingPipeBox, newLockingPipeBox;
    FLOAT32 centX = pLockingPipe->objHistoryList[pLockingPipe->ubEnd].pfCenPos.x;
    FLOAT32 centY = pLockingPipe->objHistoryList[pLockingPipe->ubEnd].pfCenPos.y;

    LockingPipeBox.x = centX - nPipeRadius;
    LockingPipeBox.y = centY - nPipeRadius;
    LockingPipeBox.w = nPipeRadius;
    LockingPipeBox.h = nPipeRadius;
    limitInBounder(nWidth, nHeight, LockingPipeBox, &newLockingPipeBox);

    //统计所有当前帧目标与管道目标的灰度、与中心距离、信噪比差异
    for (int i = 0; i < nMaxPipeNum; i++)
    {
        //指向第i个管道
        PIPE* pPipe = &m_PipeArray[i];

        //跳过空管道
        if (!pPipe->bOccupy)
        {
            continue;
        }

        // 跳过跟踪管道处理
        if (pPipe->bTrackingPipe)
        {
            continue;
        }

        if (ObjSrc::Arith_AI != pPipe->nObjTypeSrc)
        {
            continue;
        }
        

        // 跳过丢失管道
        if (pPipe->bLost)
        {
            continue;
        }

        // 计算IOU,统计目标个数
        RECT32S PipeBox, newPipeBox;
        PipeBox.w = nPipeRadius;
        PipeBox.h = nPipeRadius;
        PipeBox.x = pPipe->objHistoryList[pPipe->ubEnd].pfCenPos.x - nPipeRadius;
        PipeBox.y = pPipe->objHistoryList[pPipe->ubEnd].pfCenPos.y - nPipeRadius;
        limitInBounder(nWidth, nHeight, PipeBox, &newPipeBox);

        FLOAT32 IOU = IoUA32S(&newLockingPipeBox, &newPipeBox);
        if (IOU > 0.1)
        {
            nFrmObjsCnt += 1;
        }

        pCandidateTarget = &pPipe->objHistoryList[pPipe->ubEnd];

        // 计算表观相似度(此时是AI识别框与传统跟踪的宽高尺寸特征比较，ToDo如果质心挂掉，并且尺寸差异大，重启质心)
        FLOAT32 fAppSim = Similarity_Apparent(pLockingPipe, pCandidateTarget);

        // 计算运动相似度
        FLOAT32 fMoveSim = Similarity_Move(pLockingPipe, pCandidateTarget, fAglReso);

        fSim = IOU * fPositionModelWeight + fMoveSim * fMoveModelWeight;


        pCandidateTarget->fMatchConf = fSim;

        // 获得降采样倍数
        SINT32  nDSmpScale = pDAT_Module->GetDatParm()->nDSmpScale;
        //LSBAO, 20160705: 若尺寸宽高方向同时变化超过2个像素或者宽高单方向变化超过5个像素，则认为目标大小突变
        if (IOU < 0.3 && (ABS(pCandidateTarget->snSize.h - pLockingPipe->ObjectFilter.sfSize.h) > 5 * nDSmpScale ||
            ABS(pCandidateTarget->snSize.w - pLockingPipe->ObjectFilter.sfSize.w) > 5 * nDSmpScale))
        {
            continue;
        }

        if (fSim > fSimMax)
        {
            fSimMax = fSim;
            nSimTargetId = i;
            targerIOU = IOU;
            pBestTarget = &pPipe->objHistoryList[pPipe->ubEnd];
        }

    }

    // 单一目标条件判断：当只有一个目标时，关闭大部分条件尽力跟踪，比较符合人眼规律。
    // 由于跟踪时谨慎合并目标，很容易导致临界目标被DST和DAT同时检出，这样其实是一个目标，但数量上不满足单一目标条件，
    BBOOL bSingleTarget = (nFrmObjsCnt == 1);
    //if (nFrmObjsCnt == 2)
    //{
    //    TARGET_OBJECT* ptTarget1 = &ptTargetArray[0];
    //    TARGET_OBJECT* ptTarget2 = &ptTargetArray[1];

    //    if (ABS(ptTarget1->pfCenPos.x - ptTarget2->pfCenPos.x) <= 3 &&
    //        ABS(ptTarget1->pfCenPos.y - ptTarget2->pfCenPos.y) <= 3)
    //    {
    //        bSingleTarget = TRUE;
    //    }
    //}

    // 防跳变
    if (nSimTargetId != -1)
    {
        // 用预测位置和上一帧的位置，防止跟踪跳转到错误的目标
        FLOAT32 fAzSpeed = m_ObjStatus.sfAglSpeed.vx / fAglReso;
        FLOAT32 fPtSpeed = m_ObjStatus.sfAglSpeed.vy / fAglReso;

        SINT32  nPredictDiffThresX = 16; //  与预测位置像素差阈值
        SINT32  nPredictDiffThresY = 16; //  与预测位置像素差阈值
        SINT32  nLastDiffThresX = 10;    //  与上一帧的像素差阈值
        SINT32  nLastDiffThresY = 10;    //  与上一帧的像素差阈值
        BBOOL   bIntercept = FALSE;      //  拦截标志位

        // 没得选了
        if (m_ObjStatus.unContiLostCnt >= 3 && bSingleTarget)
        {
            nPredictDiffThresX = MAX(70, nPipeRadius * 2);
            nPredictDiffThresY = MAX(70, nPipeRadius * 2);
            nLastDiffThresX = nPipeRadius * 2;//波门范围直接取
            nLastDiffThresY = nPipeRadius * 2;//波门范围直接取
        }


        // 限定AI识别在跟踪目标中心位置，缓解AI识别结果帧同步问题(弊端：无法实现周边位置捕获)
        BBOOL st1 = ABS(pBestTarget->pfCenPos.x - centX) < 50 && ABS(pBestTarget->pfCenPos.y - centY) < 50;
        if (FALSE == st1)
        {
            fSimMax = -1;
        }
        //在不同的丢失情况下设置不同层级的阈值
        if (m_ObjStatus.unContiLostCnt < 10)
        {//未丢失，或10帧丢失范围内，阈值较为严格
            fIOUThres = 0.35;
            nPredictDiffThresX = 10;
            nPredictDiffThresY = 10;
            nLastDiffThresX = 8;
            nLastDiffThresY = 8;
            bIntercept = TRUE;
        }
        else
        {//大于帧丢失范围内，阈值放松一些
            fIOUThres = 0.3;
            bIntercept = TRUE;
        }

        if (bIntercept == TRUE)
        {
            nPredictDiffThresX = MAX(pBestTarget->snSize.w / 2, nPredictDiffThresX);
            nPredictDiffThresY = MAX(pBestTarget->snSize.h / 2, nPredictDiffThresY);

            nLastDiffThresX = MAX(pBestTarget->snSize.w / 2, nLastDiffThresX);
            nLastDiffThresY = MAX(pBestTarget->snSize.h / 2, nLastDiffThresY);

            pBestTarget = &m_PipeArray[nSimTargetId].objHistoryList[m_PipeArray[nSimTargetId].ubEnd];
            ptLockingTarget = &pLockingPipe->objHistoryList[pLockingPipe->ubEnd];

            FLOAT32 fSizeChange = 0.0f;
            FLOAT32 fPixChange = 0.0f;

            fSizeChange = (FLOAT32)(pBestTarget->snSize.s) / MAX(MAX(ptLockingTarget->snSize.s, ptLockingTarget->snAIDetSize.s), 0.001f);
            fPixChange = (FLOAT32)(pBestTarget->unObjPxlsCnt) / MAX(ptLockingTarget->unObjPxlsCnt, 0.001f);


            // 尺寸拦截强逻辑，避免错误关联
            if (pBestTarget->unObjPxlsCnt < 6 || ptLockingTarget->unObjPxlsCnt < 6)
            {
                if (12 < fPixChange || 0.1 > fPixChange)
                {
                    fSimMax = -1;
                }
            }
            else if (12 < fSizeChange || 0.1 > fSizeChange)
            {
                fSimMax = -1;
            }
            else
            {

            }

            // 距离限定强逻辑，避免错误关联
            if ((ABS(pBestTarget->pfCenPos.x - stMotionMod_mean.crnObjPrediRtLong.cx) > MAX(ABS(fAzSpeed) * 2, nPredictDiffThresX)
                || ABS(pBestTarget->pfCenPos.y - stMotionMod_mean.crnObjPrediRtLong.cy) > MAX(ABS(fPtSpeed) * 2, nPredictDiffThresY))
                && (ABS(pBestTarget->pfCenPos.x - m_ObjStatus.ptPos.x) > MAX(ABS(fAzSpeed) * 2, nLastDiffThresX)
                    || ABS(pBestTarget->pfCenPos.y - m_ObjStatus.ptPos.y) > MAX(ABS(fPtSpeed) * 2, nLastDiffThresY)))
            {
                fSimMax = -1;
            }
        }
    }

    //+++++++++++++++++++++++++++++++++++++++++++++++++++++++
    //记录管道目标跟踪相似度
    pLockingPipe->fConfidence = fSimMax;


    // 跟踪阈值动态调整
    FLOAT32 fThres = fSimThres;
    if (bSingleTarget)
    {
        fThres = 0.0f;//没有底线的跟踪
        //// 超过10帧丢失，降低阈值搜索
        //if (m_ObjStatus.unContiLostCnt >= 10)
        //{
        //    fThres = 0.1f;
        //}

    }
    else
    {
        // 超过10帧丢失，降低阈值搜索
        if (m_ObjStatus.unContiLostCnt >= 10)
        {

            fThres = fSimThres * 0.5f;
        }

        // 丢失超过10帧，直接取位置相似度高的目标

        // 跟踪前10帧防掉
        if (m_ObjStatus.unTotalCnt < 10)
        {
            fThres = fSimThres * 0.5f;
        }

    }

    //若最大相似度不满足阈值，则认为未查找到管道目标，返回-1
    if (fSimMax < fThres)
    {
        nSimTargetId = -1;
    }
    if (-1 != nSimTargetId)
    {
        //记录管道目标跟踪相似度
        pLockingPipe->fConfidence = fSimMax;
        pLockingPipe->blookout = FALSE;
    } else
    {
        pLockingPipe->fConfidence = -1;
    }
    return nSimTargetId;
}


void  SA_Tracker::TO_RecordObjFeatures(TARGET_OBJECT* ptTargetMonitor, GLB_INPUT* p_GLB_Input)
{
    // 如果SA跟踪器没有在当前帧找到目标，则不更新目标特征
    if (ptTargetMonitor->bObject == FALSE)
    {
        return;
    }


    //// 管道丢失，不进行统计
    //// 管道更新来源不是检测算法，也不能更新
    //// 注意：当前框架下，所有跟踪算法结果包含手动起批都可以刷新管道
    if (ptTargetMonitor->nObjTypeSrc != ObjSrc::Arith_DST && ptTargetMonitor->nObjTypeSrc != ObjSrc::Arith_DAT)
    {
        return;
    }

    SINT32 nIndex = g_GLB_MonitorStatus.MonitorCnts % FEA_LENS;

    // 灰度值
    g_GLB_MonitorStatus.TargetGray[nIndex] = ptTargetMonitor->pxObjGray;

    // 宽高比
    g_GLB_MonitorStatus.TargetWHRatio[nIndex] = ptTargetMonitor->snSize.w / (ptTargetMonitor->snSize.h + 0.1f);

    // 实际像素数
    g_GLB_MonitorStatus.TargetPxls[nIndex] = (FLOAT32)ptTargetMonitor->unObjPxlsCnt;

    // 信噪比
    g_GLB_MonitorStatus.TargetSNR[nIndex] = ptTargetMonitor->fSNR;

    // 背景均值
    g_GLB_MonitorStatus.TargetBKGMean[nIndex] = ptTargetMonitor->fBGMean;

    //记录数组更新帧编号
    g_GLB_MonitorStatus.unLastUpdateFrm = p_GLB_Input->unFrmId;

    g_GLB_MonitorStatus.MonitorCnts++;//记录+1


    // 统计目标管道特性
    TO_StatisticsObjFeatures();

    //20180917，如果监控帧数超过阈值，且当前帧监控数组有更新,标记监控数据有效
    if (g_GLB_MonitorStatus.MonitorCnts > GLB_MONITOR_VALID_CNT)
    {
        g_GLB_MonitorStatus.bMonitorVaildFlag = true;//监控有效
    }

}




void SA_Tracker::TO_CleanUpObjFeatures()
{
    //return;
    // 仅在跟踪器初始化时调用，信息保留到下一次跟踪初始化，或者因超时而清空
    memset(&g_GLB_MonitorStatus, 0, sizeof(MonitorStatus));
}

void SA_Tracker::Reset(OBJECTSTATUS* pObjStatus, BBOOL bUpdateMod, GD_VIDEO_FRAME_S img, SINT32 nWidth, SINT32 nHeight, GLB_INPUT* g_GLB_stInput)
{
    //memcpy(&m_ObjStatus, pObjStatus, sizeof(OBJECTSTATUS));

    //+++++++++++++++++++++++++++++++++++++++++++++++++++++++
    //重置目标位置、尺寸、速度、角度、角速度
    m_ObjStatus.ptPos = pObjStatus->ptPos;
    m_ObjStatus.ptPosFilter = pObjStatus->ptPosFilter;
    m_ObjStatus.sfSize = pObjStatus->sfSize;
    m_ObjStatus.sfSpeed = pObjStatus->sfSpeed;
    m_ObjStatus.afAngle = pObjStatus->afAngle;
    m_ObjStatus.sfAglSpeed = pObjStatus->sfAglSpeed;
    m_ObjStatus.fConfidence = pObjStatus->fConfidence;
    //m_ObjStatus.unContiLostCnt = 0;  // SA被reset,丢失帧数重新计数（不能重置，SA计数累加用于控制捕获）

    //重置目标管道
    m_LockingPipe->ObjectFilter.pfCenPos = pObjStatus->ptPos;
    m_LockingPipe->ObjectFilter.sfSize = pObjStatus->sfSize;

    //20161124Whao，重置目标管道的像素点个数，因为找管道最优目标时会进行像素点个数的强逻辑判断
    m_LockingPipe->ObjectFilter.fPxlsCnt = pObjStatus->fObjPxlsCnt;

    if (bUpdateMod)
    {
        memset(&g_GLB_MonitorStatus, 0, sizeof(MonitorStatus));

        // 检测目标
        CENTERRECT roi = { 0 };
        roi.cx = SINT16(pObjStatus->ptPos.x);
        roi.cy = SINT16(pObjStatus->ptPos.y);
        roi.w = SINT16(MAX(pObjStatus->sfSize.w * 3, 80));
        roi.h = SINT16(MAX(pObjStatus->sfSize.h * 3, 80));
        SINT32 nDAT_Num = pDAT_Module->Detect(img, roi, 0, GLB_STATUS_TRACK);
        TARGET_OBJECT tmp_target = { 0 };
        if (nDAT_Num > 0)
        {
            tmp_target = pDAT_Module->GetTargetArray()[0];
        }
        else
        {
            RECT32S bbox = { 0 };
            bbox.x = SINT32(pObjStatus->ptPos.x - pObjStatus->sfSize.w / 2);
            bbox.y = SINT32(pObjStatus->ptPos.y - pObjStatus->sfSize.h / 2);
            bbox.w = SINT32(pObjStatus->sfSize.w);
            bbox.h = SINT32(pObjStatus->sfSize.h);
            tmp_target = CreateNewTarget(img, nWidth, nHeight, bbox, g_GLB_stInput->unFrmId);
        }


        // 初始化模型
        if (1)
        {
            SINT32 nIndex = g_GLB_MonitorStatus.MonitorCnts % FEA_LENS;

            // 灰度值
            g_GLB_MonitorStatus.TargetGray[nIndex] = tmp_target.pxObjGray;

            // 宽高比
            g_GLB_MonitorStatus.TargetWHRatio[nIndex] = tmp_target.snSize.w / (tmp_target.snSize.h + 0.1f);

            // 实际像素数
            g_GLB_MonitorStatus.TargetPxls[nIndex] = (FLOAT32)tmp_target.unObjPxlsCnt;

            // 信噪比
            g_GLB_MonitorStatus.TargetSNR[nIndex] = tmp_target.fSNR;

            // 背景均值
            g_GLB_MonitorStatus.TargetBKGMean[nIndex] = tmp_target.fBGMean;

            //记录数组更新帧编号
            g_GLB_MonitorStatus.unLastUpdateFrm = g_GLB_stInput->unFrmId;

            g_GLB_MonitorStatus.MonitorCnts++;//记录+1


            // 统计目标管道特性
            TO_StatisticsObjFeatures();

        }


    }
}

void SA_Tracker::TO_StatisticsObjFeatures()
{
    SINT32 nEnd = (g_GLB_MonitorStatus.MonitorCnts - 1) % FEA_LENS;

    SINT32 nCount = MIN(FEA_LENS, g_GLB_MonitorStatus.MonitorCnts);

    //灰度监控
    TO_CalcStdOfArray(g_GLB_MonitorStatus.TargetGray, FEA_LENS, nEnd, nCount, &g_GLB_MonitorStatus.TargetGray_sta);
    //像素点个数监控
    TO_CalcStdOfArray(g_GLB_MonitorStatus.TargetPxls, FEA_LENS, nEnd, nCount, &g_GLB_MonitorStatus.TargetPxls_sta);
    // 宽高比监控
    TO_CalcStdOfArray(g_GLB_MonitorStatus.TargetWHRatio, FEA_LENS, nEnd, nCount, &g_GLB_MonitorStatus.TargetWHRatio_sta);
    // 信噪比
    TO_CalcStdOfArray(g_GLB_MonitorStatus.TargetSNR, FEA_LENS, nEnd, nCount, &g_GLB_MonitorStatus.TargetSNR_sta);
    // 背景均值
    TO_CalcStdOfArray(g_GLB_MonitorStatus.TargetBKGMean, FEA_LENS, nEnd, nCount, &g_GLB_MonitorStatus.TargetBKGMean_sta);
}


//20180305，统计数组的标准差
void SA_Tracker::TO_CalcStdOfArray(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[GLB_MONITOR_VALID_CNT] = { 0 };	//归一化后的数组
    //FLOAT32 fRange = 0.0f;	//数组值域范围

    //防错，如果统计长度大于总数组长度，则直接返回-1（异常）
    if (nCountLen > GLB_MONITOR_VALID_CNT)
    {
        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;
}


/**********************************************************
* 函数名称：UpdateObject2Tracker()
* 功能描述：更新目标跟踪器中目标信息
* 输入参数：PIPE *pPipe     --  跟踪目标管道
*           SINT32 nWidth   --  图像宽度
*           SINT32 nHeight  --  图像高度
* 输出参数：无
* 返 回 值：无
* 调用关系：无
* 其它说明：无
**********************************************************/
void SA_Tracker::UpdateObject2Tracker(TARGET_OBJECT* pTarget, GLB_INPUT* p_GLB_Input, SINT32 nTrackMemThresh)
{
    SINT32 nWidth = p_GLB_Input->nImageWidth;
    SINT32 nHeight = p_GLB_Input->nImageHeight;
    OBJECTSTATUS* pObjStatus = &this->m_ObjStatus;

    // 更新帧编号
    pObjStatus->unFrmId = p_GLB_Input->unFrmId;

    //更新目标计数器
    pObjStatus->unTotalCnt++;

    // 未找到目标
    if (!pTarget->bObject)
    {
        pObjStatus->unContiTrackedCnt = 0;
        pObjStatus->unContiLostCnt++;


        //仅使用长时预测更新目标位置
        pObjStatus->ptPosPre = pObjStatus->ptPos;
        pObjStatus->ptPos.x = m_LockingPipe->ptCurrentPnt.x; //使用管道预测位置
        pObjStatus->ptPos.y = m_LockingPipe->ptCurrentPnt.y;
        pObjStatus->ptPosFilter = pObjStatus->ptPos;
        SATrkState = Locked_Losting;

        // 其他特征均不更新
        pObjStatus->unClsType = 0;
        pObjStatus->emClsSrc = ClsSrc::Arith_CLS_UNKNOWN;
    }
    else
    {
        m_LockingPipe->nMatchpipe = 0; //重置重捕次数
        pObjStatus->unTrackedCnt++;
        pObjStatus->unContiTrackedCnt++;
        pObjStatus->unContiLostCnt = 0;

        //特征更新
        pObjStatus->ptPosPre = pObjStatus->ptPos;
        pObjStatus->ptPos = pTarget->pfCenPos;
        pObjStatus->sfSize.w = pTarget->snSize.w;
        pObjStatus->sfSize.h = pTarget->snSize.h;
        pObjStatus->sfSize.s = pObjStatus->sfSize.w * pObjStatus->sfSize.h;

        pObjStatus->pxObjGray = pTarget->pxObjGray;
        pObjStatus->fObjPxlsCnt = pTarget->unObjPxlsCnt;
        pObjStatus->fObjStd = pTarget->fObjStd;      // 目标方差
        pObjStatus->fBGStd = pTarget->fBGStd;       // 目标背景方差
        pObjStatus->fSNR = pTarget->fSNR;           // 信噪比
        pObjStatus->fBGMean = pTarget->fBGMean;
        // 滤波值
        pObjStatus->ptPosFilter.x = pObjStatus->ptPosFilter.x * 0.4 + pObjStatus->ptPos.x * 0.6;//位置在图像系滤波
        pObjStatus->ptPosFilter.y = pObjStatus->ptPosFilter.y * 0.4 + pObjStatus->ptPos.y * 0.6;//位置在图像系滤波
        pObjStatus->sfSizeFilter.w = pObjStatus->sfSizeFilter.w * 0.4 + pObjStatus->sfSize.w * 0.6;
        pObjStatus->sfSizeFilter.h = pObjStatus->sfSizeFilter.h * 0.4 + pObjStatus->sfSize.h * 0.6;
        pObjStatus->sfSizeFilter.s = pObjStatus->sfSizeFilter.s * 0.4 + pObjStatus->sfSize.s * 0.6;

        // 检测分类置信度
        pObjStatus->fDetConf = pTarget->fDetConf;

        // 来自AI的尺寸
        pObjStatus->snAIDetSize.w = pTarget->snAIDetSize.w;
        pObjStatus->snAIDetSize.h = pTarget->snAIDetSize.h;
        pObjStatus->snAIDetSize.s = pObjStatus->snAIDetSize.w * pObjStatus->snAIDetSize.h;

        //更新目标置信度为目标匹配度
        pObjStatus->fConfidence = pTarget->fMatchConf;

        // 跟踪器目标来源
        pObjStatus->nObjTypeSrc = pTarget->nObjTypeSrc;

        pObjStatus->unClsType = pTarget->unClsType;
        pObjStatus->emClsSrc = pTarget->emClsSrc;

        pObjStatus->bObjMiss = false;

        SATrkState = Locked_Tracking;
    }

    //20170915,优化，非射击期间出视场解锁
    //20200401，增加边缘检测需求，出视场边界由10改为1，调用IMGO_IsPoint16SOutImg（原IMGO_IsPoint32FOutImg）
    POINT16S pntTempPos = { 0 };
    pntTempPos.x = (SINT16)pObjStatus->ptPos.x;
    pntTempPos.y = (SINT16)pObjStatus->ptPos.y;
    BBOOL bInsideFOV = !IMGO_IsPoint16SOutImg(nWidth, nHeight, pntTempPos);
    if (bInsideFOV)
    {
        m_LockingPipe->unContinueOutFOVCnt = 0;
    }
    else
    {
        m_LockingPipe->unContinueOutFOVCnt++;
    }
    //若预测帧数超出阈值，或管道目标超出视场，则标记目标丢失(待删除)
    //更改单帧超出视场为多帧超出视场
    if (m_LockingPipe->unContinueOutFOVCnt > 10)
    {
        pObjStatus->bObjMiss = true;
        SATrkState = Unlock_MemOutFOV;
    }

    if (pObjStatus->unContiLostCnt > nTrackMemThresh)
        //|| (!bInsideFOV /*&& !g_GLB_stPara.bSetMemoryTrack*/))
    {
        pObjStatus->bObjMiss = true;
        SATrkState = Unlock_MemTimeOver;
    }


    ////更新目标速度
    //pObjStatus->sfSpeed.vx = pPipe->sfSpeed.vx;
    //pObjStatus->sfSpeed.vy = pPipe->sfSpeed.vy;

    ANGLE32F afAglPre = pObjStatus->afAngle;

    Pole targetPole = getStablePoleFromImagePos(pObjStatus->ptPos, 
        p_GLB_Input->stCamera, p_GLB_Input->servoInfo,
        p_GLB_Input->afPlatformRPY, p_GLB_Input->setupErr);
    pObjStatus->afAngle.fAz = (FLOAT32)targetPole.beta;
    pObjStatus->afAngle.fPt = (FLOAT32)targetPole.alpha;


    //更新目标角速度,直接从管道取（ab滤波器的速度）
    //pObjStatus->sfAglSpeed.vx = m_LockingPipe->stMotionMod.m_pdx;
    //pObjStatus->sfAglSpeed.vy = m_LockingPipe->stMotionMod.m_pdy;

    //长短时轨迹的速度
    pObjStatus->sfAglSpeed.vx = m_LockingPipe->sfAglSpeed.vx;
    pObjStatus->sfAglSpeed.vy = m_LockingPipe->sfAglSpeed.vy;

    ////更新目标置信度
    //pObjStatus->fConfidence = m_LockingPipe->fConfidence;
    //+++++++++++++++++++++++++++++++++++++++++++++++++++++++
    ////20150608: 更新目标最大最小值，用于弱小目标跟踪
    //PIXELTYPE pxObjGray = (PIXELTYPE)(pPipe->ObjectFilter.fGray);
    //g_TST_stOutput.pxObjGrayMin = MIN(pxObjGray, g_TST_stOutput.pxObjGrayMin);
    //g_TST_stOutput.pxObjGrayMax = MAX(pxObjGray, g_TST_stOutput.pxObjGrayMax);

    // 从管道拷贝尺寸信息到面目标匹配输出，用于控制跟踪锁定后的分割降采样倍率
    OBJECTSTATUS* m_DatTrkTarget = pDAT_Module->GetTargetMatched();
    memset(m_DatTrkTarget, 0, sizeof(OBJECTSTATUS));
    m_DatTrkTarget->sfSize.w = (FLOAT32)pObjStatus->sfSize.w;
    m_DatTrkTarget->sfSize.h = (FLOAT32)pObjStatus->sfSize.h;
    m_DatTrkTarget->sfSize.s = (FLOAT32)pObjStatus->fObjPxlsCnt;

    m_DatTrkTarget->snAIDetSize.w = (FLOAT32)pObjStatus->snAIDetSize.w;
    m_DatTrkTarget->snAIDetSize.h = (FLOAT32)pObjStatus->snAIDetSize.h;
    m_DatTrkTarget->snAIDetSize.s = (FLOAT32)pObjStatus->snAIDetSize.s;
}




//// 基于特征加权融合的相似度计算，提取最优跟踪管道
//SINT32 SA_Tracker::ReFindMatchPipe(PIPE* pPipe, PIPE* ptPipeArray, SINT32 nFrmObjsCnt, SINT32 nPipeRadius, GLB_INPUT* p_GLB_Input, SINT32 nMaxPipeNum)
//{
//    //局部变量
//    FLOAT32 fSim = 0.0f; //目标相似度
//    FLOAT32 fSimMax = 0.5f;//目标相似度最大值
//    FLOAT32 fSim_M = -1.0f;//最相似目标运动相似度
//    FLOAT32 fSim_A = -1.0f;//最相似目标表观相似度
//    FLOAT32 fSimThres = 0.6f; //相似度阈值
//    FLOAT32 fSimThresMove = 0.7f; //运动相似度阈值
//    FLOAT32 fSimThresApparent = 0.6f; //表观相似度阈值
//    SINT32  nFrmsStep = 1;    //帧间隔
//    TARGET_OBJECT* ptTarget = NULL; //极大值点的临时目标
//    BYTE8   nSimTargetId = -1;   //与管道差异最小的目标编号
//    TARGET_OBJECT* ptTargetTempA = NULL;//临时目标A
//    TARGET_OBJECT* ptTargetTempB = NULL;//临时目标B
//
//    FLOAT32 fMoveModelWeight = 0.5;//运动模型权值
//    FLOAT32 fApparentModelWeight = 0.5;//表观模型权值
//    FLOAT32 fAglReso = p_GLB_Input->stCamera.fAglReso;
//    PIPE* Pipe_Temp = NULL;//临时管道
//    TARGET_OBJECT* ptTarget_Temp = NULL; //临时目标
//    // 小目标主要使用位置信息
//    if (SizeType::SmallTarget == m_SizeMode)
//    {
//        fMoveModelWeight = 0.8;
//        fApparentModelWeight = 0.2;
//        fSimThresMove = 0.8f; //运动相似度阈值
//        fSimThresApparent = 0.5f; //表观相似度阈值
//    }
//    //累计重捕次数
//    if (pPipe->blookout)
//    {
//        pPipe->nMatchpipe++;
//    }
//    //计算所有当前帧管道与跟踪相似度
//    for (int i = 0; i < nMaxPipeNum; i++)
//    {
//        Pipe_Temp = &ptPipeArray[i];
//        //跳过空管道，丢失外推，非目标管道，干扰管道
//        if (Pipe_Temp->nPipeID == pPipe->nPipeID || !Pipe_Temp->bOccupy || Pipe_Temp->bLost || !Pipe_Temp->bTarget ||
//            (Pipe_Temp->bJammPipe[pPipe->nPipeID] &&
//             ABS(Pipe_Temp->ptCurrentPnt.x - pPipe->ptCurrentPnt.x) + ABS(Pipe_Temp->ptCurrentPnt.y - pPipe->ptCurrentPnt.y) < 3))
//        {
//            continue;
//        }
//        ptTarget_Temp = &Pipe_Temp->objHistoryList[Pipe_Temp->ubEnd];
//        FLOAT32 fAppSim = Similarity_Apparent(pPipe, ptTarget_Temp);
//        // 计算运动相似度
//        FLOAT32 fMoveSim = Similarity_Move(pPipe, Pipe_Temp, p_GLB_Input);
//        fSim = fAppSim * fApparentModelWeight + fMoveSim * fMoveModelWeight;
//        if (fMoveSim < 0.2)
//        {
//            fSim = -1;						//因为遍历全局管道，当运动毫无相似性，不允许重捕
//            continue;
//        }
//        if (m_LockingPipe->nMatchpipe > GLB_FRM_FREQ * 3 && m_LockingPipe->nPipeType == 2)
//        {
//            fSim = MAX(fAppSim, fSim);		//面目标长时间未重捕，且存在表观相似目标，允许取消运动特性判断
//        }
//        if (m_LockingPipe->nMatchpipe > GLB_FRM_FREQ * 3 && m_LockingPipe->nPipeType != 2)
//        {
//            fSim = MAX(fMoveSim, fSim);		//小目标长时间未重捕，且存在运动相似目标，允许取消表观特性判断
//        }
//        ptTarget_Temp->fMatchConf = fSim;
//        //LSBAO, 20160705: 若尺寸宽高方向同时变化超过4个像素或者宽高单方向变化超过6个像素，则认为目标大小突变
//        /*if (SizeType::SmallTarget == m_SizeMode)
//        {
//            if (ABS(Pipe_Temp->ObjectFilter.sfSize.s - pPipe->ObjectFilter.sfSize.s) > 16)
//            {
//                continue;
//            }
//        }*/
//        if (fSim > fSimMax)
//        {
//            fSimMax = fSim;
//            fSim_M = fMoveSim;
//            fSim_A = fAppSim;
//            nSimTargetId = i;
//        }
//    }
//
//    SINT32 nEnd = pPipe->ubEnd;
//    // 防跳变//           by wcw04046 @ 2020/06/22
//    if (nSimTargetId != -1 && m_ObjStatus.unContiLostCnt < 100)
//    {
//        TARGET_OBJECT* ptTarget = &pPipe->objHistoryList[nEnd];
//        Pipe_Temp = &ptPipeArray[nSimTargetId];
//        ptTarget_Temp = &(Pipe_Temp->objHistoryList[Pipe_Temp->ubEnd]);
//        FLOAT32 fSizeChange = 0.0f;
//        FLOAT32 fPixChange = 0.0f;
//
//        fSizeChange = (FLOAT32)(ptTarget_Temp->snSize.s) / MAX(ptTarget->snSize.s, 0.001f);
//        fPixChange = (FLOAT32)(ptTarget_Temp->unObjPxlsCnt) / MAX(ptTarget->unObjPxlsCnt, 0.001f);
//        // 用预测位置和上一帧的位置，防止跟踪跳转到错误的目标
//
//        FLOAT32 fAzSpeed = m_ObjStatus.sfAglSpeed.vx / fAglReso;
//        FLOAT32 fPtSpeed = m_ObjStatus.sfAglSpeed.vy / fAglReso;
//
//        SINT32  nPredictDiffThres = 16; //  与预测位置像素差阈值
//        SINT32  nLastDiffThres = 10;    //  与上一帧的像素差阈值
//
//        if (fSimMax > fSimThres && 1 == nFrmObjsCnt)  //尽力跟踪
//        {
//            nPredictDiffThres = nPipeRadius * 2;
//            nLastDiffThres = nPipeRadius * 2;
//        }
//        if ((ABS(ptTarget_Temp->pfCenPos.x - pPipe->ptCurrentPnt_Long.x) > MAX(ABS(fAzSpeed), nPredictDiffThres)
//             || ABS(ptTarget_Temp->pfCenPos.y - pPipe->ptCurrentPnt_Long.y) > MAX(ABS(fPtSpeed), nPredictDiffThres))
//            && (ABS(ptTarget_Temp->pfCenPos.x - pPipe->ptCurrentPnt_Near.x) > MAX(ABS(fAzSpeed), nLastDiffThres)
//                || ABS(ptTarget_Temp->pfCenPos.y - pPipe->ptCurrentPnt_Near.y) > MAX(ABS(fPtSpeed), nLastDiffThres)))
//        {
//            fSimMax = -1;
//        }
//
//    }
//    if (nSimTargetId == -1)
//    {
//        //记录管道目标跟踪相似度
//        pPipe->fConfidence = -1.f;
//        Pipe_Temp->fConfidence = -1.f;
//        return nSimTargetId;
//    }
//    //+++++++++++++++++++++++++++++++++++++++++++++++++++++++
//    //记录管道目标跟踪相似度
//    pPipe->fConfidence = fSimMax;
//    Pipe_Temp->fConfidence = fSimMax;
//    // 跟踪阈值动态调整
//    FLOAT32 fThres = fSimThres;
//    FLOAT32 fAppaThres = fSimThresApparent;
//    FLOAT32 fMoveThres = fSimThresMove;
//    FLOAT32 ContiLostThres = MIN(float(m_ObjStatus.unContiLostCnt) / 2000.0, 0.5);
//    if (LOST_COVER == m_ObjStatus.nLostReason || m_LockingPipe->blookout)
//    {
//        fSimThres = 0.7;
//        fThres = 0.7;
//    }
//    // 超过10帧丢失，降低阈值搜索
//    if (m_ObjStatus.unContiLostCnt >= 10)
//    {
//        fThres = MAX(fSimThres * (1 - ContiLostThres), 0.5);
//        fMoveThres = MAX(fSimThresMove * (1 - ContiLostThres), fSimThres);
//        fAppaThres = MAX(fSimThresApparent * (1 - ContiLostThres), 0.5);
//    }
//    // 跟踪前10帧防掉
//    if (m_ObjStatus.unTotalCnt < 20)
//    {
//        fThres = fSimThres * 0.5;
//        fMoveThres = 0.6;
//        fAppaThres = 0.5;
//    }
//    if (1 == nFrmObjsCnt && LOST_COVER != m_ObjStatus.nLostReason)  //尽力跟踪
//    {
//        fThres = fSimThres * 0.8;
//        fAppaThres = fSimThresApparent * 0.8;
//    }
//    //若最大相似度不满足阈值，则认为未查找到管道目标，返回-1
//    if (fSimMax < fThres || (fSim_A < fAppaThres && fSim_M < fMoveThres))
//    {
//        nSimTargetId = -1;
//    }
//
//
//    return nSimTargetId;
//}
//// 更新匹配目标，重置滤波信息，并删除匹配管道
//void SA_Tracker::ReElectionPipe(PIPE* pPipe)
//{
//    memcpy(&m_MatchedTarget, &pPipe->objHistoryList[pPipe->ubEnd], sizeof(TARGET_OBJECT));
//    memcpy(&m_LockingPipe->ObjectFilter, &pPipe->ObjectFilter, sizeof(OBJECT_FILTER));
//    //memcpy(&m_LockingPipe->stMotionMod, &pPipe->stMotionMod, sizeof(FilterNL));
//    //TO_CleanUpObjFeatures();
//    memset(pPipe, 0, sizeof(PIPE));
//}
//
//// 管道运动相似度
//FLOAT32 SA_Tracker::Similarity_Move(PIPE* pLockPipe, PIPE* pPipeTemp, GLB_INPUT* p_GLB_Input)
//{
//    FLOAT32 fAglReso = p_GLB_Input->stCamera.fAglReso;
//    // 当前帧预测位置
//    POINT32F predictPos = { pLockPipe->ptCurrentPnt.x , pLockPipe->ptCurrentPnt.y };
//
//    // 目标大小
//    SINT32 nTargetSize = pLockPipe->ObjectFilter.sfSize.w * pLockPipe->ObjectFilter.sfSize.h;
//
//    // 位置惩罚区间,根据目标大小和速度，类似波门取值方法
//    TARGET_OBJECT* ptTarget = &pPipeTemp->objHistoryList[pPipeTemp->ubEnd];
//    // 长时速度
//    SPEED32F fSpeedLong;
//    SPEED32F fAglSpeedLong = pLockPipe->sfAglSpeed;
//    fSpeedLong.vx = fAglSpeedLong.vx / fAglReso;
//    fSpeedLong.vy = fAglSpeedLong.vy / fAglReso;
//
//    SIZE16S padSize = { 0 };
//    padSize.w = MAX(ABS(fSpeedLong.vx * 3) + pLockPipe->objHistoryList[pLockPipe->ubEnd].snSize.w, 8);
//    padSize.h = MAX(ABS(fSpeedLong.vy * 3) + pLockPipe->objHistoryList[pLockPipe->ubEnd].snSize.h, 8);
//
//    SINT32 nPipeRadiusTrack = 10 + MAX(pLockPipe->ObjectFilter.sfSize.w, pLockPipe->ObjectFilter.sfSize.h);
//    //padSize.w = MAX(ABS(fSpeedLong.vx * 3), 8);
//    //padSize.h = MAX(ABS(fSpeedLong.vy * 3), 8);
//
//    ////SINT32 nPipeRadiusTrack = m_TSA_Param.nPipeRadiusTrack;
//    //SINT32 nPipeRadiusTrack = 2 * (1 + float(pLockPipe->unLostCnt) / 10.0) + 8;
//    SINT32 nPipeRadiusLost = m_TSA_Param.nPipeRadiusLost;
//
//    if (pLockPipe->bLost)
//    {
//        padSize.w = MIN(MAX(nPipeRadiusTrack, ABS(fSpeedLong.vx * pLockPipe->unLostCnt * 0.5)), nPipeRadiusLost);
//        padSize.h = MIN(MAX(nPipeRadiusTrack, ABS(fSpeedLong.vy * pLockPipe->unLostCnt * 0.5)), nPipeRadiusLost);
//    }
//    FLOAT32 pX = ABS(ptTarget->pfCenPos.x - predictPos.x);
//    FLOAT32 pY = ABS(ptTarget->pfCenPos.y - predictPos.y);
//    FLOAT32 pDistScore = 1 - MIN(1, sqrt(pX * pX + pY * pY) / MAX(padSize.w, padSize.h));
//    // 计算方向得分，以长时滤波器为准
//    // 目标运动不明显，直接取位置得分
//    if (ABS(fSpeedLong.vx) < 0.01 && ABS(fSpeedLong.vy) < 0.01 || pDistScore < 0.2)
//    {
//        return pDistScore;
//    }
//
//    POINT32F v1 = { 0 }; POINT32F v2 = { 0 };
//    v1.x = fSpeedLong.vx;
//    v1.y = fSpeedLong.vy;
//    v2.x = pPipeTemp->sfAglSpeed.vx / fAglReso;
//    v2.y = pPipeTemp->sfAglSpeed.vy / fAglReso;
//    FLOAT32 pDirScore = cos_sim(v1, v2);
//
//    // 速度方向得分与位置得分加权，其中速度得分更具有代表性
//    FLOAT32 score = pDirScore * 0.7 + pDistScore * 0.3;
//
//    return score;
//}
//
///**********************************************************
//* 函数名称：UpdateObject2Tracker()
//* 功能描述：更新目标跟踪器中目标信息
//* 输入参数：PIPE *pPipe     --  跟踪目标管道
//*           SINT32 nWidth   --  图像宽度
//*           SINT32 nHeight  --  图像高度
//* 输出参数：无
//* 返 回 值：无
//* 调用关系：无
//* 其它说明：无
//**********************************************************/
//void SA_Tracker::UpdatePipe2Tracker(SINT32 nWidth, SINT32 nHeight, PIPE* pPipe, GLB_INPUT* p_GLB_Input)
//{
//    OBJECTSTATUS* pObjStatus = &this->m_ObjStatus;
//    //更新目标计数器
//    pObjStatus->unTotalCnt++;
//    TARGET_OBJECT* pTarget = &pPipe->objHistoryList[pPipe->ubEnd];
//    pObjStatus->nLostReason = LOST_NONE;
//    // 未找到目标
//    if (!pTarget->bObject && pObjStatus->unContiLostCnt > GLB_PIPE_UNFOUND_CNT)
//    {
//        pObjStatus->nLostReason = LOST_UNFIND;
//    }
//    // 跟踪目标交错
//    if (m_LockingPipe->blookout)
//    {
//        pObjStatus->nLostReason = LOST_CROSS;
//    }
//    if (m_LockingPipe->nMatchpipe > GLB_FRM_FREQ * 5)
//    {
//        pObjStatus->nLostReason = LOST_UNFIND;	//长时间未重捕成功，回退至单目标重捕
//    }
//    //跟踪器进记忆
//    if (!pTarget->bObject)
//    {
//        pObjStatus->unContiTrackedCnt = 0;
//        pObjStatus->unContiLostCnt++;
//        //20150608: 记忆跟踪时，根据目标角度值，更新目标坐标位置
//        ANGLE32F afMemoryAngle = { 0.0 };
//        POINT32F ptMemoryPos = { m_LockingPipe->ptStopPnt.x, m_LockingPipe->ptStopPnt.y };
//
//
//        //使用预测更新目标位置
//        pObjStatus->ptPosPre = pObjStatus->ptPos;
//        pObjStatus->ptPos.x = m_LockingPipe->ptCurrentPnt.x;
//        pObjStatus->ptPos.y = m_LockingPipe->ptCurrentPnt.y;
//        afMemoryAngle.fAz = m_LockingPipe->afCurrentAgl.fAz;
//        afMemoryAngle.fPt = m_LockingPipe->afCurrentAgl.fPt;
//        // 稳定系到图像坐标系
//        Pole targetCarNUEPole;
//        targetCarNUEPole.alpha = afMemoryAngle.fPt;
//        targetCarNUEPole.beta = afMemoryAngle.fAz;
//        targetCarNUEPole.distance = m_LockingPipe->afCurrentAgl.Dr;
//        ptMemoryPos = getImagePosFromStablePole(targetCarNUEPole, p_GLB_Input->stCamera, p_GLB_Input->servoInfo, p_GLB_Input->afPlatformRPY, p_GLB_Input->setupErr);
//
//        pObjStatus->ptPosFilter.x = m_LockingPipe->ptCurrentPnt_Long.x;//记忆状态滤波轨迹取长时坐标
//        pObjStatus->ptPosFilter.y = m_LockingPipe->ptCurrentPnt_Long.y;//记忆状态滤波轨迹取长时坐标
//        // 记忆期间，目标置信度强制置为0.01
//        pObjStatus->fConfidence = 0.01;
//    } else
//    {
//        m_LockingPipe->nMatchpipe = 0;  //重置重捕次数
//
//        pObjStatus->unTrackedCnt++;
//        pObjStatus->unContiTrackedCnt++;
//        pObjStatus->unContiLostCnt = 0;
//
//        //目标尺寸,滤波
//        pObjStatus->sfSize.w = 0.6 * pObjStatus->sfSize.w + 0.4 * pTarget->snSize.w;
//        pObjStatus->sfSize.h = 0.6 * pObjStatus->sfSize.h + 0.4 * pTarget->snSize.h;
//        pObjStatus->sfSize.s = pObjStatus->sfSize.w * pObjStatus->sfSize.h;
//        pObjStatus->fObjPxlsCnt = 0.6 * pObjStatus->fObjPxlsCnt + 0.4 * pTarget->unObjPxlsCnt;
//
//        //更新目标位置
//        pObjStatus->ptPosPre = pObjStatus->ptPos;
//        pObjStatus->ptPos = pTarget->pfCenPos;
//        pObjStatus->ptPosFilter = pObjStatus->ptPos;
//
//        //更新目标置信度为目标匹配度
//        pObjStatus->fConfidence = pTarget->fMatchConf;
//
//        // 跟踪器目标来源
//        pObjStatus->nObjTypeSrc = pTarget->nObjTypeSrc;
//
//        pObjStatus->bObjMiss = false;
//
//        ReElectionPipe(pPipe);
//    }
//    POINT16S pntTempPos = { 0 };
//    pntTempPos.x = (SINT16)pObjStatus->ptPos.x;
//    pntTempPos.y = (SINT16)pObjStatus->ptPos.y;
//    BBOOL bInsideFOV = !IMGO_IsPoint16SOutImg(nWidth, nHeight, pntTempPos);
//
//    //若预测帧数超出阈值，或管道目标超出视场，则标记目标丢失
//    if ((pObjStatus->unContiLostCnt > (UINT32)(m_TSA_Param.nPipeFOVLostCnt)))
//        //|| (!bInsideFOV /*&& !g_GLB_stPara.bSetMemoryTrack*/))
//    {
//        pObjStatus->bObjMiss = true;
//    }
//
//    ANGLE32F afAglPre = pObjStatus->afAngle;
//
//    Pole targetPole = getStablePoleFromImagePos(pObjStatus->ptPos,
//                                                p_GLB_Input->stCamera, p_GLB_Input->servoInfo,
//                                                p_GLB_Input->afPlatformRPY, p_GLB_Input->setupErr);
//    pObjStatus->afAngle.fAz = targetPole.beta;
//    pObjStatus->afAngle.fPt = targetPole.alpha;
//
//    //更新目标角速度,直接从管道取
//    pObjStatus->sfAglSpeed = m_LockingPipe->sfAglSpeed;
//}





// 基于特征加权融合的相似度计算，提取最优跟踪目标
SINT32 SA_Tracker::FindMatchOneshot(PIPE* pPipe, TARGET_OBJECT* ptTargetArray, SINT32 nFrmObjsCnt, SINT32 nPipeRadius, FLOAT32 fAglReso)
{
    //局部变量
    FLOAT32 fSim = 0.0f;                //目标相似度
    FLOAT32 fSimMax = -1.0f;            //最相似目标相似度
    FLOAT32 fSim_M = -1.0f;             //最相似目标运动相似度
    FLOAT32 fSim_A = -1.0f;             //最相似目标表观相似度
    FLOAT32 fSimThres = 0.7f;           //相似度阈值
    FLOAT32 fMoveThres = 0.5f;          //运动相似度阈值
    FLOAT32 fApparentThres = 0.5f;      //表观相似度阈值
    TARGET_OBJECT* ptTarget = NULL;     //临时目标
    BYTE8   nSimTargetId = -1;          //最相似目标编号
    FLOAT32 fMoveModelWeight = 0.5;     //运动模型权值
    FLOAT32 fApparentModelWeight = 0.5; //表观模型权值

    pPipe->unSimTargetNum = 0;          //清空相似目标个数

    //若当前帧目标个数为0，则直接返回查找失败标志(差异度最小目标管道编号为-1)
    if (0 == nFrmObjsCnt)
    {
        return -1;
    }

    // 小目标主要使用位置信息
    if (SizeType::SmallTarget == m_SizeMode)
    {
        fMoveModelWeight = 0.7f;
        fApparentModelWeight = 0.3f;
        fMoveThres = 0.7f;
        fApparentThres = 0.3f;
    }

    // 使用运动模型
    FilterMeanNL stMotionMod_mean = m_LockingPipe->stMotionMod_mean;

    //统计所有当前帧目标与管道目标的灰度、与中心距离、信噪比差异
    for (int i = 0; i < nFrmObjsCnt; i++)
    {
        ptTarget = &ptTargetArray[i];
        //跳过空目标
        if (!ptTarget->bObject)
        {
            continue;
        }
        // 计算表观相似度
        FLOAT32 fAppSim = Similarity_Apparent(pPipe, ptTarget);
        // 计算运动相似度
        FLOAT32 fMoveSim = Similarity_Move(pPipe, ptTarget, fAglReso);

        fSim = fAppSim * fApparentModelWeight + fMoveSim * fMoveModelWeight;


        ptTarget->fMatchConf = fSim;

        SINT32  nDSmpScale = pDAT_Module->GetDatParm()->nDSmpScale;
        //LSBAO, 20160705: 若尺寸宽高方向同时变化超过2个像素或者宽高单方向变化超过3个像素，则认为目标大小突变 -- 丢失超过1S不判断
        if (PIPE_EVENT_JUST_LOCK != m_LockingPipe->ubEventStatus)
        {
            if (((ABS(ptTarget->snSize.h - pPipe->ObjectFilter.sfSize.h) > 2 * nDSmpScale && ABS(ptTarget->snSize.w - pPipe->ObjectFilter.sfSize.w) > 2 * nDSmpScale) ||
                 ABS(ptTarget->snSize.h - pPipe->ObjectFilter.sfSize.h) > 3 * nDSmpScale ||
                 ABS(ptTarget->snSize.w - pPipe->ObjectFilter.sfSize.w) > 3 * nDSmpScale) && m_LockingPipe->unLostCnt < GLB_FRM_FREQ)
            {
                continue;
            }
        }

        FLOAT32 fSimDis = sqrt(POW2(ptTarget->pfCenPos.x - pPipe->ptCurrentPnt.x)
                               + POW2(ptTarget->pfCenPos.y - pPipe->ptCurrentPnt.y));
        // 加外观和距离评价相似目标
        if ((fSim > fSimThres * 0.5 && fMoveSim > fMoveThres * 0.5) ||
            (fAppSim > 0.25 && fMoveSim > fMoveThres * 0.5))
        {
            pPipe->unSimTargetNum++;
        }

        if (fSim > fSimMax)
        {
            fSimMax = fSim;
            nSimTargetId = i;
            fSim_M = fMoveSim;
            fSim_A = fAppSim;
        }
    }
    //循环记录每帧相似目标个数
    nSimTargetNum_Counter[ubSimTarget_End] = pPipe->unSimTargetNum;
    ubSimTarget_End = (UBYTE8)((ubSimTarget_End + 1) % GLB_SIMOBJ_VALID_CNT);

    if (pPipe->unSimTargetNum > 1)
    {
        pPipe->blookout = TRUE;
    }
    // 单一目标条件判断：当只有一个目标时，关闭大部分条件尽力跟踪，比较符合人眼规律。
    // 由于跟踪时谨慎合并目标，很容易导致临界目标被DST和DAT同时检出，这样其实是一个目标，但数量上不满足单一目标条件，
    BBOOL bSingleTarget = ((nFrmObjsCnt == 1) && !pPipe->blookout);
    if (nFrmObjsCnt == 2)
    {
        TARGET_OBJECT* ptTarget1 = &ptTargetArray[0];
        TARGET_OBJECT* ptTarget2 = &ptTargetArray[1];

        if (ABS(ptTarget1->pfCenPos.x - ptTarget2->pfCenPos.x) <= 3 &&
            ABS(ptTarget1->pfCenPos.y - ptTarget2->pfCenPos.y) <= 3 && !pPipe->blookout)
        {
            bSingleTarget = TRUE;
        }
    }

    SINT32 nEnd = pPipe->ubEnd;
    // 防跳变//           by wcw04046 @ 2020/06/22
    if (nSimTargetId != -1 && m_ObjStatus.unContiLostCnt < 200)
    {
        ptTarget = &pPipe->objHistoryList[nEnd];

        FLOAT32 fSizeChange = 0.0f;
        FLOAT32 fPixChange = 0.0f;

        fSizeChange = (FLOAT32)(ptTargetArray[nSimTargetId].snSize.s) / MAX(ptTarget->snSize.s, 0.001f);
        fPixChange = (FLOAT32)(ptTargetArray[nSimTargetId].unObjPxlsCnt) / MAX(ptTarget->unObjPxlsCnt, 0.001f);

        // 目标运动距离强逻辑
        ptTarget = &ptTargetArray[nSimTargetId];

        // 用预测位置和上一帧的位置，防止跟踪跳转到错误的目标
        if (nSimTargetId != -1)
        {
            FLOAT32 fAzSpeed = m_ObjStatus.sfAglSpeed.vx / fAglReso;
            FLOAT32 fPtSpeed = m_ObjStatus.sfAglSpeed.vy / fAglReso;

            SINT32  nPredictDiffThres = 20; //  与预测位置像素差阈值
            SINT32  nLastDiffThres = 10;    //  与上一帧的像素差阈值

            if (bSingleTarget)  //尽力跟踪
            {
                nPredictDiffThres = nPipeRadius * 2;
                nLastDiffThres = nPipeRadius * 2;
            }
            //if (PIPE_EVENT_JUST_LOCK != m_LockingPipe->ubEventStatus)
            {
                if ((ABS(ptTarget->pfCenPos.x - stMotionMod_mean.crnObjPrediRtLong.cx) > MAX(ABS(fAzSpeed) * 2, nPredictDiffThres)
                     || ABS(ptTarget->pfCenPos.y - stMotionMod_mean.crnObjPrediRtLong.cy) > MAX(ABS(fPtSpeed) * 2, nPredictDiffThres))
                    && (ABS(ptTarget->pfCenPos.x - m_ObjStatus.ptPos.x) > MAX(ABS(fAzSpeed) * 2, nLastDiffThres)
                        || ABS(ptTarget->pfCenPos.y - m_ObjStatus.ptPos.y) > MAX(ABS(fPtSpeed) * 2, nLastDiffThres)))
                {
                    fSimMax = -1;
                }
            }
        }
    }

    // 跟踪阈值动态调整
    FLOAT32 fThres = fSimThres;
    FLOAT32 ContiLostThres = 1.f - MIN(float(m_ObjStatus.unContiLostCnt) / GLB_FRM_FREQ, 0.5);     //丢失帧比例
    if (bSingleTarget /* && m_ObjStatus.unContiLostCnt > GLB_FRM_FREQ */)
    {
        fThres = fSimThres * ContiLostThres * 0.8;//降低底线的跟踪
        fMoveThres = fMoveThres * ContiLostThres * 0.8;
    } else
    {
        fThres = fSimThres * ContiLostThres;
        fMoveThres = fMoveThres * ContiLostThres;

        // 跟踪前10帧防掉
        if (m_ObjStatus.unTotalCnt < 10)
        {
            fThres = fSimThres * 0.1f; //没有底线的跟踪
        }

    }
    if (pPipe->blookout)
    {
        fThres = 0.8 * ContiLostThres;
        fMoveThres = 0.9 * ContiLostThres;
    }
    //若最大相似度不满足阈值，则认为未查找到管道目标，返回-1
    if (fSimMax < fThres || (SmallTarget == m_SizeMode && fSim_M < fMoveThres))
    {
        nSimTargetId = -1;
    }
    if (-1 != nSimTargetId)
    {
        //记录管道目标跟踪相似度
        pPipe->fConfidence = fSimMax;
        //pPipe->blookout = FALSE;
    } else
    {
        pPipe->fConfidence = -1;
    }
    return nSimTargetId;
}

//基于特征加权融合的相似度计算，提取最优跟踪目标
SINT32 SA_Tracker::FindMatchTracklet(SINT32 nWidth, SINT32 nHeight, PIPE* pLockingPipe, PIPE* m_PipeArray, SINT32 nRealPipeNum,
                                 SINT32 nMaxPipeNum, TSA_Parameters* pTSA_Param, FLOAT32 fAglReso)
{
    //局部变量
    FLOAT32 fSim = 0.0f; //目标相似度
    FLOAT32 fSimMax = -1.0f;//目标相似度最大值
    FLOAT32 fSim_M = -1.0f;//最相似目标运动相似度
    FLOAT32 fSim_A = -1.0f;//最相似目标表观相似度
    FLOAT32 fSimThres = 0.6f; //相似度阈值
    FLOAT32 fSimThresMove = 0.7f; //运动相似度阈值
    FLOAT32 fSimThresApparent = 0.6f; //表观相似度阈值
    SINT32  nFrmsStep = 1;    //帧间隔
    TARGET_OBJECT* ptTarget = NULL; //极大值点的临时目标
    BYTE8   nSimTargetId = -1;   //与管道差异最小的目标编号
    TARGET_OBJECT* ptTargetTemp = NULL;//临时目标
    PIPE* Pipe_Temp = NULL;//临时管道
    FLOAT32 fMoveModelWeight = 0.5;//运动模型权值
    FLOAT32 fApparentModelWeight = 0.5;//表观模型权值
    FLOAT32 fMoveThres = 0.5f;                  //运动相似度阈值  
    // 小目标主要使用位置信息
    if (SizeType::SmallTarget == m_SizeMode)
    {
        fMoveModelWeight = 0.8;
        fApparentModelWeight = 0.2;
        fSimThresMove = 0.8f; //运动相似度阈值
        fSimThresApparent = 0.5f; //表观相似度阈值
    }
    pLockingPipe->unSimTargetNum = 0;
    //累计匹配次数
    pLockingPipe->nMatchpipe++;

    //计算所有当前帧管道与跟踪相似度
    for (int i = 0; i < nMaxPipeNum; i++)
    {
        Pipe_Temp = &m_PipeArray[i];
        //跳过空管道，丢失外推，非目标管道，
        if (!Pipe_Temp->bOccupy || Pipe_Temp->bLost || !Pipe_Temp->bTarget || Pipe_Temp->nPipeID == pLockingPipe->nPipeID)
        {
            continue;
        }
        //跳过干扰管道
        if ((Pipe_Temp->bJammPipe && pLockingPipe->nMatchpipe < GLB_FRM_FREQ))
        {
            continue;
        }
        FLOAT32 fSimDis = sqrt(POW2(Pipe_Temp->ptCurrentPnt.x - pLockingPipe->ptCurrentPnt.x)
                               + POW2(Pipe_Temp->ptCurrentPnt.y - pLockingPipe->ptCurrentPnt.y));
        // 近距离管道
        if (fSimDis < MAX(pLockingPipe->ObjectFilter.sfSize.w, pLockingPipe->ObjectFilter.sfSize.h) * 2)
        {
            pLockingPipe->unSimTargetNum++;
        }
        ptTargetTemp = &Pipe_Temp->objHistoryList[Pipe_Temp->ubEnd];
        FLOAT32 fAppSim = Similarity_Apparent(pLockingPipe, ptTargetTemp);
        // 计算运动相似度
        FLOAT32 fMoveSim = Similarity_Move(pLockingPipe, Pipe_Temp, fAglReso);
        fSim = fAppSim * fApparentModelWeight + fMoveSim * fMoveModelWeight;
        if (fMoveSim < 0.2)
        {
            fSim = -1;						//因为遍历全局管道，当运动毫无相似性，不允许重捕
            continue;
        }
        if (m_LockingPipe->nMatchpipe > GLB_FRM_FREQ * 3 && m_LockingPipe->nPipeType == 2)
        {
            fSim = MAX(fAppSim, fSim);		//面目标长时间未重捕，且存在表观相似目标，允许取消运动特性判断
        }
        if (m_LockingPipe->nMatchpipe > GLB_FRM_FREQ * 3 && m_LockingPipe->nPipeType != 2)
        {
            fSim = MAX(fMoveSim, fSim);		//小目标长时间未重捕，且存在运动相似目标，允许取消表观特性判断
        }
        ptTargetTemp->fMatchConf = fSim;
        if (fSim > fSimMax)
        {
            fSimMax = fSim;
            fSim_M = fMoveSim;
            fSim_A = fAppSim;
            nSimTargetId = i;
        }
    }
    //长时未重捕，取消限制，捕获最优目标
    BBOOL bRandom_Track= (m_LockingPipe->nMatchpipe > GLB_FRM_FREQ * 3);

    if (bRandom_Track && nSimTargetId != -1)
    {
        pLockingPipe->fConfidence = fSimMax;
        return nSimTargetId;
    }
    // 防跳变
    if (nSimTargetId != -1 && m_ObjStatus.unContiLostCnt < 100)
    {
        TARGET_OBJECT* ptTarget = &pLockingPipe->objHistoryList[pLockingPipe->ubEnd];
        Pipe_Temp = &m_PipeArray[nSimTargetId];
        ptTargetTemp = &(Pipe_Temp->objHistoryList[Pipe_Temp->ubEnd]);
        FLOAT32 fSizeChange = 0.0f;
        FLOAT32 fPixChange = 0.0f;

        fSizeChange = (FLOAT32)(ptTargetTemp->snSize.s) / MAX(ptTarget->snSize.s, 0.001f);
        fPixChange = (FLOAT32)(ptTargetTemp->unObjPxlsCnt) / MAX(ptTarget->unObjPxlsCnt, 0.001f);
        // 用预测位置和上一帧的位置，防止跟踪跳转到错误的目标

        FLOAT32 fAzSpeed = m_ObjStatus.sfAglSpeed.vx / fAglReso;
        FLOAT32 fPtSpeed = m_ObjStatus.sfAglSpeed.vy / fAglReso;

        SINT32  nPredictDiffThres = 20; //  与预测位置像素差阈值
        SINT32  nLastDiffThres = 10;    //  与上一帧的像素差阈值

        if ((ABS(ptTargetTemp->pfCenPos.x - pLockingPipe->stMotionMod_mean.crnObjPrediRtLong.cx) > MAX(ABS(fAzSpeed), nPredictDiffThres)
             || ABS(ptTargetTemp->pfCenPos.y - pLockingPipe->stMotionMod_mean.crnObjPrediRtLong.cy) > MAX(ABS(fPtSpeed), nPredictDiffThres))
            && (ABS(ptTargetTemp->pfCenPos.x - pLockingPipe->stMotionMod_mean.crnObjPrediRtNear.cx) > MAX(ABS(fAzSpeed), nLastDiffThres)
                || ABS(ptTargetTemp->pfCenPos.y - pLockingPipe->stMotionMod_mean.crnObjPrediRtNear.cy) > MAX(ABS(fPtSpeed), nLastDiffThres)))
        {
            fSimMax = -1;
        }
    }

    //+++++++++++++++++++++++++++++++++++++++++++++++++++++++
    //记录管道目标跟踪相似度
    pLockingPipe->fConfidence = fSimMax;


    // 跟踪阈值动态调整
    // 跟踪阈值动态调整
    FLOAT32 fThres = fSimThres;
    FLOAT32 ContiLostThres = 1.f - MIN(float(m_ObjStatus.unContiLostCnt) / GLB_FRM_FREQ * 2.0f, 0.5);     //丢失帧比例
    {
        fThres = fSimThres * ContiLostThres;
        fMoveThres = fMoveThres * ContiLostThres;

        // 跟踪前10帧防掉
        if (m_ObjStatus.unTotalCnt < 10)
        {
            fThres = fSimThres * 0.1f; //没有底线的跟踪
        }

    }
    //若最大相似度不满足阈值，则认为未查找到管道目标，返回-1
    if (fSimMax < fThres || (SmallTarget == m_SizeMode && fSim_M < fMoveThres))
    {
        nSimTargetId = -1;
    }
    if (-1 != nSimTargetId)
    {
        //记录管道目标跟踪相似度
        pLockingPipe->fConfidence = fSimMax;
        if (pLockingPipe->unSimTargetNum < 2)
        {
            pLockingPipe->blookout = FALSE;
        }
    } else
    {
        pLockingPipe->fConfidence = -1;
    }
    return nSimTargetId;
}

#endif