|
|
#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 * 3)
|
|
|
{
|
|
|
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;
|
|
|
|
|
|
nTrackTargetID = -1;
|
|
|
|
|
|
//调整搜索区域大小，为分块宽高的整数倍
|
|
|
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.x - 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;
|
|
|
pPipe->ptCurrentPnt.x = pPipe->stMotionMod_mean.crnObjPrediRtLong.cx;
|
|
|
pPipe->ptCurrentPnt.y = pPipe->stMotionMod_mean.crnObjPrediRtLong.cy;
|
|
|
|
|
|
// 取长时预测点作为管道当前位置预测
|
|
|
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; // 使用运动模型
|
|
|
//////////////////////////////////////////////////////////////////////////
|
|
|
//////////////////////////小面目标检测////////////////////////////////////
|
|
|
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;
|
|
|
}
|
|
|
// 跟踪阶段，合并距离关键参数
|
|
|
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);
|
|
|
|
|
|
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)
|
|
|
{
|
|
|
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;
|
|
|
}
|
|
|
|
|
|
// 运动相似度
|
|
|
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, 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 (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.7f; //相似度阈值
|
|
|
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;
|
|
|
}
|
|
|
}
|
|
|
|
|
|
// 阈值调整
|
|
|
if (pPipe->unLostCnt < 20)
|
|
|
{
|
|
|
fSScaleChangeLowThres = 0.3;
|
|
|
fSScaleChangeHighThres = 3.f;
|
|
|
fAScaleChangeLowThres = 0.5;
|
|
|
fAScaleChangeHighThres = 2.0;
|
|
|
nLastDiffThresX = 6;
|
|
|
nLastDiffThresY = 6;
|
|
|
nPredictDiffThresX = 8;
|
|
|
nPredictDiffThresY = 8;
|
|
|
}
|
|
|
else if (pPipe->unLostCnt < 50)
|
|
|
{
|
|
|
fSScaleChangeLowThres = 0.2;
|
|
|
fSScaleChangeHighThres = 5.f;
|
|
|
fAScaleChangeLowThres = 0.33;
|
|
|
fAScaleChangeHighThres = 3.0;
|
|
|
nLastDiffThresX = 10;
|
|
|
nLastDiffThresY = 10;
|
|
|
nPredictDiffThresX = 12;
|
|
|
nPredictDiffThresY = 12;
|
|
|
}
|
|
|
else if (pPipe->unLostCnt < 100)
|
|
|
{
|
|
|
fSScaleChangeLowThres = 0.15;
|
|
|
fSScaleChangeHighThres = 8.f;
|
|
|
fAScaleChangeLowThres = 0.33;
|
|
|
fAScaleChangeHighThres = 3.0;
|
|
|
nLastDiffThresX = 15;
|
|
|
nLastDiffThresY = 15;
|
|
|
nPredictDiffThresX = 18;
|
|
|
nPredictDiffThresY = 18;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
fSScaleChangeLowThres = 0.1;
|
|
|
fSScaleChangeHighThres = 12.f;
|
|
|
fAScaleChangeLowThres = 0.25;
|
|
|
fAScaleChangeHighThres = 4.0;
|
|
|
nLastDiffThresX = 15;
|
|
|
nLastDiffThresY = 15;
|
|
|
nPredictDiffThresX = 18;
|
|
|
nPredictDiffThresY = 18;
|
|
|
}
|
|
|
|
|
|
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 < 6 || ptMainTarget->unObjPxlsCnt < 6)
|
|
|
{
|
|
|
if (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 (g_GLB_stPara->nWorkScene == GLB_WATCH_SKY)
|
|
|
//{
|
|
|
// nPredictDiffThres = 70;
|
|
|
// nLastDiffThres = 15;
|
|
|
//}
|
|
|
|
|
|
//if (g_GLB_stPara->nWorkScene == GLB_WATCH_GROUND)
|
|
|
//{
|
|
|
// nPredictDiffThres = 5;
|
|
|
// nLastDiffThres = 5;
|
|
|
//}
|
|
|
|
|
|
// 非初始锁定阶段执行运动距离强逻辑
|
|
|
//小目标情况下，周围可能检测出干扰的相似目标（真正的目标无法检出），需要强逻辑限定
|
|
|
//面目标情况下，单个目标允许大范围关联，关闭距离限定；非单个目标限定关联距离
|
|
|
if (PIPE_EVENT_JUST_LOCK != m_LockingPipe->ubEventStatus &&
|
|
|
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;
|
|
|
}
|
|
|
//若最大相似度不满足阈值，则认为未查找到管道目标，返回-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.1f;//没有底线的跟踪
|
|
|
// 超过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->stMotionMod_mean.crnObjPrediRtLong.cx;
|
|
|
pObjStatus->ptPos.y = m_LockingPipe->stMotionMod_mean.crnObjPrediRtLong.cy;
|
|
|
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->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 |
