|
|
/*********版权所有（C）2018，武汉高德红外股份有限公司******************
|
|
|
* 文件名称： Arith_Predict.h
|
|
|
* 文件标识： 对跟踪的目标进行预测分析
|
|
|
* 内容摘要： 包含长短时预测、航迹预测等
|
|
|
* 其它说明： "Arith_Precit.h"的函数、全局变量、宏定义，统一前缀为简写"NL_"
|
|
|
* 当前版本： 固化V1.0
|
|
|
* 创建作者： zy
|
|
|
* 创建日期： 2018.01.29
|
|
|
*****************************************************************************/
|
|
|
#include "Arith_AglPredict.h"
|
|
|
#include "Arith_Common.hpp"
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/**********************************************************
|
|
|
* 函数名称： TO_ObjAglTrackPredict()
|
|
|
* 功能描述：目标当前帧轨迹预测：利用长短时目标方位、俯仰航迹
|
|
|
* 输入参数： SINT32 nWidth -- 图像宽度
|
|
|
* SINT32 nHeight -- 图像高度
|
|
|
* 输出参数：无
|
|
|
* 返 回 值：无
|
|
|
* 调用关系：无
|
|
|
* 其它说明：预测目标在当前帧的中心点, 以及搜索区间 20180129
|
|
|
**********************************************************/
|
|
|
void Predict_ObjAglTrackPredict(FilterMeanNL* pFilter, SINT32 nWidth, SINT32 nHeight, GLB_INPUT* p_GLB_Input)
|
|
|
{
|
|
|
OBJ_ANGLE_R *pObjAglListsNear = &pFilter->ObjAglListsNear;
|
|
|
OBJ_ANGLE_R *pObjAglListsLong = &pFilter->ObjAglListsLong;
|
|
|
ANGLE_R *parHistoryListNear = (ANGLE_R*)pObjAglListsNear->parHistoryList;
|
|
|
ANGLE_R *parHistoryListLong = (ANGLE_R*)pObjAglListsLong->parHistoryList;
|
|
|
SINT32 nEndNear, nEndLong;
|
|
|
UINT32 unFrmIdPreNear, unFrmIdPreLong;
|
|
|
UINT32 unFrmId, unFrmStepNear, unFrmStepLong;
|
|
|
ANGLE32F fAglPreNear, fAglPreLong;
|
|
|
ANGLE32F fAglSpeedNear, fAglSpeedLong;
|
|
|
ANGLE32F fAglPredictNear, fAglPredictLong;
|
|
|
FLOAT32 fRPreNear, fRSpeedNear, fRPredictNear;
|
|
|
FLOAT32 fRPreLong, fRSpeedLong, fRPredictLong;
|
|
|
CENTERRECT crnSrRectNear, crnSrRectLong;
|
|
|
SPEED32F fSpeedNear, fSpeedLong;
|
|
|
SIZE32S snSrRadiusNear, snSrRadiusLong;
|
|
|
|
|
|
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++
|
|
|
//初始化
|
|
|
unFrmId = p_GLB_Input->unFrmId;
|
|
|
pObjAglListsNear->arfPredict.unFrmID = unFrmId;
|
|
|
pObjAglListsLong->arfPredict.unFrmID = unFrmId;
|
|
|
|
|
|
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++
|
|
|
//目标短时间轨迹预测
|
|
|
//计算当前帧与上一次轨迹记录的间隔帧数
|
|
|
nEndNear = pObjAglListsNear->nEnd;
|
|
|
unFrmIdPreNear = parHistoryListNear[nEndNear].unFrmID;
|
|
|
if (unFrmId > unFrmIdPreNear)
|
|
|
{
|
|
|
unFrmStepNear = unFrmId - unFrmIdPreNear;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
unFrmStepNear = unFrmIdPreNear - unFrmId;
|
|
|
}
|
|
|
|
|
|
//获取上一次轨迹记录的目标角度、角速度、距离、距离变化速度
|
|
|
fAglPreNear = parHistoryListNear[nEndNear].afAngle; //目标角度
|
|
|
fAglSpeedNear = pObjAglListsNear->arfSpeed.afAngle; //目标角速度 20171023zy:角速度是滤波后的结果
|
|
|
fRPreNear = parHistoryListNear[nEndNear].fDist; //距离
|
|
|
fRSpeedNear = pObjAglListsNear->arfSpeed.fDist; //距离变化速度
|
|
|
|
|
|
//估算当前帧预测的目标角度、距离
|
|
|
fAglPredictNear.fAz = DEGLIM360(fAglPreNear.fAz + fAglSpeedNear.fAz * unFrmStepNear);
|
|
|
fAglPredictNear.fPt = DEGLIM(fAglPreNear.fPt + fAglSpeedNear.fPt * unFrmStepNear);
|
|
|
fRPredictNear = fRPreNear + fRSpeedNear * unFrmStepNear;
|
|
|
pObjAglListsNear->arfPredict.afAngle = fAglPredictNear; //20171023zy:根据预测的角度来计算预测的位置点
|
|
|
pObjAglListsNear->arfPredict.fDist = fRPredictNear;
|
|
|
|
|
|
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++
|
|
|
//估算当前帧搜索区域矩形
|
|
|
//搜索区域中心
|
|
|
|
|
|
// 稳定系到图像坐标系
|
|
|
Pole targetCarNUEPole;
|
|
|
targetCarNUEPole.alpha = fAglPredictNear.fPt;
|
|
|
targetCarNUEPole.beta = fAglPredictNear.fAz;
|
|
|
targetCarNUEPole.distance = 0;
|
|
|
|
|
|
|
|
|
POINT32F imgPos = getImagePosFromStablePole(targetCarNUEPole, p_GLB_Input->stCamera, p_GLB_Input->servoInfo, p_GLB_Input->afPlatformRPY, p_GLB_Input->setupErr
|
|
|
);
|
|
|
crnSrRectNear.cx = SINT16(imgPos.x);
|
|
|
crnSrRectNear.cy = SINT16(imgPos.y);
|
|
|
|
|
|
//搜索区域半径
|
|
|
fSpeedNear.vx = fAglSpeedNear.fAz / p_GLB_Input->stCamera.fAglReso; //由角速度转换为线速度
|
|
|
fSpeedNear.vy = fAglSpeedNear.fPt / p_GLB_Input->stCamera.fAglReso; //由角速度转换为线速度
|
|
|
|
|
|
snSrRadiusNear.w = MAX(ABS((SINT32)(fSpeedNear.vx * 2)), 30 ); //20180129zy:
|
|
|
snSrRadiusNear.h = MAX(ABS((SINT32)(fSpeedNear.vy * 2)), 30 ); //20180129zy: g_DST_stPara.nPipeRadiusTrack
|
|
|
crnSrRectNear.w = 2 * snSrRadiusNear.w + 1;
|
|
|
crnSrRectNear.h = 2 * snSrRadiusNear.h + 1;
|
|
|
crnSrRectNear.s = crnSrRectNear.w * crnSrRectNear.h;
|
|
|
pFilter->crnObjPrediRtNear = crnSrRectNear; //======20171023zy：短时轨迹的预测位置======
|
|
|
IMGO_CenRect16S2MMRect32S(crnSrRectNear, &pFilter->mrnObjPrediRtNear,nWidth, nHeight);
|
|
|
|
|
|
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++
|
|
|
//记录目标长时间轨迹（每秒记录一次）
|
|
|
//在长时间轨迹更新第2个位置之前，一直跟短时间轨迹预测位置保持一致
|
|
|
if (pObjAglListsLong->nCnt <= 2)
|
|
|
{
|
|
|
fAglSpeedLong = fAglSpeedNear;
|
|
|
fAglPredictLong = pObjAglListsNear->arfPredict.afAngle;
|
|
|
pObjAglListsLong->arfPredict.afAngle = fAglPredictLong;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
//计算当前帧与上一次轨迹记录的间隔帧数
|
|
|
nEndLong = pObjAglListsLong->nEnd;
|
|
|
unFrmIdPreLong = parHistoryListLong[nEndLong].unFrmID;
|
|
|
if (unFrmId > unFrmIdPreLong)
|
|
|
{
|
|
|
unFrmStepLong = unFrmId - unFrmIdPreLong;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
unFrmStepLong = unFrmIdPreLong - unFrmId;
|
|
|
}
|
|
|
|
|
|
//获取上一次轨迹记录的目标角度、角速度、距离、距离变化速度
|
|
|
fAglPreLong = parHistoryListLong[nEndLong].afAngle;
|
|
|
fAglSpeedLong = pObjAglListsLong->arfSpeed.afAngle;
|
|
|
fRPreLong = parHistoryListLong[nEndLong].fDist;
|
|
|
fRSpeedLong = pObjAglListsLong->arfSpeed.fDist;
|
|
|
|
|
|
//估算当前帧预测的目标角度、距离
|
|
|
fAglPredictLong.fAz = DEGLIM360(fAglPreLong.fAz + fAglSpeedLong.fAz * unFrmStepLong);
|
|
|
fAglPredictLong.fPt = DEGLIM(fAglPreLong.fPt + fAglSpeedLong.fPt * unFrmStepLong);
|
|
|
fRPredictLong = fRPreLong + fRSpeedLong * unFrmStepLong;
|
|
|
pObjAglListsLong->arfPredict.afAngle = fAglPredictLong;
|
|
|
|
|
|
pObjAglListsLong->arfPredict.fDist = fRPredictLong;
|
|
|
}
|
|
|
|
|
|
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++
|
|
|
//估算当前帧搜索区域矩形
|
|
|
//搜索区域中心
|
|
|
|
|
|
// 稳定系到图像坐标系
|
|
|
targetCarNUEPole.alpha = fAglPredictLong.fPt;
|
|
|
targetCarNUEPole.beta = fAglPredictLong.fAz;
|
|
|
targetCarNUEPole.distance = 0;
|
|
|
|
|
|
////getEOServoPoleFromCarNUEPole(targetCarNUEPole, &targetEOCamPole,
|
|
|
//// g_GLB_stInput.afPlatformRPY.fYaw, g_GLB_stInput.afPlatformRPY.fPitch, g_GLB_stInput.afPlatformRPY.fRoll,
|
|
|
//// g_SERVO_stInput.fPitching, g_SERVO_stInput.fAzimuth);
|
|
|
|
|
|
////crnSrRectLong.cx = SERVO_CalcObjCoordinateX((FLOAT32)targetEOCamPole.beta, g_GLB_stInput.nImageWidth,
|
|
|
//// 0, g_SERVO_stInput.fResolAz);
|
|
|
|
|
|
////crnSrRectLong.cy = SERVO_CalcObjCoordinateY((FLOAT32)targetEOCamPole.alpha, g_GLB_stInput.nImageHeight,
|
|
|
//// 0, g_SERVO_stInput.fResolPt);
|
|
|
|
|
|
imgPos = getImagePosFromStablePole(targetCarNUEPole, p_GLB_Input->stCamera, p_GLB_Input->servoInfo, p_GLB_Input->afPlatformRPY, p_GLB_Input->setupErr);
|
|
|
crnSrRectLong.cx = SINT16(imgPos.x);
|
|
|
crnSrRectLong.cy = SINT16(imgPos.y);
|
|
|
|
|
|
|
|
|
|
|
|
//搜索区域半径
|
|
|
fSpeedLong.vx = fAglSpeedLong.fAz / p_GLB_Input->stCamera.fAglReso;
|
|
|
fSpeedLong.vy = fAglSpeedLong.fPt / p_GLB_Input->stCamera.fAglReso;
|
|
|
snSrRadiusLong.w = MAX(ABS((SINT32)(fSpeedLong.vx * 2)), 30);
|
|
|
snSrRadiusLong.h = MAX(ABS((SINT32)(fSpeedLong.vy * 2)), 30);
|
|
|
crnSrRectLong.w = 2 * snSrRadiusLong.w + 1;
|
|
|
crnSrRectLong.h = 2 * snSrRadiusLong.h + 1;
|
|
|
crnSrRectLong.s = crnSrRectLong.w * crnSrRectLong.h;
|
|
|
pFilter->crnObjPrediRtLong = crnSrRectLong; //========20171023zy:长时轨迹的预测位置======
|
|
|
IMGO_CenRect16S2MMRect32S(crnSrRectLong, &pFilter->mrnObjPrediRtLong,nWidth, nHeight);
|
|
|
|
|
|
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++
|
|
|
//判断长短轨迹预测位置是否一致
|
|
|
DIST32S dnNLDist, dnNLDistPre;
|
|
|
dnNLDistPre = pFilter->dnObjPredictDist;
|
|
|
dnNLDist.dx = ABS(crnSrRectNear.cx - crnSrRectLong.cx);
|
|
|
dnNLDist.dy = ABS(crnSrRectNear.cy - crnSrRectLong.cy);
|
|
|
SINT32 fNLDist2 = dnNLDist.dx * dnNLDist.dx + dnNLDist.dy * dnNLDist.dy;
|
|
|
SINT32 fNLDist2Pre = dnNLDistPre.dx * dnNLDistPre.dx + dnNLDistPre.dy * dnNLDistPre.dy;
|
|
|
//SINT32 nNLDist2Thres = g_DST_stPara.nPipeRadiusTrack * g_DST_stPara.nPipeRadiusTrack;
|
|
|
// SINT32 nNLDist2Thres = 20*20;
|
|
|
//SINT32 nNLDist2Thres = (g_GLB_stOutput.ObjectStatus.sfSize.h * g_GLB_stOutput.ObjectStatus.sfSize.w) * 25;
|
|
|
SINT32 nNLDist2Thres = 10 * 25;
|
|
|
nNLDist2Thres = MAX(nNLDist2Thres,100);
|
|
|
|
|
|
if (fNLDist2 - fNLDist2Pre > 2
|
|
|
|| fNLDist2 > nNLDist2Thres)
|
|
|
{
|
|
|
pFilter->nObjPredictFarCnt++;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
pFilter->nObjPredictFarCnt = 0;
|
|
|
pFilter->bObjPredictAbnormal = FALSE;
|
|
|
}
|
|
|
if (pFilter->nObjPredictFarCnt > 10
|
|
|
&& fNLDist2 > nNLDist2Thres)
|
|
|
{
|
|
|
pFilter->bObjPredictAbnormal = TRUE;
|
|
|
}
|
|
|
pFilter->dnObjPredictDist = dnNLDist;
|
|
|
|
|
|
////利用长短时轨迹判断是否开启惯性预测,采用多帧判断 by wcw04046 @ 2022/01/17
|
|
|
//if (g_GLB_stOutput.nObjPredictFarCnt == 0)
|
|
|
//{
|
|
|
// g_GLB_stPara.nServoTrackSta++;
|
|
|
//}
|
|
|
//else
|
|
|
//{
|
|
|
// g_GLB_stPara.nServoTrackSta = 0;//一旦出现预测错误，直接关闭。
|
|
|
//}
|
|
|
//if(g_GLB_stPara.nServoTrackSta >= 50)//连续50帧稳定预测，表明目标基本匀速运动，可以开启角度预测
|
|
|
//{
|
|
|
// g_GLB_stPara.bEnableServoTrackPredi = true;
|
|
|
//}
|
|
|
//else
|
|
|
//{
|
|
|
// g_GLB_stPara.bEnableServoTrackPredi = false;
|
|
|
//}
|
|
|
}
|
|
|
|
|
|
|
|
|
/**********************************************************
|
|
|
* 函数名称：TO_ObjAglTrackFilter()
|
|
|
* 功能描述：目标轨迹滤波及速度估算
|
|
|
* 输入参数：OBJ_ANGLE_R *parObjTrackList -- 目标短时间轨迹数组
|
|
|
* SINT32 nFilterCntThres -- 管道的滤波帧数
|
|
|
* OBJECTSTATUS *pObjStatus -- 跟踪目标信息结构体
|
|
|
* 输出参数：OBJ_ANGLE_R *parObjTrackList -- 目标短时间轨迹数组
|
|
|
* 返 回 值：无
|
|
|
* 调用关系：无
|
|
|
* 其它说明：短时轨迹用于目标实时位置滤波，长时轨迹用于目标航迹判断。【待添加】
|
|
|
**********************************************************/
|
|
|
void Predict_ObjAglTrackFilter(FilterMeanNL* pFilter,OBJ_ANGLE_R *parObjTrackList, SINT32 nFilterCntThres, TARGET_OBJECT* pTarget,GLB_INPUT* p_GLB_Input)
|
|
|
{
|
|
|
ANGLE_R *parHistoryList = (ANGLE_R*)parObjTrackList->parHistoryList;
|
|
|
SINT32 i, m;
|
|
|
SINT32 nFilterCnt;//滤波帧数
|
|
|
SINT32 nSpeedStep;//速度间隔帧数
|
|
|
SINT32 nListSize = parObjTrackList->nListSize; //目标角度数组长度
|
|
|
FLOAT32 fAz, fPt;
|
|
|
FLOAT32 fAzSum, fPtSum, fRSum;
|
|
|
FLOAT32 fAzSpeed, fPtSpeed, fRSpeed;
|
|
|
FLOAT32 fAzPre, fPtPre, fRPre;
|
|
|
FLOAT32 fAzCurr, fPtCurr, fRCurr;
|
|
|
SINT32 nEnd, nPre;
|
|
|
|
|
|
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++
|
|
|
//初始化
|
|
|
m = parObjTrackList->nEnd;
|
|
|
fAzSum = 0.0f;
|
|
|
fPtSum = 0.0f;
|
|
|
fRSum = 0.0f;
|
|
|
fAzSpeed= 0.0f;
|
|
|
fPtSpeed= 0.0f;
|
|
|
fRSpeed = 0.0f;
|
|
|
parObjTrackList->arfFilter.unFrmID = p_GLB_Input->unFrmId;
|
|
|
parObjTrackList->arfSpeed.unFrmID = p_GLB_Input->unFrmId;
|
|
|
|
|
|
//防错处理
|
|
|
if (nFilterCntThres <= 1)
|
|
|
{
|
|
|
parObjTrackList->nFilterCnt = 1;
|
|
|
parObjTrackList->arfFilter.afAngle.fAz = parHistoryList[m].afAngle.fAz;
|
|
|
parObjTrackList->arfFilter.afAngle.fPt = parHistoryList[m].afAngle.fPt;
|
|
|
parObjTrackList->arfFilter.fDist = parHistoryList[m].fDist;
|
|
|
//parObjTrackList->arfSpeed.afAngle.fAz = pTarget->sfAglSpeed.vx;
|
|
|
//parObjTrackList->arfSpeed.afAngle.fPt = pTarget->sfAglSpeed.vy; //20180626zy：目标角速度长短时
|
|
|
parObjTrackList->arfSpeed.fDist = 1.0f;
|
|
|
}
|
|
|
|
|
|
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++
|
|
|
//方位角滤波
|
|
|
//判断角度是否跨越0/360度边界
|
|
|
FLOAT32 fObjSpeedX = parObjTrackList->arfSpeed.afAngle.fAz / p_GLB_Input->stCamera.fAglReso;
|
|
|
if (parObjTrackList->nCnt > 1)
|
|
|
{
|
|
|
//MSSu, 20150617: 测试发现若某帧目标速度>=20，nFilterCnt=1后，
|
|
|
// 目标角速度一直未更新，即使目标速度已下降。
|
|
|
// 因此，此处将目标短时预测角速度与相邻帧角速度取均值。
|
|
|
//MSSu, 20150808: 计算速度时，增加帧间隔，因长短轨迹记录的帧间隔不一致。
|
|
|
nEnd = parObjTrackList->nEnd;
|
|
|
nPre = (parObjTrackList->nEnd - 1 + nListSize) % nListSize;
|
|
|
fAzCurr = parHistoryList[nEnd].afAngle.fAz;
|
|
|
fAzPre = parHistoryList[nPre].afAngle.fAz;
|
|
|
FLOAT32 fObjSpeedXNear = DEGLIM(fAzCurr - fAzPre);
|
|
|
fObjSpeedXNear /= parObjTrackList->nStep;
|
|
|
fObjSpeedXNear /= p_GLB_Input->stCamera.fAglReso;
|
|
|
fObjSpeedX = (fObjSpeedX + fObjSpeedXNear) / 2;
|
|
|
}
|
|
|
fObjSpeedX = MAX(ABS(fObjSpeedX), 1.0f);
|
|
|
nFilterCnt = MAX((SINT32)(nFilterCntThres / fObjSpeedX * 1.5f), 1);
|
|
|
nFilterCnt = MIN(nFilterCnt, parObjTrackList->nCnt);
|
|
|
m = parObjTrackList->nEnd;
|
|
|
fAzSum = parHistoryList[m].afAngle.fAz;
|
|
|
FLOAT32 fAzMin = 360.0f;
|
|
|
FLOAT32 fAzMax = 0.0f;
|
|
|
BBOOL bAzCorrect = FALSE;
|
|
|
for (i = 0; i < nFilterCnt; i++)
|
|
|
{
|
|
|
fAzMin = MIN(fAzMin, parHistoryList[m].afAngle.fAz);
|
|
|
fAzMax = MAX(fAzMax, parHistoryList[m].afAngle.fAz);
|
|
|
m = (m + nListSize - 1) % nListSize;
|
|
|
}
|
|
|
if (fabs(fAzMin - fAzMax) > 180.0f)
|
|
|
{
|
|
|
bAzCorrect = TRUE;
|
|
|
}
|
|
|
m = parObjTrackList->nEnd;
|
|
|
for (i = 1; i < nFilterCnt; i++)
|
|
|
{
|
|
|
m = (m + nListSize - 1) % nListSize;
|
|
|
fAz = parHistoryList[m].afAngle.fAz;
|
|
|
if (bAzCorrect && fAz < 180.0f)
|
|
|
{
|
|
|
fAzSum += fAz + 360.f;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
fAzSum += fAz;
|
|
|
}
|
|
|
}
|
|
|
parObjTrackList->arfFilter.afAngle.fAz = DEGLIM360(fAzSum / nFilterCnt);
|
|
|
|
|
|
//方位角速度估算
|
|
|
nFilterCnt = MIN(MAX(2, nFilterCnt), parObjTrackList->nCnt);
|
|
|
nEnd = parObjTrackList->nEnd;
|
|
|
nPre = (parObjTrackList->nEnd - (nFilterCnt - 1) + nListSize) % nListSize;
|
|
|
fAzCurr = parHistoryList[nEnd].afAngle.fAz;
|
|
|
fAzPre = parHistoryList[nPre].afAngle.fAz;
|
|
|
if (parHistoryList[nEnd].unFrmID > parHistoryList[nPre].unFrmID)
|
|
|
{
|
|
|
nSpeedStep = (SINT32)(parHistoryList[nEnd].unFrmID - parHistoryList[nPre].unFrmID);
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
nSpeedStep = (SINT32)(parHistoryList[nPre].unFrmID - parHistoryList[nEnd].unFrmID);
|
|
|
}
|
|
|
if (1 == parObjTrackList->nCnt || 0 == nSpeedStep)
|
|
|
{
|
|
|
//fAzSpeed = pObjStatus->sfAglSpeed.vx;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
//=======201800626zy？？fAzPre=0.0f没有值，所以计算的角速度很大======
|
|
|
fAzSpeed = DEGLIM(fAzCurr - fAzPre) / nSpeedStep;
|
|
|
}
|
|
|
parObjTrackList->arfSpeed.afAngle.fAz = fAzSpeed; //20180626zy:目标角速度长短时
|
|
|
|
|
|
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++
|
|
|
//俯仰角滤波
|
|
|
//判断角度是否跨越0/360度边界
|
|
|
FLOAT32 fObjSpeedY = parObjTrackList->arfSpeed.afAngle.fPt / p_GLB_Input->stCamera.fAglReso;
|
|
|
if (parObjTrackList->nCnt > 1)
|
|
|
{
|
|
|
//MSSu, 20150617: 测试发现若某帧目标速度>=20，nFilterCnt=1后，
|
|
|
// 目标角速度一直未更新，即使目标速度已下降。
|
|
|
// 因此，此处将目标短时预测角速度与相邻帧角速度取均值。
|
|
|
//MSSu, 20150808: 计算速度时，增加帧间隔，因长短轨迹记录的帧间隔不一致。
|
|
|
nEnd = parObjTrackList->nEnd;
|
|
|
nPre = (parObjTrackList->nEnd - 1 + nListSize) % nListSize;
|
|
|
fPtCurr = parHistoryList[nEnd].afAngle.fPt;
|
|
|
fPtPre = parHistoryList[nPre].afAngle.fPt;
|
|
|
FLOAT32 fObjSpeedYNear = DEGLIM(fPtCurr - fPtPre);
|
|
|
fObjSpeedYNear /= parObjTrackList->nStep;
|
|
|
fObjSpeedYNear /= p_GLB_Input->stCamera.fAglReso;
|
|
|
fObjSpeedY = (fObjSpeedY + fObjSpeedYNear) / 2;
|
|
|
}
|
|
|
fObjSpeedY = MAX(ABS(fObjSpeedY), 1.0f);
|
|
|
nFilterCnt = MAX((SINT32)(nFilterCntThres / fObjSpeedY), 1);
|
|
|
nFilterCnt = MIN(nFilterCnt, parObjTrackList->nCnt);
|
|
|
m = parObjTrackList->nEnd;
|
|
|
fPtSum = parHistoryList[m].afAngle.fPt;
|
|
|
FLOAT32 fPtMin = 360.0f;
|
|
|
FLOAT32 fPtMax = 0.0f;
|
|
|
BBOOL bPtCorrect = FALSE;
|
|
|
for (i = 0; i < nFilterCnt; i++)
|
|
|
{
|
|
|
fPtMin = MIN(fPtMin, parHistoryList[m].afAngle.fPt);
|
|
|
fPtMax = MAX(fPtMax, parHistoryList[m].afAngle.fPt);
|
|
|
m = (m + nListSize - 1) % nListSize;
|
|
|
}
|
|
|
if (fabs(fPtMin - fPtMax) > 180.0f)
|
|
|
{
|
|
|
bPtCorrect = TRUE;
|
|
|
}
|
|
|
m = parObjTrackList->nEnd;
|
|
|
for (i = 1; i < nFilterCnt; i++)
|
|
|
{
|
|
|
m = (m + nListSize - 1) % nListSize;
|
|
|
fPt = parHistoryList[m].afAngle.fPt;
|
|
|
if (bPtCorrect && fPt < 180.0f)
|
|
|
{
|
|
|
fPtSum += fPt + 360.0f;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
fPtSum += fPt;
|
|
|
}
|
|
|
}
|
|
|
parObjTrackList->arfFilter.afAngle.fPt = DEGLIM(fPtSum / nFilterCnt);
|
|
|
|
|
|
//俯仰角速度估算
|
|
|
nFilterCnt = MIN(MAX(2, nFilterCnt), parObjTrackList->nCnt);
|
|
|
nEnd = parObjTrackList->nEnd;
|
|
|
nPre = (parObjTrackList->nEnd - (nFilterCnt - 1) + nListSize) % nListSize;
|
|
|
fPtCurr = parHistoryList[nEnd].afAngle.fPt;
|
|
|
fPtPre = parHistoryList[nPre].afAngle.fPt;
|
|
|
if (parHistoryList[nEnd].unFrmID > parHistoryList[nPre].unFrmID)
|
|
|
{
|
|
|
nSpeedStep = (SINT32)(parHistoryList[nEnd].unFrmID - parHistoryList[nPre].unFrmID);
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
nSpeedStep = (SINT32)(parHistoryList[nPre].unFrmID - parHistoryList[nEnd].unFrmID);
|
|
|
}
|
|
|
if (1 == parObjTrackList->nCnt || 0 == nSpeedStep)
|
|
|
{
|
|
|
//fPtSpeed = pObjStatus->sfAglSpeed.vy;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
fPtSpeed = DEGLIM(fPtCurr - fPtPre) / nSpeedStep;
|
|
|
}
|
|
|
parObjTrackList->arfSpeed.afAngle.fPt = fPtSpeed; //20180626zy：目标角速度长短时
|
|
|
|
|
|
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++
|
|
|
//距离滤波
|
|
|
nFilterCnt = MIN(nFilterCntThres, parObjTrackList->nCnt);
|
|
|
m = parObjTrackList->nEnd;
|
|
|
fRSum = 0.0f;
|
|
|
for (i = 0; i < nFilterCnt; i++)
|
|
|
{
|
|
|
fRSum = fRSum + parHistoryList[m].fDist;
|
|
|
m = (m + nListSize - 1) % nListSize;
|
|
|
}
|
|
|
parObjTrackList->arfFilter.fDist = fRSum / nFilterCnt;
|
|
|
|
|
|
//距离变化速度估算
|
|
|
nEnd = parObjTrackList->nEnd;
|
|
|
nPre = (parObjTrackList->nEnd - (nFilterCnt - 1) + nListSize) % nListSize;
|
|
|
fRCurr = parHistoryList[nEnd].fDist;
|
|
|
fRPre = parHistoryList[nPre].fDist;
|
|
|
if (parHistoryList[nEnd].unFrmID > parHistoryList[nPre].unFrmID)
|
|
|
{
|
|
|
nSpeedStep = (SINT32)(parHistoryList[nEnd].unFrmID - parHistoryList[nPre].unFrmID);
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
nSpeedStep = (SINT32)(parHistoryList[nPre].unFrmID - parHistoryList[nEnd].unFrmID);
|
|
|
}
|
|
|
nSpeedStep = MAX(nSpeedStep, 1);
|
|
|
fRSpeed = (fRCurr - fRPre) / nSpeedStep;
|
|
|
parObjTrackList->arfSpeed.fDist = fRSpeed;
|
|
|
}
|
|
|
|
|
|
|
|
|
/**********************************************************
|
|
|
* 函数名称：Predict_CleanUpObjAglTrackInfo()
|
|
|
* 功能描述：清空目标跟踪轨迹信息
|
|
|
* 输入参数：无
|
|
|
* 输出参数：无
|
|
|
* 返 回 值：无
|
|
|
* 调用关系：无
|
|
|
* 其它说明：无
|
|
|
**********************************************************/
|
|
|
void Predict_CleanUpObjAglTrackInfo(FilterMeanNL* pFilter,SINT32 nFrmQ)
|
|
|
{
|
|
|
//初始化目标短时间轨迹数组（每帧记录一次）
|
|
|
pFilter->ObjAglListsNear.nCnt = 0;
|
|
|
pFilter->ObjAglListsNear.nEnd = 0;
|
|
|
pFilter->ObjAglListsNear.nStep = 1;
|
|
|
pFilter->ObjAglListsNear.nListSize = GLB_OBJTRACK_LEN;
|
|
|
memset(pFilter->ObjAglListsNear.parHistoryList, 0, sizeof(ANGLE_R) * GLB_OBJTRACK_LEN);
|
|
|
memset(&pFilter->ObjAglListsNear.arfFilter, 0, sizeof(ANGLE_R));
|
|
|
|
|
|
//初始化目标长时间轨迹数组（每25帧记录一次）
|
|
|
pFilter->ObjAglListsLong.nCnt = 0;
|
|
|
pFilter->ObjAglListsLong.nEnd = 0;
|
|
|
pFilter->ObjAglListsLong.nStep = nFrmQ / 4;
|
|
|
pFilter->ObjAglListsLong.nListSize = GLB_OBJTRACK_LEN;
|
|
|
memset(pFilter->ObjAglListsLong.parHistoryList, 0, sizeof(ANGLE_R) * GLB_OBJTRACK_LEN);
|
|
|
memset(&pFilter->ObjAglListsLong.arfFilter, 0, sizeof(ANGLE_R));
|
|
|
|
|
|
//初始化长短轨迹差异标志
|
|
|
pFilter->dnObjPredictDist.dx = 0;
|
|
|
pFilter->dnObjPredictDist.dy = 0;
|
|
|
pFilter->nObjPredictFarCnt = 0;
|
|
|
pFilter->bObjPredictAbnormal = FALSE;
|
|
|
pFilter->bObjTrackRefoundSucc = FALSE;
|
|
|
}
|
|
|
|
|
|
|
|
|
/**********************************************************
|
|
|
* 函数名称：TO_RecordObjAglTrackInfo()
|
|
|
* 功能描述：记录目标跟踪轨迹信息
|
|
|
* 输入参数：无
|
|
|
* 输出参数：无
|
|
|
* 返 回 值：无
|
|
|
* 调用关系：无
|
|
|
* 其它说明：短时轨迹用于目标实时位置滤波，长时轨迹用于目标航迹判断。【待添加】
|
|
|
**********************************************************/
|
|
|
void Predict_RecordObjAglTrackInfo(FilterMeanNL* pFilter, TARGET_OBJECT* pTarget, GLB_INPUT* p_GLB_Input)
|
|
|
{
|
|
|
//OBJECTSTATUS *pObjStatus = &pObjStatus;
|
|
|
OBJ_ANGLE_R *pObjAglListsNear = &pFilter->ObjAglListsNear;
|
|
|
OBJ_ANGLE_R *pObjAglListsLong = &pFilter->ObjAglListsLong;
|
|
|
ANGLE_R *parHistoryListNear = (ANGLE_R*)pObjAglListsNear->parHistoryList;
|
|
|
ANGLE_R *parHistoryListLong = (ANGLE_R*)pObjAglListsLong->parHistoryList;
|
|
|
SINT32 nEndNear, nEndLong;
|
|
|
UINT32 unFrmIdPreNear, unFrmIdPreLong;
|
|
|
SINT32 nFrmStepNear, nFrmStepLong;
|
|
|
|
|
|
// 上位机测试时，当跟踪过程中接受到解锁时，队列指针被清空，报错保护 by wcw04046 @ 2022/01/17
|
|
|
// 指针使用前判空
|
|
|
if(parHistoryListNear == NULL || parHistoryListLong == NULL)
|
|
|
{
|
|
|
return;
|
|
|
}
|
|
|
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++
|
|
|
//记录目标短时间轨迹（每帧记录一次）
|
|
|
if (0 == pObjAglListsNear->nCnt)
|
|
|
{
|
|
|
nEndNear = 0;
|
|
|
parHistoryListNear[nEndNear].unFrmID = p_GLB_Input->unFrmId;
|
|
|
parHistoryListNear[nEndNear].afAngle.fAz = pTarget->afAngle.fAz;
|
|
|
parHistoryListNear[nEndNear].afAngle.fPt = pTarget->afAngle.fPt;
|
|
|
pObjAglListsNear->nCnt++;
|
|
|
pObjAglListsNear->nEnd = nEndNear;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
//上一次记录的帧数
|
|
|
nEndNear = pObjAglListsNear->nEnd;
|
|
|
unFrmIdPreNear = parHistoryListNear[nEndNear].unFrmID;
|
|
|
|
|
|
//若达到记录帧间隔
|
|
|
if (p_GLB_Input->unFrmId > unFrmIdPreNear)
|
|
|
{
|
|
|
nFrmStepNear = (SINT32)(p_GLB_Input->unFrmId - unFrmIdPreNear);
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
nFrmStepNear = (SINT32)(unFrmIdPreNear - p_GLB_Input->unFrmId);
|
|
|
}
|
|
|
|
|
|
if ((nFrmStepNear >= pObjAglListsNear->nStep) /*&& (pObjStatus->unContiLostCnt <= 0)*/)
|
|
|
{
|
|
|
nEndNear = (pObjAglListsNear->nEnd + 1) % pObjAglListsNear->nListSize;
|
|
|
parHistoryListNear[nEndNear].unFrmID = p_GLB_Input->unFrmId;
|
|
|
parHistoryListNear[nEndNear].afAngle.fAz = pTarget->afAngle.fAz;
|
|
|
parHistoryListNear[nEndNear].afAngle.fPt = pTarget->afAngle.fPt;
|
|
|
pObjAglListsNear->nCnt++;
|
|
|
pObjAglListsNear->nEnd = nEndNear;
|
|
|
}
|
|
|
}
|
|
|
|
|
|
//目标短时轨迹滤波及速度估算
|
|
|
Predict_ObjAglTrackFilter(pFilter,pObjAglListsNear, 20, pTarget, p_GLB_Input);
|
|
|
|
|
|
|
|
|
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++
|
|
|
//记录目标长时间轨迹（每25帧记录一次）
|
|
|
if (0 == pObjAglListsLong->nCnt)
|
|
|
{
|
|
|
nEndLong = 0;
|
|
|
parHistoryListLong[nEndLong].unFrmID = p_GLB_Input->unFrmId;
|
|
|
parHistoryListLong[nEndLong].afAngle.fAz = pTarget->afAngle.fAz;
|
|
|
parHistoryListLong[nEndLong].afAngle.fPt = pTarget->afAngle.fPt;
|
|
|
pObjAglListsLong->nCnt++;
|
|
|
pObjAglListsLong->nEnd = nEndLong;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
//上一次记录的帧数
|
|
|
nEndLong = pObjAglListsLong->nEnd;
|
|
|
unFrmIdPreLong = parHistoryListLong[nEndLong].unFrmID;
|
|
|
|
|
|
//若长短轨迹预测异常，则不更新长时轨迹
|
|
|
BBOOL bUpdateLong = true;
|
|
|
if (pFilter->bObjPredictAbnormal
|
|
|
&& !pFilter->bObjTrackRefoundSucc)
|
|
|
{
|
|
|
bUpdateLong = false;
|
|
|
}
|
|
|
|
|
|
//若达到记录帧间隔
|
|
|
if (p_GLB_Input->unFrmId > unFrmIdPreLong)
|
|
|
{
|
|
|
nFrmStepLong = (SINT32)(p_GLB_Input->unFrmId - unFrmIdPreLong);
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
nFrmStepLong = (SINT32)(unFrmIdPreLong - p_GLB_Input->unFrmId);
|
|
|
}
|
|
|
if (nFrmStepLong >= pObjAglListsLong->nStep
|
|
|
&& bUpdateLong
|
|
|
/*&& (pObjStatus->unContiLostCnt <= 0)*/)
|
|
|
{
|
|
|
nEndLong = (pObjAglListsLong->nEnd + 1) % pObjAglListsLong->nListSize;
|
|
|
parHistoryListLong[nEndLong].unFrmID = p_GLB_Input->unFrmId;
|
|
|
parHistoryListLong[nEndLong].afAngle.fAz = pTarget->afAngle.fAz;
|
|
|
parHistoryListLong[nEndLong].afAngle.fPt = pTarget->afAngle.fPt;
|
|
|
pObjAglListsLong->nCnt++;
|
|
|
pObjAglListsLong->nEnd = nEndLong;
|
|
|
}
|
|
|
}
|
|
|
|
|
|
//目标长时轨迹滤波及速度估算
|
|
|
if (pObjAglListsLong->nCnt <= 1)
|
|
|
{
|
|
|
pObjAglListsLong->arfFilter = pObjAglListsNear->arfFilter;
|
|
|
pObjAglListsLong->arfSpeed = pObjAglListsNear->arfSpeed;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
Predict_ObjAglTrackFilter(pFilter,pObjAglListsLong, 10, pTarget, p_GLB_Input);
|
|
|
}
|
|
|
|
|
|
|
|
|
// 长时轨迹异常处理
|
|
|
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++
|
|
|
//MSSu, 20150513: 记录长短时轨迹预测失败帧数
|
|
|
POINT32F ptObjPos = pTarget->pfCenPos;
|
|
|
MINMAXRECT32S mrnObjPrediNear = pFilter->mrnObjPrediRtNear;
|
|
|
MINMAXRECT32S mrnObjPrediLong = pFilter->mrnObjPrediRtLong;
|
|
|
if (mrnObjPrediNear.minX <= ptObjPos.x && ptObjPos.x <= mrnObjPrediNear.maxX
|
|
|
&& mrnObjPrediNear.minY <= ptObjPos.y && ptObjPos.y <= mrnObjPrediNear.maxY)
|
|
|
{
|
|
|
pFilter->nObjTrackLostCntNear = 0;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
pFilter->nObjTrackLostCntNear++;
|
|
|
}
|
|
|
if (mrnObjPrediLong.minX <= ptObjPos.x && ptObjPos.x <= mrnObjPrediLong.maxX
|
|
|
&& mrnObjPrediLong.minY <= ptObjPos.y && ptObjPos.y <= mrnObjPrediLong.maxY)
|
|
|
{
|
|
|
pFilter->nObjTrackLostCntLong = 0;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
pFilter->nObjTrackLostCntLong++;
|
|
|
}
|
|
|
|
|
|
//若长时轨迹预测失败次数过多，而短时轨迹预测成功，则可能为机动目标，
|
|
|
//此时，重置长时轨迹
|
|
|
if (pFilter->bObjPredictAbnormal
|
|
|
&& pFilter->nObjTrackLostCntLong > 5//25 改小，更快的调整预测点位
|
|
|
&& pFilter->nObjTrackLostCntNear <= 0)
|
|
|
{
|
|
|
pFilter->ObjAglListsLong.nCnt = 0;
|
|
|
pFilter->ObjAglListsLong.nEnd = 0;
|
|
|
pFilter->ObjAglListsLong.nStep = p_GLB_Input->unFreq / 4;
|
|
|
//g_GLB_stOutput.ObjAglListsLong.nListSize = GLB_OBJTRACK_LONG;
|
|
|
//g_GLB_stOutput.ObjAglListsLong.parHistoryList = (ANGLE_R*)DSP_GLB_pObjAglListsLong;
|
|
|
memset(pFilter->ObjAglListsLong.parHistoryList, 0, sizeof(ANGLE_R) * GLB_OBJTRACK_LEN);
|
|
|
memset(&pFilter->ObjAglListsLong.arfFilter, 0, sizeof(ANGLE_R));
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
//**********************************************************
|
|
|
//* 函数名称：TO_ObjAglTrackPredictNext()
|
|
|
//* 功能描述：目标轨迹预测：利用长短时目标方位、俯仰航迹，预测目标在下一帧的角度位置
|
|
|
//* 输入参数：SINT32 nWidth -- 图像宽度
|
|
|
//* SINT32 nHeight -- 图像高度
|
|
|
//* 输出参数：无
|
|
|
//* 返 回 值：无
|
|
|
//* 调用关系：无
|
|
|
//* 其它说明：
|
|
|
//* MSSu, 20171204: 将轨迹预测TO_ObjAglTrackPredict()拆分成两个部分执行：
|
|
|
//* 1）TO_ObjAglTrackPredictNext()“下一帧长短时角度预测” ：
|
|
|
//* 在当前帧TrackObject()结束时执行，
|
|
|
//* 因DSP中核A面目标检测用的搜索区域，是核B在前一帧算法结束后传递过来的。
|
|
|
//* 2）TO_ObjAglTrackPredictCurrent()“当前帧长短时预测的搜索区域”：
|
|
|
//* 在当前帧TrackObject()跟踪算法执行前执行，利用当前帧伺服角度计算图像预测区域。
|
|
|
//**********************************************************/
|
|
|
//void Predict_ObjAglTrackPredictNext(FilterMeanNL* pFilter,SINT32 nWidth, SINT32 nHeight, GLB_INPUT* g_GLB_stInput)
|
|
|
//{
|
|
|
// //OBJECTSTATUS *pObjStatus = &g_GLB_stOutput.ObjectStatus;
|
|
|
// OBJ_ANGLE_R *pObjAglListsNear = &pFilter->ObjAglListsNear;
|
|
|
// OBJ_ANGLE_R *pObjAglListsLong = &pFilter->ObjAglListsLong;
|
|
|
// ANGLE_R *parHistoryListNear = (ANGLE_R*)pObjAglListsNear->parHistoryList;
|
|
|
// ANGLE_R *parHistoryListLong = (ANGLE_R*)pObjAglListsLong->parHistoryList;
|
|
|
// SINT32 nEndNear, nEndLong;
|
|
|
// UINT32 unFrmIdPreNear, unFrmIdPreLong;
|
|
|
// UINT32 unFrmId, unFrmStepNear, unFrmStepLong;
|
|
|
// ANGLE32F fAglPreNear, fAglPreLong;
|
|
|
// ANGLE32F fAglSpeedNear, fAglSpeedLong;
|
|
|
// ANGLE32F fAglPredictNear, fAglPredictLong;
|
|
|
// FLOAT32 fRPreNear, fRSpeedNear, fRPredictNear;
|
|
|
// FLOAT32 fRPreLong, fRSpeedLong, fRPredictLong;
|
|
|
// CENTERRECT crnSrRectNear, crnSrRectLong;
|
|
|
// SPEED32F fSpeedNear, fSpeedLong;
|
|
|
// SIZE32S snSrRadiusNear, snSrRadiusLong;
|
|
|
//
|
|
|
// //+++++++++++++++++++++++++++++++++++++++++++++++++++++++
|
|
|
// //初始化
|
|
|
// unFrmId = g_GLB_stInput->unFrmId;
|
|
|
// pObjAglListsNear->arfPredict.unFrmID = unFrmId;
|
|
|
// pObjAglListsLong->arfPredict.unFrmID = unFrmId;
|
|
|
//
|
|
|
// //+++++++++++++++++++++++++++++++++++++++++++++++++++++++
|
|
|
// //目标短时间轨迹预测
|
|
|
// //计算当前帧与上一次轨迹记录的间隔帧数
|
|
|
// nEndNear = pObjAglListsNear->nEnd;
|
|
|
// unFrmIdPreNear = parHistoryListNear[nEndNear].unFrmID;
|
|
|
// if (unFrmId > unFrmIdPreNear)
|
|
|
// {
|
|
|
// unFrmStepNear = unFrmId - unFrmIdPreNear;
|
|
|
// }
|
|
|
// else
|
|
|
// {
|
|
|
// unFrmStepNear = unFrmIdPreNear - unFrmId;
|
|
|
// }
|
|
|
//
|
|
|
// //获取上一次轨迹记录的目标角度、角速度、距离、距离变化速度
|
|
|
// fAglPreNear = parHistoryListNear[nEndNear].afAngle;
|
|
|
// fAglSpeedNear = pObjAglListsNear->arfSpeed.afAngle;
|
|
|
// fRPreNear = parHistoryListNear[nEndNear].fDist;
|
|
|
// fRSpeedNear = pObjAglListsNear->arfSpeed.fDist;
|
|
|
//
|
|
|
// //估算当前帧预测的目标角度、距离
|
|
|
// fAglPredictNear.fAz = DEGLIM360(fAglPreNear.fAz + fAglSpeedNear.fAz * unFrmStepNear);
|
|
|
// fAglPredictNear.fPt = DEGLIM(fAglPreNear.fPt + fAglSpeedNear.fPt * unFrmStepNear);
|
|
|
// fRPredictNear = fRPreNear + fRSpeedNear * unFrmStepNear;
|
|
|
// pObjAglListsNear->arfPredict.afAngle = fAglPredictNear;
|
|
|
// pObjAglListsNear->arfPredict.fDist = fRPredictNear;
|
|
|
//
|
|
|
// //+++++++++++++++++++++++++++++++++++++++++++++++++++++++
|
|
|
// //估算当前帧搜索区域矩形
|
|
|
// //搜索区域中心
|
|
|
// //
|
|
|
// //搜索区域半径
|
|
|
// fSpeedNear.vx = fAglSpeedNear.fAz / g_SERVO_stInput.fResolAz;
|
|
|
// fSpeedNear.vy = fAglSpeedNear.fPt / g_SERVO_stInput.fResolPt;
|
|
|
//// snSrRadiusNear.w = MAX(ABS((SINT32)(fSpeedNear.vx * 2)), g_DST_stPara.nPipeRadiusTrack);
|
|
|
//// snSrRadiusNear.h = MAX(ABS((SINT32)(fSpeedNear.vy * 2)), g_DST_stPara.nPipeRadiusTrack);
|
|
|
// snSrRadiusNear.w = MAX(ABS((SINT32)(fSpeedNear.vx * 2)), g_PIPE_stPara.nPipeRadiusTrack);
|
|
|
// snSrRadiusNear.h = MAX(ABS((SINT32)(fSpeedNear.vy * 2)), g_PIPE_stPara.nPipeRadiusTrack);
|
|
|
//
|
|
|
// crnSrRectNear.w = 2 * snSrRadiusNear.w + 1;
|
|
|
// crnSrRectNear.h = 2 * snSrRadiusNear.h + 1;
|
|
|
// crnSrRectNear.s = crnSrRectNear.w * crnSrRectNear.h;
|
|
|
// g_GLB_stOutput.crnObjPrediRtNear = crnSrRectNear;
|
|
|
// IMGO_CenRect16S2MMRect32S(crnSrRectNear, &g_GLB_stOutput.mrnObjPrediRtNear,nWidth, nHeight);
|
|
|
//
|
|
|
// //+++++++++++++++++++++++++++++++++++++++++++++++++++++++
|
|
|
// //记录目标长时间轨迹（每秒记录一次）
|
|
|
// //在长时间轨迹更新第2个位置之前，一直跟短时间轨迹预测位置保持一致
|
|
|
// //if (pObjAglListsLong->nCnt <= 1 || g_GLB_stOutput.bObjTrackRefoundSucc)
|
|
|
// if (pObjAglListsLong->nCnt <= 2)
|
|
|
// {
|
|
|
// fAglSpeedLong = fAglSpeedNear;
|
|
|
// fAglPredictLong = pObjAglListsNear->arfPredict.afAngle;
|
|
|
// pObjAglListsLong->arfPredict.afAngle = fAglPredictLong;
|
|
|
// }
|
|
|
// else
|
|
|
// {
|
|
|
// //计算当前帧与上一次轨迹记录的间隔帧数
|
|
|
// nEndLong = pObjAglListsLong->nEnd;
|
|
|
// unFrmIdPreLong = parHistoryListLong[nEndLong].unFrmID;
|
|
|
// if (unFrmId > unFrmIdPreLong)
|
|
|
// {
|
|
|
// unFrmStepLong = unFrmId - unFrmIdPreLong;
|
|
|
// }
|
|
|
// else
|
|
|
// {
|
|
|
// unFrmStepLong = unFrmIdPreLong - unFrmId;
|
|
|
// }
|
|
|
//
|
|
|
// //获取上一次轨迹记录的目标角度、角速度、距离、距离变化速度
|
|
|
// fAglPreLong = parHistoryListLong[nEndLong].afAngle;
|
|
|
// fAglSpeedLong = pObjAglListsLong->arfSpeed.afAngle;
|
|
|
// fRPreLong = parHistoryListLong[nEndLong].fDist;
|
|
|
// fRSpeedLong = pObjAglListsLong->arfSpeed.fDist;
|
|
|
//
|
|
|
// //估算当前帧预测的目标角度、距离
|
|
|
// fAglPredictLong.fAz = SERVO_AddAzimuthScale360(fAglPreLong.fAz, fAglSpeedLong.fAz * unFrmStepLong);
|
|
|
// fAglPredictLong.fPt = SERVO_AddPitchingScale360(fAglPreLong.fPt, fAglSpeedLong.fPt * unFrmStepLong);
|
|
|
// fRPredictLong = fRPreLong + fRSpeedLong * unFrmStepLong;
|
|
|
// pObjAglListsLong->arfPredict.afAngle = fAglPredictLong;
|
|
|
// pObjAglListsLong->arfPredict.fDist = fRPredictLong;
|
|
|
// }
|
|
|
//
|
|
|
// //+++++++++++++++++++++++++++++++++++++++++++++++++++++++
|
|
|
// //估算当前帧搜索区域矩形
|
|
|
// //搜索区域中心
|
|
|
// crnSrRectLong.cx = (SINT16)(SERVO_CalcObjCoordinateX_1(fAglPredictLong.fAz,
|
|
|
// nWidth, g_SERVO_stInput.fAzimuth, g_SERVO_stInput.fResolAz, g_SERVO_stInput.fPitching));
|
|
|
// crnSrRectLong.cy = (SINT16)(SERVO_CalcObjCoordinateY(fAglPredictLong.fPt,
|
|
|
// nHeight, g_SERVO_stInput.fPitching, g_SERVO_stInput.fResolPt));
|
|
|
//
|
|
|
// //搜索区域半径
|
|
|
// fSpeedLong.vx = fAglSpeedLong.fAz / g_SERVO_stInput.fResolAz;
|
|
|
// fSpeedLong.vy = fAglSpeedLong.fPt / g_SERVO_stInput.fResolPt;
|
|
|
// //snSrRadiusLong.w = MAX(ABS((SINT32)(fSpeedLong.vx * 2)), g_DST_stPara.nPipeRadiusTrack);
|
|
|
// //snSrRadiusLong.h = MAX(ABS((SINT32)(fSpeedLong.vy * 2)), g_DST_stPara.nPipeRadiusTrack);
|
|
|
// snSrRadiusLong.w = MAX(ABS((SINT32)(fSpeedLong.vx * 2)), g_PIPE_stPara.nPipeRadiusTrack);
|
|
|
// snSrRadiusLong.h = MAX(ABS((SINT32)(fSpeedLong.vy * 2)), g_PIPE_stPara.nPipeRadiusTrack);
|
|
|
//
|
|
|
// crnSrRectLong.w = 2 * snSrRadiusLong.w + 1;
|
|
|
// crnSrRectLong.h = 2 * snSrRadiusLong.h + 1;
|
|
|
// crnSrRectLong.s = crnSrRectLong.w * crnSrRectLong.h;
|
|
|
// g_GLB_stOutput.crnObjPrediRtLong = crnSrRectLong;
|
|
|
// IMGO_CenRect16S2MMRect32S(crnSrRectLong, &g_GLB_stOutput.mrnObjPrediRtLong,nWidth, nHeight);
|
|
|
//
|
|
|
// //+++++++++++++++++++++++++++++++++++++++++++++++++++++++
|
|
|
// //判断长短轨迹预测位置是否一致
|
|
|
// DIST32S dnNLDist, dnNLDistPre;
|
|
|
// dnNLDistPre = g_GLB_stOutput.dnObjPredictDist;
|
|
|
//
|
|
|
// // 20180224 取绝对值 长短时预测异常时，修正搜索区域使用，防止搜索区域变小
|
|
|
// dnNLDist.dx = ABS(SERVO_CalcAzimuthXOffset(fAglPredictNear.fAz, fAglPredictLong.fAz, g_SERVO_stInput.fResolAz ));
|
|
|
// dnNLDist.dy = ABS(SERVO_CalcPitchingYOffset(fAglPredictNear.fPt, fAglPredictLong.fPt, g_SERVO_stInput.fResolPt));
|
|
|
//
|
|
|
// SINT32 fNLDist2 = dnNLDist.dx * dnNLDist.dx + dnNLDist.dy * dnNLDist.dy;
|
|
|
// SINT32 fNLDist2Pre = dnNLDistPre.dx * dnNLDistPre.dx + dnNLDistPre.dy * dnNLDistPre.dy;
|
|
|
// //SINT32 nNLDist2Thres = g_DST_stPara.nPipeRadiusTrack * g_DST_stPara.nPipeRadiusTrack;
|
|
|
// SINT32 nNLDist2Thres = g_PIPE_stPara.nPipeRadiusTrack * g_PIPE_stPara.nPipeRadiusTrack;
|
|
|
// if (fNLDist2 - fNLDist2Pre > 2 || fNLDist2 > nNLDist2Thres)
|
|
|
// {
|
|
|
// g_GLB_stOutput.nObjPredictFarCnt++;
|
|
|
// }
|
|
|
// else
|
|
|
// {
|
|
|
// g_GLB_stOutput.nObjPredictFarCnt = 0;
|
|
|
// g_GLB_stOutput.bObjPredictAbnormal = false;
|
|
|
// }
|
|
|
// if (g_GLB_stOutput.nObjPredictFarCnt > 10 && fNLDist2 > nNLDist2Thres)
|
|
|
// {
|
|
|
// g_GLB_stOutput.bObjPredictAbnormal = true;
|
|
|
// }
|
|
|
// g_GLB_stOutput.dnObjPredictDist = dnNLDist;
|
|
|
//
|
|
|
// ////利用长短时轨迹判断是否开启惯性预测 by wcw04046 @ 2022/01/17
|
|
|
// //if (g_GLB_stOutput.nObjPredictFarCnt == 0)
|
|
|
// //{
|
|
|
// // g_GLB_stPara.bEnableServoTrackPredi = true;
|
|
|
// //}
|
|
|
// //else
|
|
|
// //{
|
|
|
// // g_GLB_stPara.bEnableServoTrackPredi = false;
|
|
|
// //}
|
|
|
//}
|
|
|
//
|
|
|
|
|
|
///**********************************************************
|
|
|
//* 函数名称：TO_ObjAglTrackPredictCurrent()
|
|
|
//* 功能描述：目标轨迹预测：利用长短时目标方位、俯仰航迹
|
|
|
//* 输入参数：SINT32 nWidth -- 图像宽度
|
|
|
//* SINT32 nHeight -- 图像高度
|
|
|
//* 输出参数：无
|
|
|
//* 返 回 值：无
|
|
|
//* 调用关系：无
|
|
|
//* 其它说明：
|
|
|
//* MSSu, 20171204: 将轨迹预测TO_ObjAglTrackPredict()拆分成两个部分执行：
|
|
|
//* 1）TO_ObjAglTrackPredictNext()“下一帧长短时角度预测” ：
|
|
|
//* 在当前帧TrackObject()结束时执行，
|
|
|
//* 因DSP中核A面目标检测用的搜索区域，是核B在前一帧算法结束后传递过来的。
|
|
|
//* 2）TO_ObjAglTrackPredictCurrent()“当前帧长短时预测的搜索区域”：
|
|
|
//* 在当前帧TrackObject()跟踪算法执行前执行，利用当前帧伺服角度计算图像预测区域。
|
|
|
//**********************************************************/
|
|
|
//void Predict_ObjAglResolvePredictInfo(FilterMeanNL* pFilter,SINT32 nWidth, SINT32 nHeight)
|
|
|
//{
|
|
|
// OBJ_ANGLE_R *pObjAglListsNear = &g_GLB_stOutput.ObjAglListsNear;
|
|
|
// OBJ_ANGLE_R *pObjAglListsLong = &g_GLB_stOutput.ObjAglListsLong;
|
|
|
// ANGLE32F fAglPredictNear, fAglPredictLong;
|
|
|
// CENTERRECT crnSrRectNear, crnSrRectLong;
|
|
|
// SPEED32F fSpeedNear, fSpeedLong;
|
|
|
// SIZE32S snSrRadiusNear, snSrRadiusLong;
|
|
|
//
|
|
|
// //+++++++++++++++++++++++++++++++++++++++++++++++++++++++
|
|
|
// //估算当前帧短时间搜索区域矩形
|
|
|
// //搜索区域中心
|
|
|
// fAglPredictNear = pObjAglListsNear->arfPredict.afAngle;
|
|
|
// crnSrRectNear.cx = (SINT16)(SERVO_CalcObjCoordinateX_1(fAglPredictNear.fAz,
|
|
|
// nWidth, g_SERVO_stInput.fAzimuth, g_SERVO_stInput.fResolAz, g_SERVO_stInput.fPitching));
|
|
|
///* crnSrRectNear.cx = (SINT16)(SERVO_CalcObjCoordinateX_1(fAglPredictNear.fAz,
|
|
|
// nWidth, g_SERVO_stInput.fAzimuth, g_SERVO_stInput.fResolAz, g_SERVO_stInput.fPitching))*/;
|
|
|
// crnSrRectNear.cy = (SINT16)(SERVO_CalcObjCoordinateY(fAglPredictNear.fPt,
|
|
|
// nHeight, g_SERVO_stInput.fPitching, g_SERVO_stInput.fResolPt));
|
|
|
//
|
|
|
// //搜索区域半径
|
|
|
// fSpeedNear.vx = pObjAglListsNear->arfSpeed.afAngle.fAz / g_SERVO_stInput.fResolAz;
|
|
|
// fSpeedNear.vy = pObjAglListsNear->arfSpeed.afAngle.fPt / g_SERVO_stInput.fResolPt;
|
|
|
//// snSrRadiusNear.w = MAX(ABS((SINT32)(fSpeedNear.vx * 2)), g_DST_stPara.nPipeRadiusTrack);
|
|
|
//// snSrRadiusNear.h = MAX(ABS((SINT32)(fSpeedNear.vy * 2)), g_DST_stPara.nPipeRadiusTrack);
|
|
|
// snSrRadiusNear.w = MAX(ABS((SINT32)(fSpeedNear.vx * 2)), g_PIPE_stPara.nPipeRadiusTrack);
|
|
|
// snSrRadiusNear.h = MAX(ABS((SINT32)(fSpeedNear.vy * 2)), g_PIPE_stPara.nPipeRadiusTrack);
|
|
|
// crnSrRectNear.w = 2 * snSrRadiusNear.w + 1;
|
|
|
// crnSrRectNear.h = 2 * snSrRadiusNear.h + 1;
|
|
|
// crnSrRectNear.s = crnSrRectNear.w * crnSrRectNear.h;
|
|
|
// g_GLB_stOutput.crnObjPrediRtNear = crnSrRectNear;
|
|
|
// IMGO_CenRect16S2MMRect32S(crnSrRectNear, &g_GLB_stOutput.mrnObjPrediRtNear,nWidth, nHeight);
|
|
|
//
|
|
|
// //+++++++++++++++++++++++++++++++++++++++++++++++++++++++
|
|
|
// //估算当前帧长时间搜索区域矩形
|
|
|
// //20170702，wsa,搜索区域中心
|
|
|
// fAglPredictLong = pObjAglListsLong->arfPredict.afAngle;
|
|
|
// crnSrRectLong.cx = (SINT16)(SERVO_CalcObjCoordinateX_1(fAglPredictLong.fAz,
|
|
|
// nWidth, g_SERVO_stInput.fAzimuth, g_SERVO_stInput.fResolAz, g_SERVO_stInput.fPitching));
|
|
|
// //crnSrRectLong.cx = (SINT16)(SERVO_CalcObjCoordinateX_1(fAglPredictLong.fAz,
|
|
|
// // nWidth, g_SERVO_stInput.fAzimuth, g_SERVO_stInput.fResolAz, g_SERVO_stInput.fPitching));
|
|
|
// crnSrRectLong.cy = (SINT16)(SERVO_CalcObjCoordinateY(fAglPredictLong.fPt,
|
|
|
// nHeight, g_SERVO_stInput.fPitching, g_SERVO_stInput.fResolPt));
|
|
|
//
|
|
|
// //搜索区域半径
|
|
|
// fSpeedLong.vx = pObjAglListsLong->arfSpeed.afAngle.fAz / g_SERVO_stInput.fResolAz;
|
|
|
// fSpeedLong.vy = pObjAglListsLong->arfSpeed.afAngle.fPt / g_SERVO_stInput.fResolPt;
|
|
|
// //snSrRadiusLong.w = MAX(ABS((SINT32)(fSpeedLong.vx * 2)), g_DST_stPara.nPipeRadiusTrack);
|
|
|
// //snSrRadiusLong.h = MAX(ABS((SINT32)(fSpeedLong.vy * 2)), g_DST_stPara.nPipeRadiusTrack);
|
|
|
// snSrRadiusLong.w = MAX(ABS((SINT32)(fSpeedLong.vx * 2)), g_PIPE_stPara.nPipeRadiusTrack);
|
|
|
// snSrRadiusLong.h = MAX(ABS((SINT32)(fSpeedLong.vy * 2)), g_PIPE_stPara.nPipeRadiusTrack);
|
|
|
// crnSrRectLong.w = 2 * snSrRadiusLong.w + 1;
|
|
|
// crnSrRectLong.h = 2 * snSrRadiusLong.h + 1;
|
|
|
// crnSrRectLong.s = crnSrRectLong.w * crnSrRectLong.h;
|
|
|
// g_GLB_stOutput.crnObjPrediRtLong = crnSrRectLong;
|
|
|
// IMGO_CenRect16S2MMRect32S(crnSrRectLong, &g_GLB_stOutput.mrnObjPrediRtLong,nWidth, nHeight);
|
|
|
//}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
