S3315_RK3588/NeoTracker/src/Arith_Tracker.cpp

#include "Arith_Tracker.h"
#include "PIPE/API_MOT_PipeProc.h"
#include "Arith_Common.hpp"

// 模拟多态，有限类别手动指派，考虑到使用继承的效率和兼容性问题。
Tracker::Tracker(SINT32 nWidth, SINT32 nHeight, GLB_SCEN_MODE type)
{
	pGroundTracker = NULL;
	pSkyTracker = 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_CUSTOM)
	{
		pCustomTracker = new CustomTracker(nWidth,nHeight);
	}

	m_LockingPipe = NULL;

	// 跟踪器输出
	ObjStatus = { 0 };

	m_type = type;


}

Tracker::Tracker(SINT32 nWidth, SINT32 nHeight, GLB_SCEN_MODE type, Tracker_Param PARA)
{
	pGroundTracker = NULL;
	pSkyTracker = NULL;
	pCustomTracker = NULL;

	if (type == GLB_SCEN_MODE::GLB_SCEN_SKY)
	{
		pSkyTracker = new SkyTracker(nWidth, nHeight, PARA.skyParam);
	}
	else if(type == GLB_SCEN_MODE::GLB_SCEN_GROUND)
	{
		pGroundTracker = new GroundTracker(nWidth, nHeight, PARA.grdParam);
	}
	else if(type == GLB_SCEN_MODE::GLB_SCEN_CUSTOM)
	{
		pCustomTracker = new CustomTracker(nWidth,nHeight);
	}

	m_LockingPipe = NULL;

	// 跟踪器输出
	ObjStatus = { 0 };

	m_type = type;
}


Tracker::~Tracker()
{
	if (pSkyTracker)
	{
		delete pSkyTracker;
		pSkyTracker = nullptr;
	}

	if (pGroundTracker)
	{
		delete pGroundTracker;
		pGroundTracker = 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));

	if (m_type == GLB_SCEN_MODE::GLB_SCEN_SKY)
	{
		pSkyTracker->Init(img, pLockPipe, p_GLB_Input);
	}
	else if (m_type == GLB_SCEN_MODE::GLB_SCEN_GROUND)
	{
		pGroundTracker->Init(img, pLockPipe, p_GLB_Input);
	}
	else if(m_type == GLB_SCEN_MODE::GLB_SCEN_CUSTOM)
	{
		pCustomTracker->Init(img, pLockPipe, p_GLB_Input);
	}

	return true;
}

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;

	// 基于窗口平均计算管道长短时预测点
	Predict_ObjAglTrackPredict(&m_LockingPipe->stMotionMod_mean, img.u32Width, img.u32Height, p_GLB_Input);

	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.cx;
	
	// 跟踪找到目标
	if (m_type == GLB_SCEN_MODE::GLB_SCEN_SKY)
	{
		bTrackStatus = pSkyTracker->Track(img, p_GLB_Input, g_GLB_PipeProc, g_stArithPara);
		ObjStatus = pSkyTracker->ObjStatus;
	}
	else if (m_type == GLB_SCEN_MODE::GLB_SCEN_GROUND)
	{
		bTrackStatus = pGroundTracker->Track(img, p_GLB_Input, g_GLB_PipeProc);
		ObjStatus = pGroundTracker->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_CUSTOM)
	{
		pCustomTracker->Cancle();
	}

}

AIT_Command Tracker::GetAIT_Cmd()
{
	if(m_type == GLB_SCEN_MODE::GLB_SCEN_GROUND && pGroundTracker!=NULL)
	{
		return pGroundTracker->m_pAITracker->m_Ai_TkCmd;
	}

	return AIT_Command();
}

// 传输外部AI跟踪信息到跟踪器
void Tracker::SetAIT_Info(AIT_OUTPUT * out)
{
	if (m_type == GLB_SCEN_MODE::GLB_SCEN_GROUND && pGroundTracker!=NULL)
	{
		pGroundTracker->m_pAITracker->AIT_SyncInfo(out);
	}
	return;
}


// 使用新管道初始化跟踪器，且不改变原跟踪批号、管道号以及跟踪器编号。
bool Tracker::ResetByBackPipe(PIPE* pNewPipe, GD_VIDEO_FRAME_S img, GLB_INPUT* p_GLB_Input,BBOOL bReInit)
{
	// 异常保护
	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;
	pPipeOld->bTracking = false;
	// 新管道被eat,需完全删除，交外部执行，tracker没这个权限。


	// 跟踪器初始化
	Init(img,m_LockingPipe,p_GLB_Input);



	return true;
}

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;

	// 总帧数+1
	pPipe->unTotalCnt++;

	// 目标丢失
	if (ObjStatus.unContiLostCnt > 0)
	{
		//标记管道丢失
		pPipe->bLost = true;
		pPipe->unLostCnt++;
		pPipe->unContinueExistCnt = 0;

		// 用跟踪器预测位置，更新管道预测点
		pPipe->ptCurrentPnt.x = (SINT16)ObjStatus.ptPos.x;
		pPipe->ptCurrentPnt.y = (SINT16)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;

		// 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.nObjTypeSrc = ObjStatus.nObjTypeSrc;
		Target.unObjPxlsCnt = (UINT32)ObjStatus.fObjPxlsCnt;

		// 如果对空小面目标跟踪器已经检出目标，且是融合决策目标，直接加入管道
		if (type == GLB_SCEN_MODE::GLB_SCEN_SKY && pSkyTracker->mTargetFusion.bObject)
		{
			Target = pSkyTracker->mTargetFusion;
		}

		// 将跟踪决策目标加入管道
		g_GLB_PipeProc->PIPE_AddObjToOccupiedPipe(pPipe, pPipe->nPipeID, &Target, 0, p_GLB_Input);
	}

	return true;
}


// 返回对地跟踪器的TLD模块
API_TLD * Tracker::GetpTLDTracker()
{
	if (pGroundTracker)
	{
		return pGroundTracker->m_pTLDTracker;
	}
	else
	{
		return NULL;
	}
}