|
|
#include "Arith_Tracker.h"
|
|
|
#include "PIPE/API_MOT_PipeProc.h"
|
|
|
#include "Arith_Common.hpp"
|
|
|
#include <string>
|
|
|
|
|
|
// 模拟多态，有限类别手动指派，考虑到使用继承的效率和兼容性问题。
|
|
|
// Tracker::Tracker(SINT32 nWidth, SINT32 nHeight, GLB_SCEN_MODE type)
|
|
|
// {
|
|
|
// pGroundTracker = NULL;
|
|
|
// pSkyTracker = NULL;
|
|
|
// pAitTracker = NULL;
|
|
|
// pCustomTracker = NULL;
|
|
|
|
|
|
// if (type == GLB_SCEN_MODE::GLB_SCEN_SKY)
|
|
|
// {
|
|
|
// pSkyTracker = new SkyTracker(nWidth,nHeight);
|
|
|
// }
|
|
|
// else if(type == GLB_SCEN_MODE::GLB_SCEN_GROUND)
|
|
|
// {
|
|
|
// pGroundTracker = new GroundTracker(nWidth, nHeight);
|
|
|
// }
|
|
|
// else if (type == GLB_SCEN_MODE::GLB_SCEN_AIT)
|
|
|
// {
|
|
|
// std::string JsPath = std::string(SOURCE_PATH) + "/AITracker/x86_onnx/gd_ai_nanotrack_v3_241009.json";
|
|
|
// pAitTracker = new AIT_Interface(JsPath.c_str());
|
|
|
// //pAitTracker = new AIT_Interface();
|
|
|
// }
|
|
|
// else if(type == GLB_SCEN_MODE::GLB_SCEN_CUSTOM)
|
|
|
// {
|
|
|
// pCustomTracker = new CustomTracker(nWidth,nHeight);
|
|
|
// }
|
|
|
|
|
|
// m_LockingPipe = NULL;
|
|
|
|
|
|
// // 跟踪器输出
|
|
|
// ObjStatus = { 0 };
|
|
|
|
|
|
// m_type = type;
|
|
|
|
|
|
// emTUFlag = LockStateUnknown; // 目标锁定解锁标记
|
|
|
// }
|
|
|
|
|
|
Tracker::Tracker(SINT32 nWidth, SINT32 nHeight, GLB_SCEN_MODE type, ARIDLL_PARMA* pstArithPara)
|
|
|
{
|
|
|
pGroundTracker = NULL;
|
|
|
pSkyTracker = NULL;
|
|
|
pAitTracker = NULL;
|
|
|
pCustomTracker = NULL;
|
|
|
|
|
|
if (type == GLB_SCEN_MODE::GLB_SCEN_SKY)
|
|
|
{
|
|
|
pSkyTracker = new SkyTracker(nWidth, nHeight, pstArithPara->stSkyParam);
|
|
|
}
|
|
|
else if(type == GLB_SCEN_MODE::GLB_SCEN_GROUND)
|
|
|
{
|
|
|
pGroundTracker = new GroundTracker(nWidth, nHeight, pstArithPara->stGrdParam);
|
|
|
}
|
|
|
// else if (type == GLB_SCEN_MODE::GLB_SCEN_AIT)
|
|
|
// {
|
|
|
////pAitTracker = new AIT_Interface();
|
|
|
////std::string JsPath = std::string(SOURCE_PATH) + "/AITracker/x86_onnx/gd_ai_nanotrack_v3_241009.json";
|
|
|
////pAitTracker = new AIT_Interface(JsPath.c_str());
|
|
|
|
|
|
//pAitTracker = new AIT_Interface(pstArithPara->stAITParam);
|
|
|
|
|
|
////pAitTracker->nTracker = AIT_GetHandle();
|
|
|
////printf ("AIT_Interface:[%s]\n",JsPath);
|
|
|
////pAitTracker = new AIT_Interface();
|
|
|
// }
|
|
|
else if(type == GLB_SCEN_MODE::GLB_SCEN_CUSTOM)
|
|
|
{
|
|
|
pCustomTracker = new CustomTracker(nWidth,nHeight);
|
|
|
}
|
|
|
|
|
|
m_LockingPipe = NULL;
|
|
|
|
|
|
// 跟踪器输出
|
|
|
ObjStatus = { 0 };
|
|
|
|
|
|
m_type = type;
|
|
|
|
|
|
emTUFlag = LockStateUnknown; // 目标锁定解锁标记
|
|
|
}
|
|
|
|
|
|
|
|
|
Tracker::~Tracker()
|
|
|
{
|
|
|
if (pSkyTracker)
|
|
|
{
|
|
|
delete pSkyTracker;
|
|
|
pSkyTracker = nullptr;
|
|
|
}
|
|
|
|
|
|
if (pGroundTracker)
|
|
|
{
|
|
|
delete pGroundTracker;
|
|
|
pGroundTracker = nullptr;
|
|
|
}
|
|
|
|
|
|
if (pAitTracker)
|
|
|
{
|
|
|
delete pAitTracker;
|
|
|
pAitTracker = nullptr;
|
|
|
}
|
|
|
|
|
|
if(pCustomTracker)
|
|
|
{
|
|
|
delete pCustomTracker;
|
|
|
pCustomTracker = NULL;
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
bool Tracker::Init(GD_VIDEO_FRAME_S img, PIPE* pLockPipe, GLB_INPUT* p_GLB_Input)
|
|
|
{
|
|
|
m_LockingPipe = pLockPipe;
|
|
|
|
|
|
memset(&ObjStatus,0,sizeof(OBJECTSTATUS));
|
|
|
|
|
|
BBOOL bInitSucFlag = false;
|
|
|
|
|
|
if (m_type == GLB_SCEN_MODE::GLB_SCEN_SKY)
|
|
|
{
|
|
|
bInitSucFlag = pSkyTracker->Init(img, pLockPipe, p_GLB_Input);
|
|
|
}
|
|
|
else if (m_type == GLB_SCEN_MODE::GLB_SCEN_GROUND)
|
|
|
{
|
|
|
bInitSucFlag = pGroundTracker->Init(img, pLockPipe, p_GLB_Input);
|
|
|
}
|
|
|
// else if (m_type == GLB_SCEN_MODE::GLB_SCEN_AIT)
|
|
|
// {
|
|
|
//bInitSucFlag = pAitTracker->AIT_Init(img, pLockPipe, p_GLB_Input);
|
|
|
// }
|
|
|
else if(m_type == GLB_SCEN_MODE::GLB_SCEN_CUSTOM)
|
|
|
{
|
|
|
bInitSucFlag = pCustomTracker->Init(img, pLockPipe, p_GLB_Input);
|
|
|
}
|
|
|
|
|
|
return bInitSucFlag;
|
|
|
}
|
|
|
|
|
|
bool Tracker::Track(GD_VIDEO_FRAME_S img, GLB_INPUT* p_GLB_Input, API_MOT_PIPE* g_GLB_PipeProc, ARIDLL_PARMA* g_stArithPara)
|
|
|
{
|
|
|
bool bTrackStatus = false;
|
|
|
// 异常保护
|
|
|
if (m_LockingPipe == NULL || !m_LockingPipe->bTrackingPipe)
|
|
|
{
|
|
|
return false;
|
|
|
}
|
|
|
// 基于窗口平均计算管道长短时预测点
|
|
|
Predict_ObjAglTrackPredict(&m_LockingPipe->stMotionMod_mean, img.u32Width, img.u32Height, p_GLB_Input);
|
|
|
|
|
|
|
|
|
if (m_LockingPipe->stMotionMod_mean.bTrackStable)
|
|
|
{
|
|
|
m_LockingPipe->afCurrentAgl = m_LockingPipe->stMotionMod_mean.ObjAglListsLong.arfFilter.afAngle;
|
|
|
m_LockingPipe->ptCurrentPnt.x = m_LockingPipe->stMotionMod_mean.crnObjPrediRtLong.cx;
|
|
|
m_LockingPipe->ptCurrentPnt.y = m_LockingPipe->stMotionMod_mean.crnObjPrediRtLong.cy;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
m_LockingPipe->afCurrentAgl = m_LockingPipe->stMotionMod_mean.ObjAglListsNear.arfFilter.afAngle;
|
|
|
m_LockingPipe->ptCurrentPnt.x = m_LockingPipe->stMotionMod_mean.crnObjPrediRtNear.cx;
|
|
|
m_LockingPipe->ptCurrentPnt.y = m_LockingPipe->stMotionMod_mean.crnObjPrediRtNear.cy;
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
// 跟踪找到目标
|
|
|
if (m_type == GLB_SCEN_MODE::GLB_SCEN_SKY)
|
|
|
{
|
|
|
emTUFlag = pSkyTracker->Track(img, p_GLB_Input, g_GLB_PipeProc, g_stArithPara, m_type);
|
|
|
ObjStatus = pSkyTracker->ObjStatus;
|
|
|
}
|
|
|
else if (m_type == GLB_SCEN_MODE::GLB_SCEN_GROUND)
|
|
|
{
|
|
|
bTrackStatus = pGroundTracker->Track(img, p_GLB_Input, g_GLB_PipeProc, m_type);
|
|
|
ObjStatus = pGroundTracker->ObjStatus;
|
|
|
}
|
|
|
//else if (m_type == GLB_SCEN_MODE::GLB_SCEN_AIT)
|
|
|
//{
|
|
|
// bTrackStatus = pAitTracker->AIT_Run(img, p_GLB_Input, m_LockingPipe);
|
|
|
// ObjStatus = pAitTracker->m_ObjStatus;
|
|
|
//}
|
|
|
else if (m_type == GLB_SCEN_MODE::GLB_SCEN_CUSTOM)
|
|
|
{
|
|
|
bTrackStatus = pCustomTracker->Track(img, p_GLB_Input, g_GLB_PipeProc);
|
|
|
ObjStatus = pCustomTracker->ObjStatus;
|
|
|
}
|
|
|
|
|
|
// 将跟踪结果输出到管道
|
|
|
UpdateTracker2Pipe(p_GLB_Input, m_type, m_LockingPipe, g_GLB_PipeProc);
|
|
|
|
|
|
// 返回跟踪状态
|
|
|
return bTrackStatus;
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
SINT32 Tracker::MemTrack(GD_VIDEO_FRAME_S img, GLB_INPUT* p_GLB_Input, API_MOT_PIPE* g_GLB_PipeProc, ARIDLL_PARMA* g_stArithPara)
|
|
|
{
|
|
|
bool bTrackStatus = false;
|
|
|
|
|
|
// 基于窗口平均计算管道长短时预测点
|
|
|
Predict_ObjAglTrackPredict(&m_LockingPipe->stMotionMod_mean, img.u32Width, img.u32Height, p_GLB_Input);
|
|
|
|
|
|
m_LockingPipe->afCurrentAgl = m_LockingPipe->stMotionMod_mean.ObjAglListsNear.arfFilter.afAngle;
|
|
|
m_LockingPipe->ptCurrentPnt.x = m_LockingPipe->stMotionMod_mean.crnObjPrediRtNear.cx;
|
|
|
m_LockingPipe->ptCurrentPnt.y = m_LockingPipe->stMotionMod_mean.crnObjPrediRtNear.cy;
|
|
|
|
|
|
// 跟踪找到目标
|
|
|
if (m_type == GLB_SCEN_MODE::GLB_SCEN_SKY)
|
|
|
{
|
|
|
emTUFlag = pSkyTracker->MemTrack(img, p_GLB_Input, g_GLB_PipeProc, g_stArithPara, m_type);
|
|
|
ObjStatus = pSkyTracker->ObjStatus;
|
|
|
}
|
|
|
else if (m_type == GLB_SCEN_MODE::GLB_SCEN_GROUND)
|
|
|
{
|
|
|
bTrackStatus = pGroundTracker->MemTrack(img, p_GLB_Input, g_GLB_PipeProc, m_type);
|
|
|
ObjStatus = pGroundTracker->ObjStatus;
|
|
|
}
|
|
|
//else if (m_type == GLB_SCEN_MODE::GLB_SCEN_AIT)
|
|
|
//{
|
|
|
// //bTrackStatus = pAitTracker->AIT_Run(img, p_GLB_Input, m_LockingPipe);
|
|
|
// //ObjStatus = pAitTracker->m_ObjStatus;
|
|
|
//}
|
|
|
else if (m_type == GLB_SCEN_MODE::GLB_SCEN_CUSTOM)
|
|
|
{
|
|
|
//bTrackStatus = pCustomTracker->Track(img, p_GLB_Input, g_GLB_PipeProc);
|
|
|
//ObjStatus = pCustomTracker->ObjStatus;
|
|
|
}
|
|
|
|
|
|
// 将跟踪结果输出到管道
|
|
|
UpdateTracker2Pipe(p_GLB_Input, m_type, m_LockingPipe, g_GLB_PipeProc);
|
|
|
|
|
|
// 返回跟踪状态
|
|
|
return bTrackStatus;
|
|
|
}
|
|
|
|
|
|
// 解锁：清除管道锁定标记
|
|
|
void Tracker::Cancle()
|
|
|
{
|
|
|
if (m_type == GLB_SCEN_MODE::GLB_SCEN_SKY)
|
|
|
{
|
|
|
pSkyTracker->Cancle();
|
|
|
}
|
|
|
else if(m_type == GLB_SCEN_MODE::GLB_SCEN_GROUND)
|
|
|
{
|
|
|
pGroundTracker->Cancle();
|
|
|
}
|
|
|
// else if (m_type == GLB_SCEN_MODE::GLB_SCEN_AIT)
|
|
|
// {
|
|
|
//pAitTracker->AIT_Cancle();
|
|
|
// }
|
|
|
else if(m_type == GLB_SCEN_MODE::GLB_SCEN_CUSTOM)
|
|
|
{
|
|
|
pCustomTracker->Cancle();
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// 使用新管道初始化跟踪器，且不改变原跟踪批号、管道号以及跟踪器编号。
|
|
|
bool Tracker::ResetByBackPipe(PIPE* pNewPipe, GD_VIDEO_FRAME_S img, GLB_INPUT* p_GLB_Input,BBOOL bReInit)
|
|
|
{
|
|
|
BBOOL bInitSucc = TRUE;
|
|
|
// 异常保护
|
|
|
if (m_LockingPipe == NULL || !m_LockingPipe->bTrackingPipe)
|
|
|
{
|
|
|
return false;
|
|
|
}
|
|
|
|
|
|
// 正在跟踪的管道
|
|
|
PIPE * pPipeOld = m_LockingPipe;
|
|
|
// 遗传告警信息
|
|
|
SINT32 nInAlarmArrayIndex_1 = pPipeOld->nInAlarmArrayIndex_1;//管道位于告警队列位置，从1开始
|
|
|
SINT32 nAlarmBatchID_1 = pPipeOld->nAlarmBatchID_1;//管道告警批号，从1开始
|
|
|
SINT32 nGuideBatchID= pPipeOld->nGuideBatchID;//外部批号，如果存在外部批号，则使用外部批号送显，内部批号不变，灵活度高
|
|
|
|
|
|
// 将新管道信息完全拷贝到跟踪管道
|
|
|
memcpy(pPipeOld,pNewPipe,sizeof(PIPE));
|
|
|
|
|
|
// 恢复原管道告警关联信息
|
|
|
pPipeOld->nInAlarmArrayIndex_1 = nInAlarmArrayIndex_1;
|
|
|
pPipeOld->nAlarmBatchID_1 = nAlarmBatchID_1;
|
|
|
pPipeOld->nGuideBatchID = nGuideBatchID;
|
|
|
|
|
|
// 恢复跟踪标记
|
|
|
pPipeOld->bTrackingPipe = true;
|
|
|
|
|
|
// 新管道被eat,需完全删除，交外部执行，tracker没这个权限。
|
|
|
|
|
|
|
|
|
// 跟踪器初始化
|
|
|
bInitSucc = Init(img,m_LockingPipe,p_GLB_Input);
|
|
|
|
|
|
|
|
|
return bInitSucc;
|
|
|
}
|
|
|
|
|
|
//Tracker_Param Tracker::GetParam()
|
|
|
//{
|
|
|
// return Tracker_Param();
|
|
|
//}
|
|
|
|
|
|
|
|
|
RECT32S Tracker::getTrackerBox()
|
|
|
{
|
|
|
RECT32S rc = { 0 };
|
|
|
rc.x = SINT32(ObjStatus.ptPos.x - ObjStatus.sfSize.w / 2);
|
|
|
rc.y = SINT32(ObjStatus.ptPos.y - ObjStatus.sfSize.h / 2);
|
|
|
rc.w = SINT32(ObjStatus.sfSize.w);
|
|
|
rc.h = SINT32(ObjStatus.sfSize.h);
|
|
|
|
|
|
return rc;
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
* 将跟踪器结果输出为管道目标信息，不要丢特征，否则可能导致SA跟踪器特征突变一直无法跟踪
|
|
|
*/
|
|
|
bool Tracker::UpdateTracker2Pipe(GLB_INPUT* p_GLB_Input,GLB_SCEN_MODE type, PIPE* pPipeOut, API_MOT_PIPE* g_GLB_PipeProc)
|
|
|
{
|
|
|
PIPE* pPipe = pPipeOut;
|
|
|
|
|
|
if (!pPipe->bTrackingPipe)
|
|
|
{
|
|
|
g_GLB_PipeProc->DelPipe(pPipe);
|
|
|
return false;
|
|
|
}
|
|
|
|
|
|
// 总帧数+1
|
|
|
pPipe->unTotalCnt++;
|
|
|
|
|
|
// 目标丢失
|
|
|
if (ObjStatus.unContiLostCnt > 0)
|
|
|
{
|
|
|
//标记管道丢失
|
|
|
pPipe->bLost = true;
|
|
|
pPipe->unLostCnt++;
|
|
|
pPipe->unContinueExistCnt = 0;
|
|
|
|
|
|
// 用跟踪器预测位置，更新管道预测点
|
|
|
pPipe->ptCurrentPnt.x = ObjStatus.ptPos.x;
|
|
|
pPipe->ptCurrentPnt.y = ObjStatus.ptPos.y;
|
|
|
pPipe->afCurrentAgl.fAz = ObjStatus.afAngle.fAz;
|
|
|
pPipe->afCurrentAgl.fPt = ObjStatus.afAngle.fPt;
|
|
|
|
|
|
// ab滤波器预测
|
|
|
Filter_Predict(&pPipe->stMotionMod, p_GLB_Input->unFrmId);
|
|
|
|
|
|
// 3d滤波器预测
|
|
|
PodXYZFilter_Predict(&pPipe->stXYZMotionMod, p_GLB_Input->unFrmId);
|
|
|
}
|
|
|
// 目标更新
|
|
|
else
|
|
|
{
|
|
|
//获取在当前帧中查找到的最相似目标
|
|
|
//KCF获得的目标只有位置和尺度信息
|
|
|
TARGET_OBJECT Target = { 0 };
|
|
|
Target.unFrmID = p_GLB_Input->unFrmId;
|
|
|
Target.bObject = true;
|
|
|
Target.pfCenPos = ObjStatus.ptPos;
|
|
|
//ptTarget.pnMaxPos = ptTarget.pfCenPos;
|
|
|
Target.afAngle = ObjStatus.afAngle;
|
|
|
Target.unClsType = ObjStatus.unClsType;
|
|
|
|
|
|
// 3D坐标计算
|
|
|
Pole targetPole = {0};
|
|
|
targetPole.alpha = ObjStatus.afAngle.fPt;
|
|
|
targetPole.beta = ObjStatus.afAngle.fAz;
|
|
|
Target.pos3d = getXYZFromPole_withHeight(targetPole,p_GLB_Input->nElevationDiff);
|
|
|
|
|
|
Target.fMatchConf = ObjStatus.fConfidence;
|
|
|
|
|
|
// 尺度，重要，KCF和SA之间除位置外唯一的切换特征
|
|
|
Target.snSize.w = (SINT16)ObjStatus.sfSize.w;
|
|
|
Target.snSize.h = (SINT16)ObjStatus.sfSize.h;
|
|
|
Target.snSize.s = (SINT32)ObjStatus.sfSize.s;
|
|
|
|
|
|
Target.snAIDetSize.w = (SINT16)ObjStatus.snAIDetSize.w;
|
|
|
Target.snAIDetSize.h = (SINT16)ObjStatus.snAIDetSize.h;
|
|
|
Target.snAIDetSize.s = (SINT32)ObjStatus.snAIDetSize.s;
|
|
|
|
|
|
Target.nObjTypeSrc = ObjStatus.nObjTypeSrc;
|
|
|
Target.unObjPxlsCnt = (UINT32)ObjStatus.fObjPxlsCnt;
|
|
|
Target.pxObjGray = ObjStatus.pxObjGray;
|
|
|
Target.fSNR = ObjStatus.fSNR;
|
|
|
Target.emClsSrc = ObjStatus.emClsSrc;
|
|
|
Target.fBGMean = ObjStatus.fBGMean;
|
|
|
Target.fBGStd = ObjStatus.fBGStd;
|
|
|
|
|
|
//// 如果对空小面目标跟踪器已经检出目标，且是融合决策目标，直接加入管道
|
|
|
//if (type == GLB_SCEN_MODE::GLB_SCEN_SKY && pSkyTracker->mTargetFusion.bObject)
|
|
|
//{
|
|
|
// Target = pSkyTracker->mTargetFusion;
|
|
|
//}
|
|
|
UINT16 fGrayFilter; // 灰度滤波值
|
|
|
POINT32F ptCentroid; // 质心定位置
|
|
|
|
|
|
// 将跟踪决策目标加入管道
|
|
|
g_GLB_PipeProc->PIPE_AddObjToOccupiedPipe(pPipe, pPipe->nPipeID, &Target, 0, p_GLB_Input);
|
|
|
|
|
|
// LOG_INFO("FrmId:{0},Cx:{1},Cy:{2},fAz:{3},fPt:{4},AlgSpeedX:{5},AlgSpeedY:{6},pixSpeedX:{7},pixSpeedY:{8},fObjStd:{9},fBGStd:{10},fSNR:{11},fObjPxlsCnt:{12},fGrayFilter:{13},ptCentroidX:{14},ptCentroidX:{15}",
|
|
|
// p_GLB_Input->unFrmId, ObjStatus.ptPos.x, ObjStatus.ptPos.y,
|
|
|
// ObjStatus.afAngle.fAz, ObjStatus.afAngle.fPt, ObjStatus.sfAglSpeed.vx, ObjStatus.sfAglSpeed.vy,
|
|
|
// ObjStatus.sfSpeed.vx, ObjStatus.sfSpeed.vy, ObjStatus.fObjStd, ObjStatus.fBGStd, ObjStatus.fSNR, ObjStatus.fObjPxlsCnt,
|
|
|
// pPipe->ObjectFilter.fGray, ObjStatus.ptCentroid.x, ObjStatus.ptCentroid.y
|
|
|
// );
|
|
|
}
|
|
|
|
|
|
return true;
|
|
|
}
|
|
|
|
|
|
|
|
|
// 返回对地跟踪器的TLD模块
|
|
|
API_TLD * Tracker::GetpTLDTracker()
|
|
|
{
|
|
|
if (pGroundTracker)
|
|
|
{
|
|
|
return pGroundTracker->m_pTLDTracker;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
return NULL;
|
|
|
}
|
|
|
}
|
|
|
|
