S3315_RK3588/NeoTracker/src/Arith_Tracker.cpp

#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);

	//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;

	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.ObjAglListsLong.arfFilter.afAngle;
	m_LockingPipe->ptCurrentPnt.x = m_LockingPipe->stMotionMod_mean.crnObjPrediRtLong.cx;
	m_LockingPipe->ptCurrentPnt.y = m_LockingPipe->stMotionMod_mean.crnObjPrediRtLong.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;
	}
}