S3315_RK3588/NeoTracker/src/Arith_ObjArrest.cpp

/*********版权所有（C）2024，武汉高德红外股份有限公司***************
* 文件名称： Arith_ObjArrest.cpp
* 文件标识： 跟踪目标重捕时机的返回
* 内容摘要：
* 其它说明： "Arith_ObjArrest.cpp"的函数、全局变量、宏定义，统一前缀为简写"ObjArrest_"
* 当前版本： 固化V2.0
* 创建作者： lk
* 创建日期： 2024.01.23
*************************************************************************/
#include "Arith_OccludeJudge.h"
#include "Arith_ImgOperate.h"
#include "Arith_ObjArrest.h"
#include "Arith_CoordModule.h"
#include "TLD/API_TLD.h"



TargetArrest::TargetArrest(OccJudge* pOccFea, API_KCF_Tracker* pKCFTracker, API_TLD* pTLDTracker)
{
	// 遮挡判断模块
	m_pOccFea = pOccFea;

	// 跟踪器模块
	m_pKCFTracker = pKCFTracker;
	m_pTLDTracker = pTLDTracker;

	m_ArrestCntKCF = 0;                                    // TLD_KCF重捕调用次数计数
	m_ArrestCntAI = 0;                                       //TLD_DaSiamRPN重捕调用次数计数
	m_bRecaptureKCF = 0;                       //TLD_KCF重捕成功标志位
	m_bRecaptureAI = 0;                           //TLD_DaSiamRPN重捕成功标志位 0-未重捕1-强重捕标志2-弱重捕标志
	//brCurrentBoxTLDPre = { 0 };        //记录上一帧TLD检测位置
	//brCurrentBoxNNPre = { 0 };
	//brCurrentBoxNNPreAI = { 0 };
	m_nContiFrameCnt = 0;                                   //DaSiamRPN重捕连续帧计数
	m_nNNContiFrameCnt = 0;                              //第二条成功重捕连续帧计数
	m_nDelayArrestCnt = 0;                              //延时重捕帧计数

	 g_GLB_nConfShortConfirmContinueFrame = 0;       // 相关性确认的连续帧数，避免出现错误重捕
	 g_GLB_nConfLongConfirmContinueFrame = 0;       // 相关性确认的连续帧数，避免出现错误重捕
	 g_GLB_nMaxConfContinueFrame = 0;                 //最大NN置信度处，相关性确认的连续帧数，避免出现错误重捕

	 m_nArrestKCFStatus = 0;
	 m_nArrestAIStatus = 0;

	  m_bEnableArrestCorr = false;
	  m_bEnableArrestAngle = false;
	  m_bEnableAreestEsay = true;
	  m_nArrestEsayCnt = 30;
}

TargetArrest::~TargetArrest()
{
}


//计算两向量夹角
float  AngleBetweenVectors(POINT32F v1, POINT32F v2)
{
	FLOAT32 cosTheta = 0.f;
	FLOAT32 dotProduct = v1.x * v2.x + v1.y * v2.y;
	FLOAT32 mag1 = (FLOAT32)sqrt(v1.x * v1.x + v1.y * v1.y);
	FLOAT32 mag2 = (FLOAT32)sqrt(v2.x * v2.x + v2.y * v2.y);
	if (mag1 * mag2 < 0.001f)
	{
		cosTheta = 0;
	}
	else
	{
		cosTheta = dotProduct / (mag1 * mag2);
	}

	return FLOAT32(acos(cosTheta) * (180.0f / PI));
}

//求两个矩形交并比
float DecideOverlap(CENTERRECT32F r1, CENTERRECT32F r2)
{
	float x1 = r1.cx;
	float y1 = r1.cy;
	float width1 = r1.w;
	float height1 = r1.h;

	float x2 = r2.cx;
	float y2 = r2.cy;
	float width2 = r2.w;
	float height2 = r2.h;

	int endx = int(MAX(x1 + width1, x2 + width2));
	int startx = int(MIN(x1, x2));
	int width = int(width1 + width2 - (endx - startx));

	int endy = int(MAX(y1 + height1, y2 + height2));
	int starty = int(MIN(y1, y2));
	int height = int(height1 + height2 - (endy - starty));

	float ratio = 0.0f;
	float Area, Area1, Area2;

	if (width <= 0 || height <= 0)
		return 0.0f;
	else
	{
		Area = float(width * height);
		Area1 = float(width1 * height1);
		Area2 = float(width2 * height2);
		ratio = Area / (Area1 + Area2 - Area);
	}

	return ratio;
}

void TargetArrest::AID_RecaptureKCF_Run(GD_VIDEO_FRAME_S img, PIPE* pLockingPipe, GLB_INPUT* p_GLB_Input)
{
	if (NULL == pLockingPipe)
	{
		return;
	}
    // 重捕区域
	OBJECTSTATUS* pObjStatusKCF = m_pKCFTracker->GetTrackeStatus();

    // 位置宽高改变
    pObjStatusKCF->ptPos.x = pLockingPipe->ptCurrentPnt.x;
    pObjStatusKCF->ptPos.y = pLockingPipe->ptCurrentPnt.y;
    pObjStatusKCF->sfSize.w = pLockingPipe->ObjectFilter.sfSize.w;
    pObjStatusKCF->sfSize.h = pLockingPipe->ObjectFilter.sfSize.h;

    //重置KCF遮挡状态
    pObjStatusKCF->nOcclude_flag = 0;
    // 初始化目标响应及相似度信息
    m_pOccFea->Occ_CleanUpObjOccInfo();

    // 清空重捕需要的缓存信息
    ObjArrest_CleanUpObjArrestInfo();

    // 更新当前目标信息//存在于清空重捕信息之后处理
    //m_pKCFTracker->KCF_UpdateTracker(img.u32Width, img.u32Height, pObjStatusKCF, g_GLB_Input);
    //重捕成功直接重置风险：挂前景干扰物
    m_pKCFTracker->KCF_Reset(pObjStatusKCF, FALSE, img, p_GLB_Input);
}

/****************************************************
* 函数名称：Recapture_Run
* 功能描述：Recapture_重捕决策模块
* 输入参数：RECT32F rfBox			 ----当前帧KCF更新跟踪后的结果
UINT32  nConTrackedCnts  ----目标连续跟踪帧数
FLOAT32 fConfidence		 ----KCF跟踪置信度
* 输出参数：
* 返 回 值：
* 调用关系：
* 其它说明：
*****************************************************/
void TargetArrest::TLD_RecaptureKCF_Run(GD_VIDEO_FRAME_S img, PIPE* pLockingPipe, GLB_INPUT* p_GLB_Input)
{
	m_bRecaptureKCF = FALSE;
	FLOAT32 res = 0.f; //TLD输出候选框与模板相关性数值
	POINT32F pt = { 0 };       // TLD计算完相关之后的偏移量
	FLOAT32 cx = 0.f;
	FLOAT32 cy = 0.f;
	RECT32S pKCFCandidateRect = { 0 };            // 基于TLD重捕中心点送给KCF相关确认的区域
	MINMAXRECT	brCurrentBoxTLD = { 0 };     //TLD当前检测位置
	MINMAXRECT	brCurrentBoxNN = { 0 };       //TLD当前最大响应检测位置
	MINMAXRECT brPredictBox = { 0 };             //TLD当前重捕范围
	MINMAXRECT mmRectKCF = { 0 };
	DOUBLE64 fIOU = 0.f;
	FLOAT32 fArrest_thresh = 0.f;                          //重捕阈值
	FLOAT32 fArrest_threshLong = 0.f;
	FLOAT32 fArrestCorr = 0.f;                                //重捕范围设置系数
	UINT16 nArrest_frmS = 1;                             // 强重捕确认帧数
	UINT16 nArrest_frmL = 2;                             // 弱重捕确认帧数
	BBOOL bIouConfirm = FALSE;                         //重捕范围条件确认
	BBOOL bIouConfirm2 = FALSE;
	BBOOL bAngleConfirm = FALSE;                         //重捕范围条件确认
	BBOOL bAngleConfirm2 = FALSE;
	BBOOL bSameObj = FALSE;
	BBOOL bSameObj2 = FALSE;


	FLOAT32 fArrestLength = 0.f;                        //重捕范围
	FLOAT32 fArrestAngle = 0.f;                         //重捕角度
	FLOAT32 fArrestAngTH = 0.f;                     //角度阈值



	KcfPara* pKcfpara = m_pKCFTracker->GetKcfPara();

	OBJECTSTATUS* pObjStatusKCF = m_pKCFTracker->GetTrackeStatus();


	// 重捕区域
	CENTERRECT32F crfCandiRect = { 0 };

	//上位机参数控制
	if (!m_bEnableArrestCorr)
	{
		 bIouConfirm = TRUE;                         
		 bIouConfirm2 = TRUE;
	}
	if (!m_bEnableArrestAngle)
	{
		 bAngleConfirm = TRUE;                       
		 bAngleConfirm2 = TRUE;
	}

	// 遮挡判断后持续重捕帧
	m_ArrestCntKCF++;

	//阈值计算
	fArrest_thresh = m_pOccFea->m_Occ_Para.fFullOccThre;
	fArrest_threshLong = fArrest_thresh * 0.95f;

	//根据长时轨迹的速度计算重捕范围
	FLOAT32   v1 = pObjStatusKCF->sfAglSpeed.vx / p_GLB_Input->stCamera.fAglReso;
	FLOAT32   v2 = pObjStatusKCF->sfAglSpeed.vy / p_GLB_Input->stCamera.fAglReso;
	FLOAT32   v3 = (pObjStatusKCF->sfSize.w + pObjStatusKCF->sfSize.h) / (2 * p_GLB_Input->unFreq);
	fArrestLength = 0.5f * FLOAT32(sqrt(v1 * v1 + v2 * v2)) + 0.5f * v3;
	fArrestLength = m_ArrestCntKCF * fArrestLength;

	if (m_ArrestCntKCF < p_GLB_Input->unFreq)      // 1s内（1x1）
	{
		fArrestAngTH = 120.0f;
	}
	else if (m_ArrestCntKCF < p_GLB_Input->unFreq * 2)//2s内（2x2）
	{
		fArrestAngTH = 20.0f;
	}
	else if (m_ArrestCntKCF < p_GLB_Input->unFreq * 4)//3s内（3x3）
	{
		fArrestAngTH = 40.0f;
	}
	else if (m_ArrestCntKCF < p_GLB_Input->unFreq * 6)
	{
		fArrestAngTH = 60.0f;
	}
	else
	{
		fArrestAngTH = 120.0f;
	}

	/* 根据遮挡时长，设置重捕参数，时间越短，重捕条件越严格 */
	//if (g_ArrestCntKCF < g_GLB_stInput.stParaLn.unFreq * 2)      // 1s内（1x1）
	//{
	//	fArrestCorr = (FLOAT32)2 / 2;
	//}
	//else if (g_ArrestCntKCF < g_GLB_stInput.stParaLn.unFreq * 4)//2s内（2x2）
	//{
	//	fArrestCorr = (FLOAT32)3 / 2;
	//}
	//else if (g_ArrestCntKCF < g_GLB_stInput.stParaLn.unFreq * 6)//3s内（3x3）
	//{
	//	fArrestCorr = (FLOAT32)5 / 2;
	//}
	//else
	//{
	//	fArrestCorr = -1;
	//}
	//关闭检测范围控制
	//fArrestCorr = -1;
	//+++++++++++++++++++++++++++++++++++++++++++++++++

	TLD_Para* pTLD_Para = m_pTLDTracker->TLD_GetPara();

	// 仅开TLD重捕
	//if (TRUE == g_GLB_stOutput.bEnableTLD)
	{

		//打印重捕调试信息
		if (pTLD_Para->nClusterNum <= 0)
		{
			m_nArrestKCFStatus = 1;
		}

		// 获取TLD第一个聚类框
		RECT32S tld_ClusterRect0 = m_pTLDTracker->TLD_GetClusterRect()[0];
		// 获取TLD 的NN最相似框
		RECT32S tld_BestNNRect = pTLD_Para->rsBestNNRect;

		// 重捕特征1：TLD有稳定的检测结果（NNconf>0.65）------------------------1st
		if (pTLD_Para->nClusterNum > 0 && m_ArrestCntKCF > 10)
		{
			//TLD当前帧检测位置
			brCurrentBoxTLD.minX = tld_ClusterRect0.x;
			brCurrentBoxTLD.maxX = tld_ClusterRect0.x + tld_ClusterRect0.w;
			brCurrentBoxTLD.minY = tld_ClusterRect0.y;
			brCurrentBoxTLD.maxY = tld_ClusterRect0.y + tld_ClusterRect0.h;
			////////////////////////////////////////////////////////////////////////////////////////////////////////////
			//重捕特征2：目标KCF响应值----------------------------------------------------2st
			// TLD送过来的候选区域与目标框计算相关性做确认
			cx = (FLOAT32)tld_ClusterRect0.x + tld_ClusterRect0.w / 2.0f;    //TLD重捕x中心点
			cy = (FLOAT32)tld_ClusterRect0.y + tld_ClusterRect0.h / 2.0f;    //TLD重捕y中心点
			/*> 送KCF确认的框位置赋值 */
			pKCFCandidateRect.w = SINT32(1.0 * pKcfpara->scale * pKcfpara->tempsize.w);
			pKCFCandidateRect.h = SINT32(1.0 * pKcfpara->scale * pKcfpara->tempsize.h);
			pKCFCandidateRect.x = SINT32(cx - pKCFCandidateRect.w / 2.0f);
			pKCFCandidateRect.y = SINT32(cy - pKCFCandidateRect.h / 2.0f);
			res = m_pKCFTracker->KCF_Detect(img, &pt, pKCFCandidateRect);
			pt.x -= pKcfpara->featureSizeX / 2.0f;
			pt.y -= pKcfpara->featureSizeY / 2.0f;

			crfCandiRect.cx = pKcfpara->scale * pKcfpara->cell_size * pt.x + tld_ClusterRect0.x + tld_ClusterRect0.w / 2.0f;
			crfCandiRect.cy = pKcfpara->scale * pKcfpara->cell_size * pt.y + tld_ClusterRect0.y + tld_ClusterRect0.h / 2.0f;
			m_fArrestKcfRes = res;

			//g_GLB_stOutput.fArrKCFRes = res; //dsp调试参数
			//连续帧管道确认稳定捕获
			// KCF响应短时重捕帧数累计
			if (res > fArrest_thresh)
			{
				//计算当前帧TLD框与上一帧TLD框是否存在交并
				fIOU = IMGO_CalcObjIou(brCurrentBoxTLD, brCurrentBoxTLDPre);
				if (fIOU > 0.35f)
				{
					g_GLB_nConfShortConfirmContinueFrame++;
				}
				else
				{
					g_GLB_nConfShortConfirmContinueFrame = 0;
				}
			}
			// KCF响应長时重捕帧数累计
			//保存上一帧TLD检测框
			brCurrentBoxTLDPre = brCurrentBoxTLD;
			//打印重捕调试信息
			if (g_GLB_nConfShortConfirmContinueFrame < nArrest_frmS)
			{
				m_nArrestKCFStatus = 2;
			}

			//重捕条件3：TLD检测位置与KCF重捕位置存在交集---------------------------------3st
			mmRectKCF.minX = SINT16(crfCandiRect.cx - pKcfpara->rfBox.w / 2.0f);
			mmRectKCF.maxX = SINT16(crfCandiRect.cx + pKcfpara->rfBox.w / 2.0f);
			mmRectKCF.minY = SINT16(crfCandiRect.cy - pKcfpara->rfBox.h / 2.0f);
			mmRectKCF.maxY = SINT16(crfCandiRect.cy + pKcfpara->rfBox.h / 2.0f);

			//计算交并比
			fIOU = IMGO_CalcObjIou(brCurrentBoxTLD, mmRectKCF);
			if (fIOU > 0.35f)
			{
				bSameObj = TRUE;
			}
			//打印重捕调试信息
			if (bSameObj == FALSE)
			{
				m_nArrestKCFStatus = 3;
			}

			///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
			//重捕特征3：短时遮挡目标与预测位置距离信息-------------------------------3st
			//计算搜索范围
			brPredictBox.minX = SINT16(MAX(pObjStatusKCF->ptPos.x - (fArrestLength + tld_ClusterRect0.w) / 2.0f, 0));
			brPredictBox.maxX = SINT16(MIN(pObjStatusKCF->ptPos.x + (fArrestLength + tld_ClusterRect0.w) / 2.0f, img.u32Width));
			brPredictBox.minY = SINT16(MAX(pObjStatusKCF->ptPos.y - (fArrestLength + tld_ClusterRect0.h) / 2.0f, 0.0));
			brPredictBox.maxY = SINT16(MIN(pObjStatusKCF->ptPos.y + (fArrestLength + tld_ClusterRect0.h) / 2.0f, img.u32Height));
			//计算交并比
			fIOU = IMGO_CalcObjIou(brPredictBox, brCurrentBoxTLD);
			if (fIOU > 0.001)
			{
				bIouConfirm = TRUE;
			}

			//打印重捕调试信息
			if (bIouConfirm == FALSE)
			{
				m_nArrestKCFStatus = 4;
			}

			///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
			//重捕特征4：目标重捕角度范围限制-------------------------------4st
			Pole pPreAgl = { 0};
			//pPreAgl.beta = pObjDirectList[pObjDirectInfo->nStart].fAz;
			//pPreAgl.alpha = pObjDirectList[pObjDirectInfo->nStart].fPt;

			// 取出跟踪管道的短时轨迹
			OBJ_ANGLE_R* pTrackAglListNear = &pLockingPipe->stMotionMod_mean.ObjAglListsNear;

			// 获取前10帧的位置
			SINT32 index = -1;
			// 若未填满队列
			if (pTrackAglListNear->nCnt < pTrackAglListNear->nListSize)
			{
				index = 0;
			}
			else
			{
				index = (pTrackAglListNear->nEnd + GLB_OBJTRACK_LEN - 10) % GLB_OBJTRACK_LEN;
			}
			pPreAgl.beta = pTrackAglListNear->parHistoryList[index].afAngle.fAz;
			pPreAgl.alpha = pTrackAglListNear->parHistoryList[index].afAngle.fPt;

			POINT32F pCurrent = { cx,cy };
			Pole pCurrentAgl = getStablePoleFromImagePos(pCurrent, p_GLB_Input->stCamera, p_GLB_Input->servoInfo, p_GLB_Input->afPlatformRPY, p_GLB_Input->setupErr);

			POINT32F pMag1 = { pObjStatusKCF->sfAglSpeed.vx * 1000, pObjStatusKCF->sfAglSpeed.vy * 1000 };
			POINT32F pMag2 = { FLOAT32(pCurrentAgl.beta - pPreAgl.beta) * 1000, FLOAT32(pCurrentAgl.alpha - pPreAgl.alpha) * 1000 };
			fArrestAngle = AngleBetweenVectors(pMag1, pMag2);

			if (fArrestAngle < fArrestAngTH)
			{
				bAngleConfirm = TRUE;
			}

			////打印重捕调试信息
			if (bAngleConfirm == FALSE)
			{
				m_nArrestKCFStatus = 5;
			}

			//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
			////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
			//重捕触发条件：+++++++++++++++++++++++++++++++++++++++++++++
			if (g_GLB_nConfShortConfirmContinueFrame >= nArrest_frmS && bIouConfirm && bAngleConfirm && bSameObj)
			{
				// 获取TLD预测位置，初始化目标跟踪器
				//pt.x -= g_kcfpara.featureSizeX / 2;
				//pt.y -= g_kcfpara.featureSizeY / 2;
				//g_GLB_stPara.crfCandiRect.cx = g_kcfpara.scale * g_kcfpara.cell_size * pt.x + g_TLD_ClusterRect[0].x + g_TLD_ClusterRect[0].w / 2;
				//g_GLB_stPara.crfCandiRect.cy = g_kcfpara.scale * g_kcfpara.cell_size * pt.y + g_TLD_ClusterRect[0].y + g_TLD_ClusterRect[0].h / 2;
				m_bRecaptureKCF = 1;
				m_nArrestKCFStatus = 6;
			}
		}
		//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
	   //NN最大响应重捕++++++++++++++++++++++++++++++++++++++++++++++++++
	  //重捕特征1：TLD有稳定的最大响应检测结果（NNconf>0.4）------------------------1st
		else if (pTLD_Para->fNNConfMax > 0.5 && pTLD_Para->fNNPosConfMax > 0.8 && m_ArrestCntKCF > m_nArrestEsayCnt
			&& m_bEnableAreestEsay)
		{
			//NN最大响应当前帧位置
			brCurrentBoxNN.minX = tld_BestNNRect.x;
			brCurrentBoxNN.maxX = tld_BestNNRect.x + tld_BestNNRect.w;
			brCurrentBoxNN.minY = tld_BestNNRect.y;
			brCurrentBoxNN.maxY = tld_BestNNRect.y + tld_BestNNRect.h;

			//重捕特征2：目标KCF响应值----------------------------------------------------2st
			// TLD低响应送过来的候选区域与目标框计算相关性做确认
			cx = tld_BestNNRect.x + tld_BestNNRect.w / 2.0f;    //中心点x坐标
			cy = tld_BestNNRect.y + tld_BestNNRect.h / 2.0f;     //中心点y坐标
			/*> 送KCF确认的框位置赋值 */
			/*> 送KCF确认的框位置赋值 */
			pKCFCandidateRect.w = SINT32(1.0 * pKcfpara->scale * pKcfpara->tempsize.w);
			pKCFCandidateRect.h = SINT32(1.0 * pKcfpara->scale * pKcfpara->tempsize.h);
			pKCFCandidateRect.x = SINT32(cx - pKCFCandidateRect.w / 2);
			pKCFCandidateRect.y = SINT32(cy - pKCFCandidateRect.h / 2);
			res = m_pKCFTracker->KCF_Detect(img, &pt, pKCFCandidateRect);
			pt.x -= pKcfpara->featureSizeX / 2.0f;
			pt.y -= pKcfpara->featureSizeY / 2.0f;

			crfCandiRect.cx = pKcfpara->scale * pKcfpara->cell_size * pt.x + tld_BestNNRect.x + tld_BestNNRect.w / 2;
			crfCandiRect.cy = pKcfpara->scale * pKcfpara->cell_size * pt.y + tld_BestNNRect.y + tld_BestNNRect.h / 2;
			m_fArrestKcfRes = res;
 
			//连续帧管道确认稳定捕获
			//计算当前帧NN框与上一帧NN框是否存在交并
			//kcf高响应计数,提升响应
			if (res > fArrest_threshLong)
			{
				fIOU = IMGO_CalcObjIou(brCurrentBoxNN, brCurrentBoxNNPre);
				if (fIOU > 0.35)
				{
					g_GLB_nMaxConfContinueFrame++;
				}
				else
				{
					g_GLB_nMaxConfContinueFrame = 0;
				}
			}
			//保留上一帧NN
			brCurrentBoxNNPre = brCurrentBoxNN;
			////打印重捕调试信息
			if (g_GLB_nMaxConfContinueFrame < nArrest_frmL)
			{
				m_nArrestKCFStatus = 7;
			}

			//重捕条件3：TLD检测位置与KCF重捕位置存在交集---------------------------------3st
			mmRectKCF.minX = SINT16(crfCandiRect.cx - pKcfpara->rfBox.w / 2);
			mmRectKCF.maxX = SINT16(crfCandiRect.cx + pKcfpara->rfBox.w / 2);
			mmRectKCF.minY = SINT16(crfCandiRect.cy - pKcfpara->rfBox.h / 2);
			mmRectKCF.maxY = SINT16(crfCandiRect.cy + pKcfpara->rfBox.h / 2);

			//计算交并比
			fIOU = IMGO_CalcObjIou(brCurrentBoxNN, mmRectKCF);
			if (fIOU > 0.35)
			{
				bSameObj2 = TRUE;
			}
			//打印重捕调试信息
			if (bSameObj2 == FALSE)
			{
				m_nArrestKCFStatus = 8;
			}

			////////////////////////////////////////////////////////////////////////////////////////////////////////////
			//重捕特征2：目标与预测位置交并比信息-------------------------------3st
			//计算搜索范围
			brPredictBox.minX = SINT16(MAX(pObjStatusKCF->ptPos.x - (fArrestLength + tld_BestNNRect.w) / 2, 0));
			brPredictBox.maxX = SINT16(MIN(pObjStatusKCF->ptPos.x + (fArrestLength + tld_BestNNRect.w) / 2, img.u32Width));
			brPredictBox.minY = SINT16(MAX(pObjStatusKCF->ptPos.y - (fArrestLength + tld_BestNNRect.h) / 2, 0));
			brPredictBox.maxY = SINT16(MIN(pObjStatusKCF->ptPos.y + (fArrestLength + tld_BestNNRect.h) / 2, img.u32Height));
			//计算交并比
			fIOU = IMGO_CalcObjIou(brPredictBox, brCurrentBoxNN);
			if (fIOU > 0.001)
			{
				bIouConfirm2 = TRUE;
			}

			//打印重捕调试信息
			if (bIouConfirm2 == FALSE)
			{
				m_nArrestKCFStatus = 9;
			}

			///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
			//重捕特征4：目标重捕角度范围限制-------------------------------4st
			//POINT32F pCurrent;
			//POINT32F pCurrentPre;
			//pCurrent.x = SERVO_CalcObjAzimuth(cx, g_GLB_stInput.snImgSize.w, g_SERVO_stInput.fAzimuth, g_SERVO_stInput.fResolAz);
			//pCurrent.y = SERVO_CalcObjPitching(cy, g_GLB_stInput.snImgSize.h, g_SERVO_stInput.fPitching, g_SERVO_stInput.fResolPt);
			//pCurrentPre.x = pObjDirectList[pObjDirectInfo->nStart].fAz;
			//pCurrentPre.y = pObjDirectList[pObjDirectInfo->nStart].fPt;
			////pCurrent.x = SERVO_CalcObjCoordinateX_1(fAz, nWidth, g_SERVO_stInput.fAzimuth, g_SERVO_stInput.fResolAz, g_SERVO_stInput.fPitching);
			////pCurrent.y = SERVO_CalcObjCoordinateY(fPt, nHeight, g_SERVO_stInput.fPitching, g_SERVO_stInput.fResolPt);
			//POINT32F pMag1 = { pObjStatusKCF->sfAglSpeed.vx * 1000, pObjStatusKCF->sfAglSpeed.vy * 1000 };
			//POINT32F pMag2 = { (pCurrent.x - pCurrentPre.x) * 1000, (pCurrent.y - pCurrentPre.y) * 1000 };
			//fArrestAngle = AngleBetweenVectors(pMag1, pMag2);

			Pole pPreAgl = { 0 };
			// 取出跟踪管道的短时轨迹
			OBJ_ANGLE_R* pTrackAglListNear = &pLockingPipe->stMotionMod_mean.ObjAglListsNear;

			// 获取前10帧的位置
			SINT32 index = -1;
			// 若未填满队列
			if (pTrackAglListNear->nCnt < pTrackAglListNear->nListSize)
			{
				index = 0;
			}
			else
			{
				index = (pTrackAglListNear->nEnd + GLB_OBJTRACK_LEN - 10) % GLB_OBJTRACK_LEN;
			}
			pPreAgl.beta = pTrackAglListNear->parHistoryList[index].afAngle.fAz;
			pPreAgl.alpha = pTrackAglListNear->parHistoryList[index].afAngle.fPt;

			POINT32F pCurrent = { cx,cy };
			Pole pCurrentAgl = getStablePoleFromImagePos(pCurrent, p_GLB_Input->stCamera, p_GLB_Input->servoInfo, p_GLB_Input->afPlatformRPY, p_GLB_Input->setupErr);

			POINT32F pMag1 = { pObjStatusKCF->sfAglSpeed.vx * 1000, pObjStatusKCF->sfAglSpeed.vy * 1000 };
			POINT32F pMag2 = { FLOAT32(pCurrentAgl.beta - pPreAgl.beta) * 1000, FLOAT32(pCurrentAgl.alpha - pPreAgl.alpha) * 1000 };
			fArrestAngle = AngleBetweenVectors(pMag1, pMag2);


			if (fArrestAngle < fArrestAngTH)
			{
				bAngleConfirm2 = TRUE;
			}

			//打印重捕调试信息
			if (bAngleConfirm2 == FALSE)
			{
				m_nArrestKCFStatus = 10;
			}

			/////////////////////////////////////////////////////////////////////////////////////////////////////////////
			//重捕触发条件++++++++++++++++++++++++++++++++++++++++++
			if (g_GLB_nMaxConfContinueFrame >= nArrest_frmL && bIouConfirm2 && bAngleConfirm2 && bSameObj2)
			{
				// 获取TLD预测位置，初始化目标跟踪器
				//pt.x -= g_kcfpara.featureSizeX / 2;
				//pt.y -= g_kcfpara.featureSizeY / 2;
				//g_GLB_stPara.crfCandiRect.cx = g_kcfpara.scale * g_kcfpara.cell_size * pt.x + g_TLD_rsBestNNRect.x + g_TLD_rsBestNNRect.w/2;
			   // g_GLB_stPara.crfCandiRect.cy = g_kcfpara.scale * g_kcfpara.cell_size * pt.y + g_TLD_rsBestNNRect.y + g_TLD_rsBestNNRect.h/2;
				m_bRecaptureKCF = 2;
				m_nArrestKCFStatus = 11;
			}
		}
	}


	// 如果重捕成功，调用初始化
	if (m_bRecaptureKCF > 0)
	{
		// 位置改变，宽高不变
		pObjStatusKCF->ptPos.x = crfCandiRect.cx;
		pObjStatusKCF->ptPos.y = crfCandiRect.cy;

		//重置KCF遮挡状态
		pObjStatusKCF->nOcclude_flag = 0;
		// 初始化目标响应及相似度信息
		m_pOccFea->Occ_CleanUpObjOccInfo();

		// 清空重捕需要的缓存信息
		ObjArrest_CleanUpObjArrestInfo();

		// 更新当前目标信息//存在于清空重捕信息之后处理
		//m_pKCFTracker->KCF_UpdateTracker(img.u32Width, img.u32Height, pObjStatusKCF, g_GLB_Input);
		//重捕成功直接重置风险：挂前景干扰物
		m_pKCFTracker->KCF_Reset(pObjStatusKCF, FALSE, img, p_GLB_Input);

		//清除TLD学习连续帧计数
		pTLD_Para->nLearnedNum = 0;
	}

	return;
}








/**********************************************************
* 函数名称：ObjArrest_CleanUpObjArrestInfo()
* 功能描述：初始化重捕信息
* 输入参数：无
* 输出参数：无
* 返 回 值：无
* 调用关系：无
* 其它说明：无
**********************************************************/
void TargetArrest::ObjArrest_CleanUpObjArrestInfo(void)
{
	m_ArrestCntKCF = 0;
	g_GLB_nConfShortConfirmContinueFrame = 0;
	g_GLB_nConfLongConfirmContinueFrame = 0;
}


//初始化解锁清理
void TargetArrest::ObjArrest_CleanUpAllKCFArrestInfo(void)
{
	ObjArrest_CleanUpObjArrestInfo();
	m_bRecaptureKCF = 0;
}



void TargetArrest::ObjArrest_setArrestCorrPara(BBOOL bEnableArrestCorr)
{
	 m_bEnableArrestCorr = bEnableArrestCorr;
}

void TargetArrest::ObjArrest_setArrestAnglePara(BBOOL bEnableArrestAngle)
{
	m_bEnableArrestAngle = bEnableArrestAngle;
}

void TargetArrest::ObjArrest_setArrestEsayPara(BBOOL bEnableArrestEsay)
{
	m_bEnableAreestEsay = bEnableArrestEsay;
}

void TargetArrest::ObjArrest_setArrestEsayCntPara(SINT32 nArrestEsayCnt)
{
	m_nArrestEsayCnt = nArrestEsayCnt;
}