/*********版权所有（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; }