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