|
|
#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 = 25; // 使用AI信息进行修正的频率
|
|
|
|
|
|
g_GLB_MonitorStatus = { 0 };
|
|
|
g_GLB_MonitorStatus.bMonitorVaildFlag = TRUE;
|
|
|
|
|
|
nTrackTargetID = -1;
|
|
|
m_nTargetNum = 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; //小目标和面目标相互切换计数器
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
SA_Tracker::~SA_Tracker()
|
|
|
{
|
|
|
if (pDST_Module)
|
|
|
{
|
|
|
delete pDST_Module;
|
|
|
}
|
|
|
if (pDAT_Module)
|
|
|
{
|
|
|
delete pDAT_Module;
|
|
|
}
|
|
|
|
|
|
|
|
|
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;
|
|
|
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);
|
|
|
|
|
|
return true;
|
|
|
}
|
|
|
|
|
|
int SA_Tracker::Track(GD_VIDEO_FRAME_S img, SINT32 nWidth, SINT32 nHeight, GLB_INPUT* p_GLB_Input, API_MOT_PIPE* g_GLB_PipeProc)
|
|
|
{
|
|
|
nTrackTargetID = -1;
|
|
|
m_MatchedTarget = { 0 };
|
|
|
|
|
|
|
|
|
//调整搜索区域大小，为分块宽高的整数倍
|
|
|
SetAutoSearchZone(nWidth, nHeight, p_GLB_Input);
|
|
|
|
|
|
|
|
|
// 使用运动模型
|
|
|
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;
|
|
|
|
|
|
// 小目标跟踪模式
|
|
|
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, nWidth, nHeight, m_TSA_Input.crCenterRect, GLB_STATUS_TRACK);
|
|
|
|
|
|
// 小目标可以开面目标检测
|
|
|
if (bEnableAreaObjDetect)
|
|
|
{
|
|
|
pDAT_Module->Detect(img, nWidth, nHeight, m_TSA_Input.crCenterRect, GLB_STATUS_TRACK);
|
|
|
}
|
|
|
}
|
|
|
|
|
|
// 面目标跟踪模式
|
|
|
if (m_SizeMode == SizeType::AreaTarget)
|
|
|
{
|
|
|
pDAT_Module->Detect(img, nWidth, nHeight, m_TSA_Input.crCenterRect, GLB_STATUS_TRACK);
|
|
|
}
|
|
|
|
|
|
//将面目标检测结果复制到小目标检测结果数组
|
|
|
// m_nTargetNum = pDST_Module->AddTargetArray(pDAT_Module->GetTargetArray(), pDAT_Module->m_FrmObjsCnt);
|
|
|
|
|
|
// //小、面目标合并
|
|
|
// SINT32 nDist = pDST_Module->m_DST_stPara.nObjCombineDist;
|
|
|
// m_nTargetNum = MergeSmallAndAreaTarget(m_Target_Array, pDST_Module->m_FrmObjsCnt, pDAT_Module->m_FrmObjsCnt,nDist,GLB_STATUS_TRACK);
|
|
|
// 将小目标队列拷贝进输出队列
|
|
|
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; //目标运动的方位角速度(单位：像素)
|
|
|
|
|
|
//指向当前目标
|
|
|
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, 12)
|
|
|
|| ABS(ptTarget->pfCenPos.y - stMotionMod_mean.crnObjPrediRtLong.cy) > MAX(ABS(fPtSpeed) * 3, 12))
|
|
|
&& (ABS(ptTarget->pfCenPos.x - pObjStatus->ptPos.x) > MAX(ABS(fAzSpeed) * 2, 12)
|
|
|
|| ABS(ptTarget->pfCenPos.y - pObjStatus->ptPos.y) > MAX(ABS(fPtSpeed) * 2, 12)))
|
|
|
{
|
|
|
nInvalidTargetCnt++;
|
|
|
}
|
|
|
}
|
|
|
|
|
|
//若所有目标均不满足强逻辑，清空检测结果
|
|
|
if (nInvalidTargetCnt == m_nTargetNum)
|
|
|
{
|
|
|
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_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_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;
|
|
|
}
|
|
|
|
|
|
PIPE* m_PipeArray = g_GLB_PipeProc->getPipeArray();
|
|
|
SINT32 nRealPipeNum = g_GLB_PipeProc->PIPE_GetAlarmNum();
|
|
|
SINT32 m_nMaxPipeNum = g_GLB_PipeProc->PIPE_GetMaxPipeNum();
|
|
|
|
|
|
if (GLB_VIDEO_TYPE::GLB_VIDEO_VL == p_GLB_Input->unVideoSrc)
|
|
|
{
|
|
|
// 第一次查找：在波门范围查找全图管道目标
|
|
|
nTrackTargetID = FindMatchPipe(nWidth, nHeight, m_LockingPipe, m_PipeArray, nRealPipeNum, m_nMaxPipeNum,
|
|
|
&m_TSA_Param, p_GLB_Input->stCamera.fAglReso);
|
|
|
// 将搜索到的匹配目标输出到跟踪器，使用AI识别进行匹配关联对识别结果的同步性要求较高
|
|
|
if (nTrackTargetID != -1)
|
|
|
{
|
|
|
memcpy(&m_MatchedTarget, &m_PipeArray[nTrackTargetID].objHistoryList[m_PipeArray[nTrackTargetID].ubEnd], sizeof(TARGET_OBJECT));
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
// 第二次查找：在波门范围查找区域小面检测目标
|
|
|
nTrackTargetID = FindMatchTarget(m_LockingPipe, m_Target_Array, m_nTargetNum, m_TSA_Param.nPipeRadiusTrack, p_GLB_Input->stCamera.fAglReso);
|
|
|
if (-1 != nTrackTargetID)
|
|
|
{
|
|
|
memcpy(&m_MatchedTarget, &m_Target_Array[nTrackTargetID], sizeof(TARGET_OBJECT));
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
memset(&m_MatchedTarget, 0, sizeof(TARGET_OBJECT));
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
// 第一次查找：在波门范围查找区域小面检测目标
|
|
|
nTrackTargetID = FindMatchTarget(m_LockingPipe, m_Target_Array, m_nTargetNum, m_TSA_Param.nPipeRadiusTrack, p_GLB_Input->stCamera.fAglReso);
|
|
|
|
|
|
if (nTrackTargetID != -1)
|
|
|
{
|
|
|
memcpy(&m_MatchedTarget, &m_Target_Array[nTrackTargetID], sizeof(TARGET_OBJECT));
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
// 第二次查找：在波门范围查找全图管道目标
|
|
|
// 将搜索到的匹配目标输出到跟踪器，使用AI识别进行匹配关联对识别结果的同步性要求较高
|
|
|
nTrackTargetID = FindMatchPipe(nWidth, nHeight, m_LockingPipe, m_PipeArray, nRealPipeNum, m_nMaxPipeNum,
|
|
|
&m_TSA_Param, p_GLB_Input->stCamera.fAglReso);
|
|
|
if (-1 != nTrackTargetID)
|
|
|
{
|
|
|
memcpy(&m_MatchedTarget, &m_PipeArray[nTrackTargetID].objHistoryList[m_PipeArray[nTrackTargetID].ubEnd], sizeof(TARGET_OBJECT));
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
memset(&m_MatchedTarget, 0, sizeof(TARGET_OBJECT));
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
|
|
|
// 将匹配目标的特征更新到跟踪器
|
|
|
UpdateObject2Tracker(nWidth, nHeight, &m_MatchedTarget, p_GLB_Input);
|
|
|
|
|
|
// 局部区域的单帧检测结果拷贝到输出
|
|
|
m_TSA_output.m_nTargetNum = m_nTargetNum;
|
|
|
memcpy(m_TSA_output.mTarget_Array, m_Target_Array, sizeof(TARGET_OBJECT)* m_nTargetNum);
|
|
|
// 跟踪器局部的检测器结果清空
|
|
|
memset(m_Target_Array, 0, sizeof(TARGET_OBJECT) * m_nTargetNum);
|
|
|
m_nTargetNum = 0;
|
|
|
|
|
|
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; //搜索区域四角矩形
|
|
|
SINT32 nPipeRadiusTrack = m_TSA_Param.nPipeRadiusTrack;
|
|
|
SINT32 nPipeRadiusLost = m_TSA_Param.nPipeRadiusLost;
|
|
|
POINT32F PipeCurrentPnt = { 0 };//跟踪管道当前帧位置预测
|
|
|
PIPE* pPipe = m_LockingPipe;
|
|
|
//上一帧跟踪结果
|
|
|
OBJECTSTATUS* pObjStatus = &this->m_ObjStatus;
|
|
|
|
|
|
//根据目标速度预测目标在当前帧的坐标位置
|
|
|
nEnd = m_LockingPipe->ubEnd;
|
|
|
|
|
|
// 取长时预测点作为管道当前位置预测
|
|
|
pPipe->ptCurrentPnt.x = pPipe->stMotionMod_mean.crnObjPrediRtLong.cx;
|
|
|
pPipe->ptCurrentPnt.y = pPipe->stMotionMod_mean.crnObjPrediRtLong.cy;
|
|
|
|
|
|
//判断管道目标是否超出视场
|
|
|
//if (IMGO_IsPoint16SOutImg(nWidth, nHeight, pPipe->ptCurrentPnt))
|
|
|
if(IMGO_IsPoint32FOutImg(nWidth, nHeight, pPipe->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 = pPipe->ptCurrentPnt.x - snPipeRadius.w;
|
|
|
mmCenterRect.maxX = pPipe->ptCurrentPnt.x + snPipeRadius.w - 1;
|
|
|
mmCenterRect.minY = pPipe->ptCurrentPnt.y - snPipeRadius.h;
|
|
|
mmCenterRect.maxY = pPipe->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::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 (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)
|
|
|
{
|
|
|
// 当前帧预测位置
|
|
|
POINT32F predictPos = pPipe->ptCurrentPnt;
|
|
|
|
|
|
// 目标大小
|
|
|
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 = 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 (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;
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
// 基于特征加权融合的相似度计算，提取最优跟踪目标
|
|
|
SINT32 SA_Tracker::FindMatchTarget(PIPE* pPipe, TARGET_OBJECT* ptTargetArray, SINT32 nFrmObjsCnt, SINT32 nPipeRadius, FLOAT32 fAglReso)
|
|
|
{
|
|
|
//局部变量
|
|
|
FLOAT32 fSim = 0.0f; //目标相似度
|
|
|
FLOAT32 fSimMax = -1.0f;//目标相似度最大值
|
|
|
FLOAT32 fSimThres = 0.7f; //相似度阈值
|
|
|
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 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; //候选目标运动相似度
|
|
|
|
|
|
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);
|
|
|
if (Arith_DIM == ptTarget->nObjTypeSrc && fMoveSim<0.9)
|
|
|
{
|
|
|
continue;
|
|
|
}
|
|
|
fSim = fAppSim * fApparentModelWeight + fMoveSim * fMoveModelWeight;
|
|
|
|
|
|
|
|
|
ptTarget->fMatchConf = fSim;
|
|
|
|
|
|
SINT32 nDSmpScale = pDAT_Module->GetDatParm()->nDSmpScale;
|
|
|
//LSBAO, 20160705: 若尺寸宽高方向同时变化超过2个像素或者宽高单方向变化超过3个像素，则认为目标大小突变 -- 丢失超过1S不判断
|
|
|
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;
|
|
|
}
|
|
|
|
|
|
if (fSim > fSimThres && fMoveSim > fMoveThres)
|
|
|
{
|
|
|
pPipe->unSimTargetNum++;
|
|
|
}
|
|
|
|
|
|
if (fSim > fSimMax)
|
|
|
{
|
|
|
fSimMax = fSim;
|
|
|
nSimTargetId = i;
|
|
|
fMoveSim_Target = fMoveSim;
|
|
|
}
|
|
|
|
|
|
}
|
|
|
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)
|
|
|
{
|
|
|
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);
|
|
|
|
|
|
// 大于DAT_TARGET_MIN的目标可能是临界目标
|
|
|
if (ptTargetArray[nSimTargetId].unObjPxlsCnt < 6
|
|
|
|| ptTarget->unObjPxlsCnt < 6)
|
|
|
{
|
|
|
if (fSScaleChangeHighThres < fPixChange || fSScaleChangeLowThres > fPixChange)
|
|
|
{
|
|
|
fSimMax = -1;
|
|
|
}
|
|
|
}
|
|
|
else if (fSScaleChangeHighThres < fSizeChange || fSScaleChangeLowThres > fSizeChange)
|
|
|
{
|
|
|
fSimMax = -1;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
|
|
|
}
|
|
|
|
|
|
// 目标运动距离强逻辑
|
|
|
ptTarget = &ptTargetArray[nSimTargetId];
|
|
|
//CENTERRECT* pcrnSrRect = &g_GLB_stOutput.crnObjPrediRtLong;
|
|
|
//CENTERRECT* pcrnSrRect = &g_GLB_stOutput.crnObjPrediRtNear;
|
|
|
|
|
|
// 用预测位置和上一帧的位置，防止跟踪跳转到错误的目标
|
|
|
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 ((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 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; //相似度阈值
|
|
|
SINT32 nFrmsStep = 1; //帧间隔
|
|
|
TARGET_OBJECT* ptLockingTarget = NULL; // 主目标管道指向的目标
|
|
|
TARGET_OBJECT *pCandidateTarget = NULL; //管道数组指向的目标
|
|
|
BYTE8 nSimTargetId = -1; //与管道差异最小的目标编号
|
|
|
|
|
|
SINT32 nFrmObjsCnt = 0; // 管道框在关联区域内目标计数
|
|
|
FLOAT32 fMoveModelWeight = 0.7;//运动模型权值
|
|
|
FLOAT32 fApparentModelWeight = 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)
|
|
|
{
|
|
|
return -1;
|
|
|
}
|
|
|
|
|
|
//基于频率控制AI识别使用
|
|
|
if (pTSA_Param->nUseAIDetFeq > 0)
|
|
|
{
|
|
|
pTSA_Param->nUseAIDetFeq -= 1;
|
|
|
return -1;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
pTSA_Param->nUseAIDetFeq = 25;
|
|
|
}
|
|
|
|
|
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++
|
|
|
//若长短轨迹预测异常，则直接取距离搜索区域中心最近的目标
|
|
|
if (stMotionMod_mean.bObjPredictAbnormal
|
|
|
&& m_LockingPipe->unLostCnt > 2)
|
|
|
{
|
|
|
fMoveModelWeight = 1.0;
|
|
|
fApparentModelWeight = 0.0;
|
|
|
}
|
|
|
|
|
|
//锁定后的50帧优先使用位置信息，降低锁定后立即丢失
|
|
|
//if(g_GLB_stOutput.ObjectStatus.unTotalCnt < 50)
|
|
|
//{
|
|
|
// fMoveModelWeight = 1.0;
|
|
|
// fApparentModelWeight = 0.0;
|
|
|
//}
|
|
|
|
|
|
RECT32S LockingPipeBox, newLockingPipeBox;
|
|
|
LockingPipeBox.x = pLockingPipe->objHistoryList[pLockingPipe->ubEnd].pfCenPos.x - nPipeRadius;
|
|
|
LockingPipeBox.y = pLockingPipe->objHistoryList[pLockingPipe->ubEnd].pfCenPos.y - 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 (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 targerIOU = IoUA32S(&newLockingPipeBox, &newPipeBox);
|
|
|
if (targerIOU > 0.1)
|
|
|
{
|
|
|
nFrmObjsCnt += 1;
|
|
|
}
|
|
|
|
|
|
pCandidateTarget = &pPipe->objHistoryList[pPipe->ubEnd];
|
|
|
|
|
|
// 计算表观相似度
|
|
|
FLOAT32 fAppSim = Similarity_Apparent(pLockingPipe, pCandidateTarget);
|
|
|
|
|
|
// 计算运动相似度
|
|
|
FLOAT32 fMoveSim = Similarity_Move(pLockingPipe, pCandidateTarget, fAglReso);
|
|
|
|
|
|
fSim = fAppSim * fApparentModelWeight + fMoveSim * fMoveModelWeight;
|
|
|
|
|
|
|
|
|
pCandidateTarget->fMatchConf = fSim;
|
|
|
|
|
|
// 获得降采样倍数
|
|
|
SINT32 nDSmpScale = pDAT_Module->GetDatParm()->nDSmpScale;
|
|
|
//LSBAO, 20160705: 若尺寸宽高方向同时变化超过2个像素或者宽高单方向变化超过5个像素，则认为目标大小突变
|
|
|
if (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;
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
// 单一目标条件判断：当只有一个目标时，关闭大部分条件尽力跟踪，比较符合人眼规律。
|
|
|
// 由于跟踪时谨慎合并目标，很容易导致临界目标被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;
|
|
|
// }
|
|
|
//}
|
|
|
|
|
|
|
|
|
|
|
|
// 防跳变// by wcw04046 @ 2020/06/22
|
|
|
if (nSimTargetId != -1)
|
|
|
{
|
|
|
pCandidateTarget = &m_PipeArray[nSimTargetId].objHistoryList[m_PipeArray[nSimTargetId].ubEnd];
|
|
|
if (m_ObjStatus.unContiLostCnt < 100)
|
|
|
{
|
|
|
ptLockingTarget = &pLockingPipe->objHistoryList[pLockingPipe->ubEnd];
|
|
|
|
|
|
FLOAT32 fSizeChange = 0.0f;
|
|
|
FLOAT32 fPixChange = 0.0f;
|
|
|
|
|
|
fSizeChange = (FLOAT32)(pCandidateTarget->snSize.s) / MAX(ptLockingTarget->snSize.s, 0.001f);
|
|
|
fPixChange = (FLOAT32)(pCandidateTarget->unObjPxlsCnt) / MAX(ptLockingTarget->unObjPxlsCnt, 0.001f);
|
|
|
|
|
|
// 大于DAT_TARGET_MIN的目标可能是临界目标
|
|
|
if (pCandidateTarget->unObjPxlsCnt < 6 || ptLockingTarget->unObjPxlsCnt < 6)
|
|
|
{
|
|
|
if (12 < fPixChange || 0.1 > fPixChange)
|
|
|
{
|
|
|
fSimMax = -1;
|
|
|
}
|
|
|
}
|
|
|
else if (12 < fSizeChange || 0.1 > fSizeChange)
|
|
|
{
|
|
|
fSimMax = -1;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
|
|
|
}
|
|
|
}
|
|
|
// 目标运动距离强逻辑
|
|
|
//ptTarget = &ptTargetArray[nSimTargetId];
|
|
|
//CENTERRECT* pcrnSrRect = &g_GLB_stOutput.crnObjPrediRtLong;
|
|
|
//CENTERRECT* pcrnSrRect = &g_GLB_stOutput.crnObjPrediRtNear;
|
|
|
|
|
|
// 用预测位置和上一帧的位置，防止跟踪跳转到错误的目标
|
|
|
|
|
|
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; // 与上一帧的像素差阈值
|
|
|
|
|
|
// 没得选了
|
|
|
if (m_ObjStatus.unContiLostCnt >= 3 && bSingleTarget)
|
|
|
{
|
|
|
nPredictDiffThresX = MAX(70, nPipeRadius * 2);
|
|
|
nPredictDiffThresY = MAX(70, nPipeRadius * 2);
|
|
|
nLastDiffThresX = nPipeRadius * 2;//波门范围直接取
|
|
|
nLastDiffThresY = nPipeRadius * 2;//波门范围直接取
|
|
|
}
|
|
|
|
|
|
nPredictDiffThresX = MAX(pCandidateTarget->snSize.w / 2, nPredictDiffThresX);
|
|
|
nPredictDiffThresY = MAX(pCandidateTarget->snSize.h / 2, nPredictDiffThresY);
|
|
|
|
|
|
nLastDiffThresX = MAX(pCandidateTarget->snSize.w / 2, nLastDiffThresX);
|
|
|
nLastDiffThresY = MAX(pCandidateTarget->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;
|
|
|
//}
|
|
|
|
|
|
// 限定AI识别在图像中心位置，缓解AI识别结果帧同步问题
|
|
|
BBOOL st1 = ABS(pCandidateTarget->pfCenPos.x - nWidth / 2) < 50 && ABS(pCandidateTarget->pfCenPos.y - nHeight / 2) < 50;
|
|
|
if (FALSE == st1)
|
|
|
{
|
|
|
fSimMax = -1;
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
// 距离限定强逻辑，避免错误关联
|
|
|
if ((ABS(pCandidateTarget->pfCenPos.x - stMotionMod_mean.crnObjPrediRtLong.cx) > MAX(ABS(fAzSpeed) * 2, nPredictDiffThresX)
|
|
|
|| ABS(pCandidateTarget->pfCenPos.y - stMotionMod_mean.crnObjPrediRtLong.cy) > MAX(ABS(fPtSpeed) * 2, nPredictDiffThresY))
|
|
|
&& (ABS(pCandidateTarget->pfCenPos.x - m_ObjStatus.ptPos.x) > MAX(ABS(fAzSpeed) * 2, nLastDiffThresX)
|
|
|
|| ABS(pCandidateTarget->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_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, nWidth, nHeight, roi, 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(SINT32 nWidth, SINT32 nHeight,TARGET_OBJECT* pTarget, GLB_INPUT* p_GLB_Input)
|
|
|
{
|
|
|
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;
|
|
|
|
|
|
// 其他特征均不更新
|
|
|
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
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->fObjPxlsCnt = pTarget->unObjPxlsCnt;
|
|
|
|
|
|
// 滤波值
|
|
|
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->fConfidence = pTarget->fMatchConf;
|
|
|
|
|
|
// 跟踪器目标来源
|
|
|
pObjStatus->nObjTypeSrc = pTarget->nObjTypeSrc;
|
|
|
|
|
|
pObjStatus->bObjMiss = false;
|
|
|
}
|
|
|
|
|
|
//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 ((pObjStatus->unContiLostCnt > (UINT32)(m_TSA_Param.nPipeFOVLostCnt))
|
|
|
|| (m_LockingPipe->unContinueOutFOVCnt>10))
|
|
|
//|| (!bInsideFOV /*&& !g_GLB_stPara.bSetMemoryTrack*/))
|
|
|
{
|
|
|
pObjStatus->bObjMiss = true;
|
|
|
}
|
|
|
|
|
|
|
|
|
////更新目标速度
|
|
|
//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);
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
//// 基于特征加权融合的相似度计算，提取最优跟踪管道
|
|
|
//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;
|
|
|
//}
|
|
|
|
|
|
#endif |
