You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
1275 lines
48 KiB
1275 lines
48 KiB
#include <thread>
|
|
#include "daSiamRpnTrck.h"
|
|
#include <opencv2/imgproc.hpp>
|
|
// #include "sample_comm_ive_.h"
|
|
#include "guidePlat.h"
|
|
#include "gd_alg_type.h"
|
|
#include "cJSON.h"
|
|
|
|
using namespace GUD::ALG;
|
|
|
|
void cvImageToTensor(const unsigned char* img_data, unsigned char* tensor, int channels, int height, int width) {
|
|
int align = 2;
|
|
int dstStride = (width + (align - width % align) % align);
|
|
int hs = height * dstStride;
|
|
int iwc = 0;
|
|
int is = 0;
|
|
int jc = 0;
|
|
for (int i=0; i<height; i++) {
|
|
iwc = i * width * channels;
|
|
is = i * dstStride;
|
|
for (int j=0; j<width; j++) {
|
|
jc = j * channels;
|
|
for (int k=0; k<channels; k++) {
|
|
const size_t offsetCv = iwc + jc + k;
|
|
const size_t offset = k * hs + is + j;
|
|
tensor[offset] = img_data[offsetCv];
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
template <typename T> static
|
|
T sizeCal(const T& w, const T& h)
|
|
{
|
|
T pad = (w + h) * T(0.5);
|
|
T sz2 = (w + pad) * (h + pad);
|
|
return sqrt(sz2);
|
|
}
|
|
|
|
template <>
|
|
cv::Mat sizeCal(const cv::Mat& w, const cv::Mat& h)
|
|
{
|
|
cv::Mat pad = (w + h) * 0.5;
|
|
cv::Mat sz2 = (w + pad).mul((h + pad));
|
|
|
|
cv::sqrt(sz2, sz2);
|
|
return sz2;
|
|
}
|
|
|
|
void elementMax(cv::Mat& src)
|
|
{
|
|
int* p = src.size.p;
|
|
int index[4] = { 0, 0, 0, 0 };
|
|
for (int n = 0; n < *p; n++)
|
|
{
|
|
for (int k = 0; k < *(p + 1); k++)
|
|
{
|
|
for (int i = 0; i < *(p + 2); i++)
|
|
{
|
|
for (int j = 0; j < *(p + 3); j++)
|
|
{
|
|
index[0] = n, index[1] = k, index[2] = i, index[3] = j;
|
|
float& v = src.at<float>(index);
|
|
v = fmax(v, 1.0f / v);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
DaSiamRPNtracker::DaSiamRPNtracker(const gud_siamrpn_config_t config)
|
|
{
|
|
initFlag_ = 0;
|
|
ReidInit_ = 0;
|
|
MnnLoad_ = 0;
|
|
mnnIdx_ = 0;
|
|
nnieIdx_ = -1;
|
|
TempNum_ = 5;
|
|
UpdateFrq_= 5;
|
|
detThd_ = config.ac_score;
|
|
acNumThd_ = config.ac_num;
|
|
trackState.lr = config.lr_rate;
|
|
|
|
UpdataTemp_ = config.use_update;
|
|
UpdateFrq_ = config.update_frq;
|
|
|
|
temple_net_.loadModel(config.nnieModel_0);
|
|
detect_net_.loadModel(config.nnieModel_1);
|
|
|
|
/// mnn net init (new method to load two mnn models with double inputs dynamic weights)
|
|
mnnDoubleBranch_ = new ForwardNet(config);
|
|
MnnLoad_ = 1;
|
|
tkTime_.maxTime = 0;
|
|
tkTime_.minTime = 1e6;
|
|
tkTime_.timeThd = 18;
|
|
|
|
#define AI_MODULE_PATH "/app/private/extapp/data"
|
|
if (UpdataTemp_){
|
|
const char *uptemp_model = AI_MODULE_PATH"/nnie_model/siamrpn_v1_ch256_template127_240628_low.wk";
|
|
uptemp_net_.loadModel(uptemp_model);
|
|
}
|
|
|
|
std::thread aithread(&DaSiamRPNtracker::nnieSearThrd, std::ref(*this));
|
|
bindCpusToThread(aithread.native_handle(), A73_0,A73_1);
|
|
//bindCpusToThread(aithread.native_handle(), A73_1,A53_1);
|
|
aithread.detach();
|
|
|
|
bufTmpAlloc(&tmpBuf0,991232);
|
|
//bufTmpAlloc(&tmpBuf1,sizeof(float)*4*1805);
|
|
imgTmpAlloc(&subImg,trackState.instanceSize,trackState.instanceSize,GUIDE_U8C3_PLANAR);
|
|
outTnsr[0].data = (float *)tmpBuf0.virAddr;
|
|
outTnsr[1].data = (float *)(tmpBuf0.virAddr + 495616);
|
|
|
|
if (config.use_reid){
|
|
reid_jsonReader(config.reid_json, &reid_config_);
|
|
nnie_reid_ = new NNIE_REID(reid_config_);
|
|
printf ("Using REID Model:[%s], init sucess!\n",config.reid_json);
|
|
ReidInit_ = 1;
|
|
ReidFrq_ = config.reid_frq;
|
|
ReidCycleUp_ = reid_config_.cycle_update;
|
|
ReidUpRatio_ = reid_config_.update_ratio;
|
|
}
|
|
|
|
if (config.use_trace){
|
|
UseTrace_ = 1;
|
|
TraceLen_ = (unsigned int)(config.trace_len);
|
|
}
|
|
|
|
traces_ = std::deque<gud_rect_t>();
|
|
tagTemplates_ = std::deque<cv::Mat>();
|
|
|
|
}
|
|
|
|
DaSiamRPNtracker::~DaSiamRPNtracker()
|
|
{
|
|
bufTmpFree(&tmpBuf0);
|
|
//bufTmpFree(&tmpBuf1);
|
|
delete mnnDoubleBranch_;
|
|
imgTmpFree(&subImg);
|
|
delete nnie_reid_;
|
|
}
|
|
|
|
int DaSiamRPNtracker::nnieSearThrd(){
|
|
|
|
while(MnnLoad_) {
|
|
if (initFlag_) {
|
|
if (nnieIdx_ == mnnIdx_)
|
|
{
|
|
//printf ("NnieIDX==MnnIdx:[%d %d] do trackerUpdateNpu\n",nnieIdx_,mnnIdx_);
|
|
trackerUpdateNpu(fullImg,outBox_,updateReg_);
|
|
|
|
} else{
|
|
//printf ("NnieIDX != MnnIdx:[%d %d]\n",nnieIdx_,mnnIdx_);
|
|
usleep(200);
|
|
if (mnnIdx_ > nnieIdx_){
|
|
nnieIdx_ = mnnIdx_;
|
|
}
|
|
}
|
|
}
|
|
else{
|
|
usleep(200);
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
gud_rect_t DaSiamRPNtracker::selectTagFromDetection(gud_rect_t bbox, ai_detect_result_t *AIod_res)
|
|
{
|
|
gud_rect_t sBox = bbox;
|
|
/// Lock from AI-detection results if manual-Lock point in some targets'rect region
|
|
float disMin = 1e6;
|
|
int sIdx = -1;
|
|
if (AIod_res->detect_num > 0) {
|
|
float Pcx = static_cast<float>(bbox.cx); /// NOTE: manual select point_center_x
|
|
float Pcy = static_cast<float>(bbox.cy); /// NOTE: manual select point_center_y
|
|
for (int i = 0; i < AIod_res->detect_num; i++) {
|
|
int txmin = AIod_res->ai_result[i].x;
|
|
int tymin = AIod_res->ai_result[i].y;
|
|
int tw = AIod_res->ai_result[i].w;
|
|
int th = AIod_res->ai_result[i].h;
|
|
int txmax = txmin + tw;
|
|
int tymax = tymin + th;
|
|
float tcx = (txmin + tw / 2)*1.0;
|
|
float tcy = (tymin + th / 2)*1.0;
|
|
float dis = sqrt(pow(Pcx-tcx,2.0) + pow(Pcy-tcy,2.0));
|
|
|
|
if ((txmin <= Pcx) and (tymin <= Pcy) and (txmax >= Pcx) and (tymax >= Pcy)) {
|
|
sIdx = i;
|
|
break;
|
|
}
|
|
}
|
|
if (sIdx!=(-1)) {
|
|
sBox.cx = static_cast<int>(AIod_res->ai_result[sIdx].x + AIod_res->ai_result[sIdx].w/2.0);
|
|
sBox.cy = static_cast<int>(AIod_res->ai_result[sIdx].y + AIod_res->ai_result[sIdx].h/2.0);
|
|
sBox.w = static_cast<int>(AIod_res->ai_result[sIdx].w);
|
|
sBox.h = static_cast<int>(AIod_res->ai_result[sIdx].h);
|
|
sBox.label = AIod_res->ai_result[sIdx].ID;
|
|
sBox.used = 1;
|
|
printf("Select Target from DetectionResult[%d]_class:[%d]_conf:[%.2f]\n",sIdx,AIod_res->ai_result[sIdx].ID,
|
|
AIod_res->ai_result[sIdx].prob);
|
|
}else{
|
|
sBox = bbox;
|
|
}
|
|
}
|
|
return sBox;
|
|
}
|
|
|
|
gud_rect_f DaSiamRPNtracker::selectTagFromDetectionF(gud_rect_f bbox, ai_detect_result_t *AIod_res)
|
|
{
|
|
gud_rect_f sBox = bbox;
|
|
/// Lock from AI-detection results if manual-Lock point in some targets'rect region
|
|
float disMin = 1e6;
|
|
int sIdx = -1;
|
|
if (AIod_res->detect_num > 0) {
|
|
float Pcx = bbox.cx; /// NOTE: manual select point_center_x
|
|
float Pcy = bbox.cy; /// NOTE: manual select point_center_y
|
|
for (int i = 0; i < AIod_res->detect_num; i++) {
|
|
int txmin = AIod_res->ai_result[i].x;
|
|
int tymin = AIod_res->ai_result[i].y;
|
|
int tw = AIod_res->ai_result[i].w;
|
|
int th = AIod_res->ai_result[i].h;
|
|
int txmax = txmin + tw;
|
|
int tymax = tymin + th;
|
|
float tcx = (txmin + tw / 2)*1.0;
|
|
float tcy = (tymin + th / 2)*1.0;
|
|
float dis = sqrt(pow(Pcx-tcx,2.0) + pow(Pcy-tcy,2.0));
|
|
|
|
if ((txmin <= Pcx) and (tymin <= Pcy) and (txmax >= Pcx) and (tymax >= Pcy)) {
|
|
sIdx = i;
|
|
break;
|
|
}
|
|
}
|
|
if (sIdx!=(-1)) {
|
|
sBox.cx = static_cast<float>(AIod_res->ai_result[sIdx].x + AIod_res->ai_result[sIdx].w/2.0);
|
|
sBox.cy = static_cast<float>(AIod_res->ai_result[sIdx].y + AIod_res->ai_result[sIdx].h/2.0);
|
|
sBox.w = static_cast<float>(AIod_res->ai_result[sIdx].w);
|
|
sBox.h = static_cast<float>(AIod_res->ai_result[sIdx].h);
|
|
sBox.label = AIod_res->ai_result[sIdx].ID;
|
|
sBox.used = 1;
|
|
printf("Select Target from DetectionResult[%d]_class:[%d]_conf:[%.2f]\n",sIdx,AIod_res->ai_result[sIdx].ID,
|
|
AIod_res->ai_result[sIdx].prob);
|
|
}else{
|
|
sBox = bbox;
|
|
}
|
|
}
|
|
return sBox;
|
|
}
|
|
|
|
void DaSiamRPNtracker::trackerInit(GuideImage *pSrcImg, gud_rect_t initBox, ai_detect_result_t *od_res)
|
|
{
|
|
mnnIdx_ = 0;
|
|
nnieIdx_ = -1;
|
|
fullImg = pSrcImg;
|
|
|
|
gud_rect_t tagBox = selectTagFromDetection(initBox,od_res);
|
|
|
|
int width = pSrcImg->img.au32Stride[0];
|
|
int height = pSrcImg->img.u32Height;
|
|
int bgrSize = width * height * 3;
|
|
char *pSrcBgr = (char*)HI_MPI_SYS_MmapCache(pSrcImg->img.au64PhyAddr[0],bgrSize);
|
|
cv::Mat img = cv::Mat(height,width, CV_8UC3,pSrcBgr);
|
|
|
|
trackState.tracking_score = 1.0;
|
|
trackState.label = tagBox.label;
|
|
trackState.targetBox.x = static_cast<float>(tagBox.cx);
|
|
trackState.targetBox.y = static_cast<float>(tagBox.cy);
|
|
trackState.targetBox.width = static_cast<float>(tagBox.w);
|
|
trackState.targetBox.height = static_cast<float>(tagBox.h);
|
|
printf ("LockBox:[%.2f %.2f %.2f %.2f ] label:[%d]\n",trackState.targetBox.x ,trackState.targetBox.y,trackState.targetBox.width,
|
|
trackState.targetBox.height,trackState.label);
|
|
cv::Rect2f targetBox = trackState.targetBox;
|
|
cv::Rect2f searchBox = trackState.targetBox;
|
|
|
|
cv::Mat anchors = generateAnchors();
|
|
trackState.anchors = anchors;
|
|
cv::Mat windows = generateHanningWindow();
|
|
|
|
trackState.windows = windows;
|
|
trackState.imgSize = img.size();
|
|
|
|
trackState.avgChans = mean(img);
|
|
float wc = targetBox.width + trackState.contextAmount * (targetBox.width + targetBox.height);
|
|
float hc = targetBox.height + trackState.contextAmount * (targetBox.width + targetBox.height);
|
|
float sz = (float)cvRound(sqrt(wc * hc));
|
|
cv::Mat zCrop = getSubwindow(img, targetBox, sz, trackState.avgChans, trackState.exemplarSize);
|
|
HI_MPI_SYS_Munmap(pSrcBgr,bgrSize);
|
|
|
|
// std::string regionImg = "/nfsroot/001_3559/results/init" + std::to_string(trackState.FRN) + ".jpg";
|
|
// cv::imwrite(regionImg, zCrop);
|
|
uint8_t *pTmpBgr = (uint8_t *)subImg.img.au64VirAddr[0];
|
|
cvImageToTensor(zCrop.data, pTmpBgr, 3, trackState.exemplarSize, trackState.exemplarSize);
|
|
|
|
//tt1_ = steady_clock::now();
|
|
temple_net_.run(&subImg,outTnsr);
|
|
//tt2_ = steady_clock::now();
|
|
//time_span_ = duration_cast < duration < double >> (tt2_ - tt1_);
|
|
//printf("#Run temple_net_.run time is %f ms.\n",1000 * time_span_.count());
|
|
|
|
tem_output01 = outTnsr[0];//temple_net_.getOutputTensor(0); // tem_output01.data shape [1, 10240, 4, 4]
|
|
tem_output02 = outTnsr[1];//temple_net_.getOutputTensor(1);
|
|
// printf ("Reg_TempOut01:[%d %d %d %d]\n",tem_output01.n,tem_output01.channel,tem_output01.height,tem_output01.width);
|
|
// printf ("CLs_TempOut02:[%d %d %d %d]\n",tem_output02.n,tem_output02.channel,tem_output02.height,tem_output02.width);
|
|
// printf ("######### showFloatInputs(ClsKernelTensor_) ##############\n");
|
|
// showFloatInputs(tem_output02.data, 10*256*4*4,256*4*4);
|
|
// printf ("######### showFloatInputs(RegKernelTensor_) ##############\n");
|
|
// showFloatInputs(tem_output01.data, 20*256*4*4,256*4*4);
|
|
tkTime_.maxTime = 0;
|
|
tkTime_.minTime = 1e6;
|
|
tkTime_.updateTimes = 0;
|
|
tkTime_.expandTimes = 0;
|
|
tkTime_.expandRate = 0;
|
|
mnnDoubleBranch_->updateTwoKernels(tem_output02.data, tem_output01.data); // when switch new track-target, do not need load mnn models
|
|
printf("init ------------- index:%d\n", pSrcImg->indx);
|
|
|
|
if (UseTrace_){
|
|
if (traces_.size()!=0)
|
|
traces_.clear();
|
|
traces_.push_back(tagBox);
|
|
}
|
|
|
|
|
|
if (ReidInit_) {
|
|
nnie_reid_->Clear_Gallery_Featrue_Pools();
|
|
//nnie_reid_->Init_Gallery_Featrue_Pools(pSrcImg, tagBox);
|
|
}
|
|
if (UpdataTemp_){
|
|
if (tagTemplates_.size()!=0)
|
|
tagTemplates_.clear();
|
|
if (trkRes_.size()!=0)
|
|
trkRes_.clear();
|
|
}
|
|
|
|
trackerUpdateNpu(pSrcImg, searchBox, true);
|
|
initFlag_ = 1;
|
|
}
|
|
|
|
|
|
|
|
void DaSiamRPNtracker::trackerReInit(GuideImage *pSrcImg, gud_rect_t initBox, ai_detect_result_t *od_res)
|
|
{
|
|
mnnIdx_ = 0;
|
|
nnieIdx_ = -1;
|
|
fullImg = pSrcImg;
|
|
|
|
gud_rect_t tagBox = selectTagFromDetection(initBox,od_res);
|
|
|
|
int width = pSrcImg->img.au32Stride[0];
|
|
int height = pSrcImg->img.u32Height;
|
|
int bgrSize = width * height * 3;
|
|
char *pSrcBgr = (char*)HI_MPI_SYS_MmapCache(pSrcImg->img.au64PhyAddr[0],bgrSize);
|
|
cv::Mat img = cv::Mat(height,width, CV_8UC3,pSrcBgr);
|
|
|
|
trackState.tracking_score = 1.0;
|
|
trackState.label = tagBox.label;
|
|
trackState.targetBox.x = static_cast<float>(tagBox.cx);
|
|
trackState.targetBox.y = static_cast<float>(tagBox.cy);
|
|
trackState.targetBox.width = static_cast<float>(tagBox.w);
|
|
trackState.targetBox.height = static_cast<float>(tagBox.h);
|
|
printf ("ReLockBox:[%.2f %.2f %.2f %.2f ] label:[%d]\n",trackState.targetBox.x ,trackState.targetBox.y,trackState.targetBox.width,
|
|
trackState.targetBox.height,trackState.label);
|
|
cv::Rect2f targetBox = trackState.targetBox;
|
|
cv::Rect2f searchBox = trackState.targetBox;
|
|
|
|
cv::Mat anchors = generateAnchors();
|
|
trackState.anchors = anchors;
|
|
cv::Mat windows = generateHanningWindow();
|
|
|
|
trackState.windows = windows;
|
|
trackState.imgSize = img.size();
|
|
|
|
trackState.avgChans = mean(img);
|
|
float wc = targetBox.width + trackState.contextAmount * (targetBox.width + targetBox.height);
|
|
float hc = targetBox.height + trackState.contextAmount * (targetBox.width + targetBox.height);
|
|
float sz = (float)cvRound(sqrt(wc * hc));
|
|
cv::Mat zCrop = getSubwindow(img, targetBox, sz, trackState.avgChans, trackState.exemplarSize);
|
|
HI_MPI_SYS_Munmap(pSrcBgr,bgrSize);
|
|
|
|
uint8_t *pTmpBgr = (uint8_t *)subImg.img.au64VirAddr[0];
|
|
cvImageToTensor(zCrop.data, pTmpBgr, 3, trackState.exemplarSize, trackState.exemplarSize);
|
|
temple_net_.run(&subImg,outTnsr);
|
|
tem_output01 = outTnsr[0];//temple_net_.getOutputTensor(0); // tem_output01.data shape [1, 10240, 4, 4]
|
|
tem_output02 = outTnsr[1];//temple_net_.getOutputTensor(1);
|
|
|
|
tkTime_.maxTime = 0;
|
|
tkTime_.minTime = 1e6;
|
|
tkTime_.updateTimes = 0;
|
|
tkTime_.expandTimes = 0;
|
|
tkTime_.expandRate = 0;
|
|
mnnDoubleBranch_->updateTwoKernels(tem_output02.data, tem_output01.data); // when switch new track-target, do not need load mnn models
|
|
printf("init ------------- index:%d\n", pSrcImg->indx);
|
|
|
|
if (UseTrace_){
|
|
if (traces_.size()!=0)
|
|
traces_.clear();
|
|
traces_.push_back(tagBox);
|
|
}
|
|
|
|
|
|
if (ReidInit_) {
|
|
nnie_reid_->Clear_Gallery_Featrue_Pools();
|
|
//nnie_reid_->Init_Gallery_Featrue_Pools(pSrcImg, tagBox);
|
|
}
|
|
if (UpdataTemp_){
|
|
if (tagTemplates_.size()!=0)
|
|
tagTemplates_.clear();
|
|
if (trkRes_.size()!=0)
|
|
trkRes_.clear();
|
|
}
|
|
nnieIdx_++;
|
|
initFlag_ = 1;
|
|
}
|
|
|
|
|
|
void DaSiamRPNtracker::trackerReInitFloat(GuideImage *pSrcImg, gud_rect_f initBox, ai_detect_result_t *od_res)
|
|
{
|
|
mnnIdx_ = 0;
|
|
nnieIdx_ = -1;
|
|
fullImg = pSrcImg;
|
|
|
|
//gud_rect_f tagBox = selectTagFromDetectionF(initBox,od_res);
|
|
gud_rect_f tagBox = initBox;
|
|
int width = pSrcImg->img.au32Stride[0];
|
|
int height = pSrcImg->img.u32Height;
|
|
int bgrSize = width * height * 3;
|
|
char *pSrcBgr = (char*)HI_MPI_SYS_MmapCache(pSrcImg->img.au64PhyAddr[0],bgrSize);
|
|
cv::Mat img = cv::Mat(height,width, CV_8UC3,pSrcBgr);
|
|
|
|
trackState.tracking_score = 1.0;
|
|
trackState.label = tagBox.label;
|
|
trackState.targetBox.x = tagBox.cx;
|
|
trackState.targetBox.y = tagBox.cy;
|
|
trackState.targetBox.width = tagBox.w;
|
|
trackState.targetBox.height = tagBox.h;
|
|
printf ("ReLockBox:[%.2f %.2f %.2f %.2f ] label:[%d]\n",trackState.targetBox.x ,trackState.targetBox.y,trackState.targetBox.width,
|
|
trackState.targetBox.height,trackState.label);
|
|
cv::Rect2f targetBox = trackState.targetBox;
|
|
cv::Rect2f searchBox = trackState.targetBox;
|
|
|
|
//tt3_ = steady_clock::now();
|
|
cv::Mat anchors = generateAnchors();
|
|
trackState.anchors = anchors;
|
|
cv::Mat windows = generateHanningWindow();
|
|
//tt4_ = steady_clock::now();
|
|
//time_span_ = duration_cast < duration < double >> (tt4_ - tt3_);
|
|
//printf("#FRN:[%d] generateAnchors+generateHanningWindow time is (%f) ms.\n",pSrcImg->frmId,1000 * time_span_.count());
|
|
|
|
|
|
trackState.windows = windows;
|
|
trackState.imgSize = img.size();
|
|
//tt3_ = steady_clock::now();
|
|
trackState.avgChans = mean(img);
|
|
float wc = targetBox.width + trackState.contextAmount * (targetBox.width + targetBox.height);
|
|
float hc = targetBox.height + trackState.contextAmount * (targetBox.width + targetBox.height);
|
|
float sz = (float)cvRound(sqrt(wc * hc));
|
|
cv::Mat zCrop = getSubwindow(img, targetBox, sz, trackState.avgChans, trackState.exemplarSize);
|
|
HI_MPI_SYS_Munmap(pSrcBgr,bgrSize);
|
|
//tt4_ = steady_clock::now();
|
|
//time_span_ = duration_cast < duration < double >> (tt4_ - tt3_);
|
|
//printf("#FRN:[%d] getSubwindow time is (%f) ms.\n",pSrcImg->frmId,1000 * time_span_.count());
|
|
|
|
|
|
//tt3_ = steady_clock::now();
|
|
uint8_t *pTmpBgr = (uint8_t *)subImg.img.au64VirAddr[0];
|
|
cvImageToTensor(zCrop.data, pTmpBgr, 3, trackState.exemplarSize, trackState.exemplarSize);
|
|
temple_net_.run(&subImg,outTnsr);
|
|
tem_output01 = outTnsr[0];//temple_net_.getOutputTensor(0); // tem_output01.data shape [1, 10240, 4, 4]
|
|
tem_output02 = outTnsr[1];//temple_net_.getOutputTensor(1);
|
|
|
|
//tt4_ = steady_clock::now();
|
|
//time_span_ = duration_cast < duration < double >> (tt4_ - tt3_);
|
|
//printf("#FRN:[%d] temple_net_.run time is (%f) ms.\n",pSrcImg->frmId,1000 * time_span_.count());
|
|
|
|
tkTime_.maxTime = 0;
|
|
tkTime_.minTime = 1e6;
|
|
tkTime_.updateTimes = 0;
|
|
tkTime_.expandTimes = 0;
|
|
tkTime_.expandRate = 0;
|
|
|
|
//tt3_ = steady_clock::now();
|
|
mnnDoubleBranch_->updateTwoKernels(tem_output02.data, tem_output01.data);
|
|
//tt4_ = steady_clock::now();
|
|
//time_span_ = duration_cast < duration < double >> (tt4_ - tt3_);
|
|
//printf("#FRN:[%d] updateTwoKernels is (%f) ms.\n",pSrcImg->frmId,1000 * time_span_.count());
|
|
//printf("init ------------- index:%d\n", pSrcImg->indx);
|
|
|
|
nnieIdx_++;
|
|
initFlag_ = 1;
|
|
}
|
|
|
|
|
|
void DaSiamRPNtracker::trackerProcess(GuideImage *pSrcImg, gud_rect_t searchBox, ai_detect_result_t *od_res, AIT_OUTPUT *pAiOut)
|
|
{
|
|
if (searchBox.used){
|
|
outBox_.x = static_cast<float>(searchBox.cx);
|
|
outBox_.y = static_cast<float>(searchBox.cy);
|
|
outBox_.width = static_cast<float>(searchBox.w);
|
|
outBox_.height = static_cast<float>(searchBox.h);
|
|
updateReg_ = true;
|
|
}
|
|
else{
|
|
updateReg_ = false;
|
|
}
|
|
fullImg = pSrcImg;
|
|
|
|
//trackerUpdateNpu(pSrcImg, outBox_, updateReg_);
|
|
trackerUpdateMnn(pAiOut);
|
|
|
|
gud_rect_t trackRect, sOdRect;
|
|
trackRect.cx = pAiOut->nX;
|
|
trackRect.cy = pAiOut->nY;
|
|
trackRect.w = pAiOut->nObjW;
|
|
trackRect.h = pAiOut->nObjH;
|
|
int trkLabel = trackState.label;
|
|
|
|
|
|
if (ReidInit_) {
|
|
if (nnie_reid_->gallery_status_ == 0){
|
|
nnie_reid_->Init_Gallery_Featrue_Pools(pSrcImg, trackRect);
|
|
}else{
|
|
if (pSrcImg->frmId % ReidFrq_ == 0){
|
|
tt3_ = steady_clock::now();
|
|
nnie_reid_->Compare_QueryToGallery(pSrcImg, trackRect, &reid_result_);
|
|
tt4_ = steady_clock::now();
|
|
time_span_ = duration_cast < duration < double >> (tt4_ - tt3_);
|
|
printf("#FRN:[%d] nnie_reid_Compare_QueryToGallery time is (%f) ms.\n",pSrcImg->frmId,1000 * time_span_.count());
|
|
}
|
|
|
|
if (ReidCycleUp_){
|
|
if (trackState.tracking_score > UPDATE_TEMPLATE_ANCHOR_SCORE and pAiOut->nDetectNum > UPDATE_TEMPLATE_ANCHOR_THD){
|
|
int poolIdx = static_cast<int>((1.0 - ReidUpRatio_) * reid_config_.gallery_num);
|
|
nnie_reid_->Clear_Gallery_SetIndx_Featrue_Pools(poolIdx);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (UpdataTemp_){
|
|
gud_trk_res_t curTrk;
|
|
curTrk.target = trackRect;
|
|
curTrk.track_score = trackState.tracking_score;
|
|
curTrk.reid_sim = reid_result_.avg_sim;
|
|
curTrk.anchor_num = pAiOut->nDetectNum;
|
|
if (trkRes_.size() < TempNum_){
|
|
trkRes_.push_back(curTrk);
|
|
}
|
|
else{
|
|
trkRes_.pop_front();
|
|
trkRes_.push_back(curTrk);
|
|
if (pSrcImg->frmId % UpdateFrq_ == 0){
|
|
int maxIDX = 0;
|
|
float weighScore = 0;
|
|
float MaxScore = 0;
|
|
for (int i = 0; i < int(TempNum_); ++i) {
|
|
gud_trk_res_t tmpRes = trkRes_[i];
|
|
if (ReidInit_){
|
|
weighScore = tmpRes.reid_sim + (tmpRes.anchor_num / AIT_DETECTION_MAX_NUM)*1.0 + tmpRes.track_score;
|
|
}
|
|
else{
|
|
weighScore = (tmpRes.anchor_num / AIT_DETECTION_MAX_NUM)*1.0 + tmpRes.track_score;
|
|
}
|
|
if (weighScore > MaxScore){
|
|
MaxScore = weighScore;
|
|
maxIDX = i;
|
|
}
|
|
}
|
|
gud_trk_res_t selectTrk = trkRes_[maxIDX];
|
|
printf ("DUSE_TARGET_UPDATE0; use [%d] template to update! weightscore:[%.2f] (anchorNum:[%d],trackSocre:[%.2f] ReidSIm:[%.2f]\n",
|
|
maxIDX,MaxScore,selectTrk.anchor_num,selectTrk.track_score,selectTrk.reid_sim);
|
|
|
|
if (selectTrk.track_score > UPDATE_TEMPLATE_ANCHOR_SCORE and selectTrk.anchor_num > UPDATE_TEMPLATE_ANCHOR_THD)// and selectTrk.reid_sim > 0.9)
|
|
{
|
|
tt1_ = steady_clock::now();
|
|
cv::Mat zCrop = tagTemplates_[maxIDX];
|
|
uint8_t *pTmpBgr = (uint8_t *)subImg.img.au64VirAddr[0];
|
|
cvImageToTensor(zCrop.data, pTmpBgr, 3, trackState.exemplarSize, trackState.exemplarSize);
|
|
uptemp_net_.run(&subImg,outTnsr);
|
|
tem_output01 = outTnsr[0];
|
|
tem_output02 = outTnsr[1];
|
|
mnnDoubleBranch_->updateTwoKernels(tem_output02.data, tem_output01.data);
|
|
tt2_ = steady_clock::now();
|
|
time_span_ = duration_cast < duration < double >> (tt2_ - tt1_);
|
|
printf("#Run DUSE_TARGET_UPDATE1.run time is %f ms.\n",1000 * time_span_.count());
|
|
printf ("DUSE_TARGET_UPDATE1; use [%d] template to update! weightscore:[%.2f] (anchorNum:[%d],trackSocre:[%.2f] ReidSIm:[%.2f]\n",
|
|
maxIDX,MaxScore,selectTrk.anchor_num,selectTrk.track_score,selectTrk.reid_sim);
|
|
if (ReidInit_){
|
|
nnie_reid_->gallery_num_--;
|
|
nnie_reid_->Init_Gallery_Featrue_Pools(pSrcImg, trackRect);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (UseTrace_){
|
|
if (traces_.size() < TraceLen_){
|
|
traces_.push_back(trackRect);
|
|
}
|
|
else{
|
|
traces_.pop_front();
|
|
traces_.push_back(trackRect);
|
|
}
|
|
}
|
|
|
|
if (pAiOut->nDetectNum >= acNumThd_ and reid_result_.match_status ==1) {
|
|
pAiOut->nStatus = 3;
|
|
} else{
|
|
//pAiOut->nStatus = 1;
|
|
if (UseTrace_ and (int(traces_.size()) > int(TraceLen_/2))){
|
|
gud_rect_t boxPre = LinearSpeed_TracePredict(traces_);
|
|
pAiOut->nX = boxPre.cx;
|
|
pAiOut->nY = boxPre.cy;
|
|
pAiOut->nObjW = boxPre.w;
|
|
pAiOut->nObjH = boxPre.h;
|
|
/// 跟踪不稳定时候是否使用线性预测位置来更新搜索区？待测
|
|
// trackState.targetBox.x = boxPre.cx;
|
|
// trackState.targetBox.y = boxPre.cy;
|
|
// trackState.targetBox.width = boxPre.w;
|
|
// trackState.targetBox.height = boxPre.h;
|
|
}
|
|
}
|
|
#if (DEBUG_PRINT == 1)
|
|
printf ("nDetectNum:[%d]?[%d], reidStatus:[%d] ( max:[%.2f] min:[%.2f] avg:[%.2f])\n",pAiOut->nDetectNum,
|
|
acNumThd_,reid_result_.match_status,reid_result_.max_sim,reid_result_.min_sim,reid_result_.avg_sim);
|
|
#endif
|
|
|
|
}
|
|
|
|
|
|
void DaSiamRPNtracker::trackerProcessF(GuideImage *pSrcImg, gud_rect_f searchBox, ai_detect_result_t *od_res, AIT_OUTPUT *pAiOut)
|
|
{
|
|
if (searchBox.used){
|
|
outBox_.x = searchBox.cx;
|
|
outBox_.y = searchBox.cy;
|
|
#if (0)
|
|
outBox_.width = searchBox.w;
|
|
outBox_.height = searchBox.h;
|
|
#else
|
|
outBox_.width = trackState.targetBox.width;
|
|
outBox_.height = trackState.targetBox.height;
|
|
#endif
|
|
updateReg_ = true;
|
|
}
|
|
else{
|
|
updateReg_ = false;
|
|
}
|
|
fullImg = pSrcImg;
|
|
|
|
trackerUpdateMnn(pAiOut);
|
|
|
|
if (pAiOut->nDetectNum >= acNumThd_ and reid_result_.match_status ==1) {
|
|
pAiOut->nStatus = 3;
|
|
} else{
|
|
//pAiOut->nStatus = 1;
|
|
if (UseTrace_ and (int(traces_.size()) > int(TraceLen_/2))){
|
|
gud_rect_t boxPre = LinearSpeed_TracePredict(traces_);
|
|
pAiOut->nX = boxPre.cx;
|
|
pAiOut->nY = boxPre.cy;
|
|
pAiOut->nObjW = boxPre.w;
|
|
pAiOut->nObjH = boxPre.h;
|
|
/// 跟踪不稳定时候是否使用线性预测位置来更新搜索区？待测
|
|
// trackState.targetBox.x = boxPre.cx;
|
|
// trackState.targetBox.y = boxPre.cy;
|
|
// trackState.targetBox.width = boxPre.w;
|
|
// trackState.targetBox.height = boxPre.h;
|
|
}
|
|
}
|
|
}
|
|
|
|
void DaSiamRPNtracker::trackerUpdateNpu(GuideImage *pSrcImg, cv::Rect2f &searRegion, bool useOutReg)
|
|
{
|
|
cv::Rect2f targetBox;
|
|
if (useOutReg) {
|
|
targetBox = searRegion;
|
|
}
|
|
else {
|
|
targetBox = trackState.targetBox;
|
|
}
|
|
float wc = targetBox.height + trackState.contextAmount * (targetBox.width + targetBox.height);
|
|
float hc = targetBox.width + trackState.contextAmount * (targetBox.width + targetBox.height);
|
|
float sz = sqrt(wc * hc);
|
|
float scaleZ = trackState.exemplarSize / sz;
|
|
float searchSize = float((trackState.instanceSize - trackState.exemplarSize) / 2);
|
|
float pad = searchSize / scaleZ;
|
|
float sx = sz + 2 * pad;
|
|
|
|
int width = pSrcImg->img.au32Stride[0];
|
|
int height = pSrcImg->img.u32Height;
|
|
int bgrSize = width * height * 3;
|
|
char *pSrcBgr = (char*)HI_MPI_SYS_MmapCache(pSrcImg->img.au64PhyAddr[0],bgrSize);
|
|
cv::Mat img = cv::Mat(height,width, CV_8UC3,pSrcBgr);
|
|
trackState.searchBox = targetBox;
|
|
xCrop = getSubwindow(img, targetBox, (float)cvRound(sx), trackState.avgChans, trackState.instanceSize);
|
|
|
|
tt1_ = steady_clock::now();
|
|
uint8_t *pTmpBgr = (uint8_t *)subImg.img.au64VirAddr[0];
|
|
cvImageToTensor(xCrop.data, pTmpBgr, 3, trackState.instanceSize, trackState.instanceSize);
|
|
|
|
if (UpdataTemp_){
|
|
float wc = targetBox.width + trackState.contextAmount * (targetBox.width + targetBox.height);
|
|
float hc = targetBox.height + trackState.contextAmount * (targetBox.width + targetBox.height);
|
|
float sz = (float)cvRound(sqrt(wc * hc));
|
|
cv::Mat zCrop = getSubwindow(img, targetBox, sz, trackState.avgChans, trackState.exemplarSize);
|
|
if (tagTemplates_.size() < TempNum_){
|
|
tagTemplates_.push_back(zCrop);
|
|
}
|
|
else{
|
|
tagTemplates_.pop_front();
|
|
tagTemplates_.push_back(zCrop);
|
|
}
|
|
}
|
|
HI_MPI_SYS_Munmap(pSrcBgr,bgrSize);
|
|
|
|
detect_net_.run(&subImg,outTnsr);
|
|
det_output01 = outTnsr[0]; // regBranch
|
|
det_output02 = outTnsr[1]; // clsBranch
|
|
//mnnDoubleBranch_->updateTwoMaps(det_output02.data, det_output01.data);
|
|
memcpy(RegMap,det_output01.data, sizeof(float)*256*22*22);
|
|
memcpy(ClsMap,det_output02.data, sizeof(float)*256*22*22);
|
|
//usleep(DELAY_30MS);
|
|
//printf("UpdateNpu ------------- index:%d\n", pSrcImg->indx);
|
|
tt2_ = steady_clock::now();
|
|
time_span_ = duration_cast < duration < double >> (tt2_ - tt1_);
|
|
//printf("#Run detect_net_.run time is %f ms.\n",1000 * time_span_.count());
|
|
nnieIdx_++;
|
|
|
|
}
|
|
|
|
void DaSiamRPNtracker::trackerUpdateMnn(AIT_OUTPUT *pAiOut)
|
|
{
|
|
cv::Rect2f targetBox = trackState.targetBox;
|
|
if (updateReg_){
|
|
targetBox = outBox_;
|
|
//printf ("OUTBOX:[%.2f %.2f %.2f %.2f ]\n",outBox_.x,outBox_.y,outBox_.width,outBox_.height);
|
|
}
|
|
float wc = targetBox.height + trackState.contextAmount * (targetBox.width + targetBox.height);
|
|
float hc = targetBox.width + trackState.contextAmount * (targetBox.width + targetBox.height);
|
|
float sz = sqrt(wc * hc);
|
|
float scaleZ = trackState.exemplarSize / sz;
|
|
|
|
tt3_ = steady_clock::now();
|
|
//mnnDoubleBranch_->clsMnnForward(det_output01.data, ClsOutputData, true);
|
|
//mnnDoubleBranch_->regMnnForward(det_output02.data, RegOutputData, true);
|
|
mnnDoubleBranch_->clsMnnForward(ClsMap, ClsOutputData, false);
|
|
mnnDoubleBranch_->regMnnForward(RegMap, RegOutputData, false);
|
|
|
|
tt4_ = steady_clock::now();
|
|
time_span_ = duration_cast < duration < double >> (tt4_ - tt3_);
|
|
#if (DEBUG_PRINT == 1)
|
|
printf("#Run DoubleBranch_MnnForward Process (SumTotal)time is %f ms.\n",1000 * time_span_.count());
|
|
#endif
|
|
double ttSpand = 1000 * time_span_.count();
|
|
//showTrackerTimes(ttSpand);
|
|
|
|
//tt1_ = steady_clock::now();
|
|
cv::Mat delta(4, trackState.anchorNum * trackState.scoreSize * trackState.scoreSize, CV_32FC1, RegOutputData);
|
|
cv::Mat score(2, trackState.anchorNum * trackState.scoreSize * trackState.scoreSize, CV_32FC1, ClsOutputData);
|
|
|
|
score = score.reshape(0, { 2, trackState.anchorNum, trackState.scoreSize, trackState.scoreSize });
|
|
score = score.row(1);
|
|
score = score.reshape(0, { 5, 19, 19 });
|
|
delta = delta.reshape(0, { 4, trackState.anchorNum, trackState.scoreSize, trackState.scoreSize });
|
|
|
|
// post process
|
|
delta.row(0) = delta.row(0).mul(trackState.anchors.row(2)) + trackState.anchors.row(0);
|
|
delta.row(1) = delta.row(1).mul(trackState.anchors.row(3)) + trackState.anchors.row(1);
|
|
exp(delta.row(2), delta.row(2));
|
|
delta.row(2) = delta.row(2).mul(trackState.anchors.row(2));
|
|
exp(delta.row(3), delta.row(3));
|
|
delta.row(3) = delta.row(3).mul(trackState.anchors.row(3));
|
|
|
|
targetBox.width *= scaleZ;
|
|
targetBox.height *= scaleZ;
|
|
|
|
sc = sizeCal(delta.row(2), delta.row(3)) / sizeCal(targetBox.width, targetBox.height);
|
|
elementMax(sc);
|
|
|
|
rc = delta.row(2).mul(1 / delta.row(3));
|
|
rc = (targetBox.width / targetBox.height) / rc;
|
|
elementMax(rc);
|
|
|
|
exp(((rc.mul(sc) - 1.) * trackState.penaltyK * (-1.0)), penalty);
|
|
penalty = penalty.reshape(0, { trackState.anchorNum, trackState.scoreSize, trackState.scoreSize });
|
|
|
|
pscore = penalty.mul(score);
|
|
pscore = pscore * (1.0 - trackState.windowInfluence) + trackState.windows * trackState.windowInfluence;
|
|
|
|
int bestID[2] = { 0, 0 };
|
|
// Find the index of best score.
|
|
minMaxIdx(pscore.reshape(0, { trackState.anchorNum * trackState.scoreSize * trackState.scoreSize, 1 }), 0, 0, 0, bestID);
|
|
delta = delta.reshape(0, { 4, trackState.anchorNum * trackState.scoreSize * trackState.scoreSize });
|
|
penalty = penalty.reshape(0, { trackState.anchorNum * trackState.scoreSize * trackState.scoreSize, 1 });
|
|
score = score.reshape(0, { trackState.anchorNum * trackState.scoreSize * trackState.scoreSize, 1 });
|
|
|
|
int index[2] = { 0, bestID[0] };
|
|
resBox.x = delta.at<float>(index) / scaleZ;
|
|
index[0] = 1;
|
|
resBox.y = delta.at<float>(index) / scaleZ;
|
|
index[0] = 2;
|
|
resBox.width = delta.at<float>(index) / scaleZ;
|
|
index[0] = 3;
|
|
resBox.height = delta.at<float>(index) / scaleZ;
|
|
|
|
float lr = penalty.at<float>(bestID) * score.at<float>(bestID) * trackState.lr;
|
|
|
|
resBox.x = resBox.x + targetBox.x;
|
|
resBox.y = resBox.y + targetBox.y;
|
|
targetBox.width /= scaleZ;
|
|
targetBox.height /= scaleZ;
|
|
|
|
resBox.width = targetBox.width * (1 - lr) + resBox.width * lr;
|
|
resBox.height = targetBox.height * (1 - lr) + resBox.height * lr;
|
|
// resBox.x += resBox.width / 2;
|
|
// resBox.y += resBox.height / 2;
|
|
|
|
resBox.x = float(fmax(0., fmin(float(trackState.imgSize.width), resBox.x)));
|
|
resBox.y = float(fmax(0., fmin(float(trackState.imgSize.height), resBox.y)));
|
|
resBox.width = float(fmax(D_MIN_TARGET_SIZE*1.0, fmin(float(trackState.imgSize.width), resBox.width)));
|
|
resBox.height = float(fmax(D_MIN_TARGET_SIZE*1.0, fmin(float(trackState.imgSize.height), resBox.height)));
|
|
|
|
|
|
trackState.targetBox = resBox;
|
|
trackState.tracking_score = score.at<float>(bestID);
|
|
//printf ("ResBox:{%.2f %.2f %.2f %.2f } bestId:{%d %d}\n",resBox.x,resBox.y,resBox.width,resBox.height,bestID[0],bestID[1]);
|
|
|
|
cv::Mat sortID;
|
|
cv::Mat sortScore;
|
|
//得分排序
|
|
cv::sort(score, sortScore, cv::SORT_EVERY_COLUMN + cv::SORT_DESCENDING);
|
|
//序号排序
|
|
cv::sortIdx(score, sortID, cv::SORT_EVERY_COLUMN + cv::SORT_DESCENDING);
|
|
|
|
//获取得分靠前50个目标且score>0.85
|
|
int nCnt = 0;
|
|
int i = 0;
|
|
AIT_TARGET *tTargetArray = (AIT_TARGET *)pAiOut->nDetectArray;
|
|
|
|
cv::Rect2f targetSimilarBox = targetBox;
|
|
for (i = 0; i < sortScore.rows; i++) {
|
|
float fscore = sortScore.ptr<float>(i)[0];
|
|
int nIdex = sortID.ptr<int>(i)[0];
|
|
if (fscore > detThd_) {
|
|
cv::Rect2f *tTarget = &tTargetArray[i].tTarget;
|
|
int selectID[2] = { nIdex, 0 };
|
|
int index[2] = { 0, nIdex };
|
|
cv::Rect2f resBox = { 0, 0, 0, 0 };
|
|
resBox.x = delta.at<float>(index) / scaleZ;
|
|
index[0] = 1;
|
|
resBox.y = delta.at<float>(index) / scaleZ;
|
|
index[0] = 2;
|
|
resBox.width = delta.at<float>(index) / scaleZ;
|
|
index[0] = 3;
|
|
resBox.height = delta.at<float>(index) / scaleZ;
|
|
|
|
float lr = penalty.at<float>(selectID) * score.at<float>(selectID) * trackState.lr;
|
|
|
|
resBox.x = resBox.x + targetSimilarBox.x;
|
|
resBox.y = resBox.y + targetSimilarBox.y;
|
|
|
|
resBox.width = targetSimilarBox.width * (1 - lr) + resBox.width * lr;
|
|
resBox.height = targetSimilarBox.height * (1 - lr) + resBox.height * lr;
|
|
|
|
resBox.x = float(fmax(0., fmin(float(trackState.imgSize.width), resBox.x)));
|
|
resBox.y = float(fmax(0., fmin(float(trackState.imgSize.height), resBox.y)));
|
|
resBox.width = float(fmax(D_MIN_TARGET_SIZE*1.0, fmin(float(trackState.imgSize.width), resBox.width)));
|
|
resBox.height = float(fmax(D_MIN_TARGET_SIZE*1.0, fmin(float(trackState.imgSize.height), resBox.height)));
|
|
nCnt++;
|
|
|
|
if (nCnt == AIT_DETECTION_MAX_NUM) {
|
|
break;
|
|
}
|
|
memcpy(tTarget, &resBox, sizeof(cv::Rect2f));
|
|
tTargetArray[i].fScore = score.at<float>(selectID);
|
|
}
|
|
}
|
|
|
|
pAiOut->nDetectNum = nCnt;
|
|
|
|
if (pAiOut->nDetectNum >= acNumThd_) {
|
|
pAiOut->nStatus = 3;
|
|
}
|
|
// else{
|
|
// pAiOut->nStatus = 1;
|
|
// }
|
|
|
|
std::vector<cv::Rect> vec;
|
|
for (i = 0; i < pAiOut->nDetectNum; i++) {
|
|
cv::Rect ttTarget;
|
|
ttTarget.x = (int)pAiOut->nDetectArray[i].tTarget.x - pAiOut->nDetectArray[i].tTarget.width / 2;
|
|
ttTarget.y = (int)pAiOut->nDetectArray[i].tTarget.y - pAiOut->nDetectArray[i].tTarget.height / 2;
|
|
ttTarget.width = (int)pAiOut->nDetectArray[i].tTarget.width;
|
|
ttTarget.height = (int)pAiOut->nDetectArray[i].tTarget.height;
|
|
vec.push_back(ttTarget);
|
|
}
|
|
cv::groupRectangles(vec, 1, 0.5);
|
|
|
|
for (i = 0; i < (int)vec.size(); i++) {
|
|
pAiOut->nSimilarArray[i].x = (int)vec[i].x + (int)vec[i].width / 2;
|
|
pAiOut->nSimilarArray[i].y = (int)vec[i].y + (int)vec[i].height / 2;
|
|
pAiOut->nSimilarArray[i].width = (int)vec[i].width ;
|
|
pAiOut->nSimilarArray[i].height = (int)vec[i].height ;
|
|
}
|
|
pAiOut->nClusterNum = (int)vec.size();
|
|
pAiOut->fProb = trackState.tracking_score;
|
|
pAiOut->nX = trackState.targetBox.x;
|
|
pAiOut->nY = trackState.targetBox.y;
|
|
pAiOut->nObjW = trackState.targetBox.width;
|
|
pAiOut->nObjH = trackState.targetBox.height;
|
|
|
|
pAiOut->fX = trackState.targetBox.x;
|
|
pAiOut->fY = trackState.targetBox.y;
|
|
pAiOut->fObjW = trackState.targetBox.width;
|
|
pAiOut->fObjH = trackState.targetBox.height;
|
|
#if (DEBUG_PRINT == 1)
|
|
printf ("*** tracking_score:{%.2f} nDetectNum:{%d} nClusterNum:{%d}\n",
|
|
trackState.tracking_score,pAiOut->nDetectNum,pAiOut->nClusterNum);
|
|
#endif
|
|
mnnIdx_++;
|
|
// tt2_ = steady_clock::now();
|
|
// time_span_ = duration_cast < duration < double >> (tt2_ - tt1_);
|
|
// printf("#Run TotalPostProcess time is %f ms.\n",1000 * time_span_.count());
|
|
}
|
|
|
|
|
|
|
|
cv::Mat DaSiamRPNtracker::generateAnchors()
|
|
{
|
|
int totalStride = trackState.totalStride, scales = trackState.scale, scoreSize = trackState.scoreSize;
|
|
std::vector<float> ratios = trackState.ratios;
|
|
std::vector<cv::Rect2f> baseAnchors;
|
|
int anchorNum = int(ratios.size());
|
|
int size = totalStride * totalStride;
|
|
float ori = -(float(scoreSize / 2)) * float(totalStride);
|
|
|
|
for (auto i = 0; i < anchorNum; i++)
|
|
{
|
|
int ws = int(sqrt(size / ratios[i]));
|
|
int hs = int(ws * ratios[i]);
|
|
|
|
float wws = float(ws) * scales;
|
|
float hhs = float(hs) * scales;
|
|
cv::Rect2f anchor = { 0, 0, wws, hhs };
|
|
baseAnchors.push_back(anchor);
|
|
}
|
|
|
|
int anchorIndex[4] = { 0, 0, 0, 0 };
|
|
const int sizes[4] = { 4, (int)ratios.size(), scoreSize, scoreSize };
|
|
|
|
cv::Mat anchors(4, sizes, CV_32F);
|
|
|
|
for (auto i = 0; i < scoreSize; i++)
|
|
{
|
|
for (auto j = 0; j < scoreSize; j++)
|
|
{
|
|
for (auto k = 0; k < anchorNum; k++)
|
|
{
|
|
anchorIndex[0] = 1, anchorIndex[1] = k, anchorIndex[2] = i, anchorIndex[3] = j;
|
|
anchors.at<float>(anchorIndex) = ori + totalStride * i;
|
|
|
|
anchorIndex[0] = 0;
|
|
anchors.at<float>(anchorIndex) = ori + totalStride * j;
|
|
|
|
anchorIndex[0] = 2;
|
|
anchors.at<float>(anchorIndex) = baseAnchors[k].width;
|
|
|
|
anchorIndex[0] = 3;
|
|
anchors.at<float>(anchorIndex) = baseAnchors[k].height;
|
|
}
|
|
}
|
|
}
|
|
return anchors;
|
|
}
|
|
|
|
cv::Mat DaSiamRPNtracker::generateHanningWindow()
|
|
{
|
|
cv::Mat baseWindows, HanningWindows;
|
|
cv::createHanningWindow(baseWindows, cv::Size(trackState.scoreSize, trackState.scoreSize), CV_32F);
|
|
baseWindows = baseWindows.reshape(0, { 1, trackState.scoreSize, trackState.scoreSize });
|
|
HanningWindows = baseWindows.clone();
|
|
for (int i = 1; i < trackState.anchorNum; i++)
|
|
{
|
|
HanningWindows.push_back(baseWindows);
|
|
}
|
|
return HanningWindows;
|
|
}
|
|
|
|
cv::Mat DaSiamRPNtracker::getSubwindow(cv::Mat& img, const cv::Rect2f& targetBox, float originalSize,
|
|
cv::Scalar avgChans, const int& out_sz)
|
|
{
|
|
struct timeval tStart0, tStart;
|
|
|
|
cv::Mat zCrop, dst;
|
|
|
|
cv::Size imgSize = img.size();
|
|
float c = (originalSize + 1) / 2;
|
|
float xMin = (float)cvRound(targetBox.x - c);
|
|
float xMax = xMin + originalSize - 1;
|
|
float yMin = (float)cvRound(targetBox.y - c);
|
|
float yMax = yMin + originalSize - 1;
|
|
|
|
int leftPad = (int)(fmax(0., -xMin));
|
|
int topPad = (int)(fmax(0., -yMin));
|
|
int rightPad = (int)(fmax(0., xMax - imgSize.width + 1));
|
|
int bottomPad = (int)(fmax(0., yMax - imgSize.height + 1));
|
|
|
|
xMin = xMin + leftPad;
|
|
xMax = xMax + leftPad;
|
|
yMax = yMax + topPad;
|
|
yMin = yMin + topPad;
|
|
|
|
if (topPad == 0 && bottomPad == 0 && leftPad == 0 && rightPad == 0)
|
|
{
|
|
img(cv::Rect(int(xMin), int(yMin), int(xMax - xMin + 1), int(yMax - yMin + 1))).copyTo(zCrop);
|
|
|
|
}
|
|
else
|
|
{
|
|
cv::copyMakeBorder(img, dst, topPad, bottomPad, leftPad, rightPad, cv::BORDER_CONSTANT, avgChans);
|
|
dst(cv::Rect(int(xMin), int(yMin), int(xMax - xMin + 1), int(yMax - yMin + 1))).copyTo(zCrop);
|
|
}
|
|
|
|
trackState.searchBox.x = int(xMin);
|
|
trackState.searchBox.y = int(yMin);
|
|
trackState.searchBox.width = int(xMax - xMin + 1);
|
|
trackState.searchBox.height = int(yMax - yMin + 1);
|
|
|
|
// draw_rect(img,cv::Rect(int(xMin), int(yMin), int(xMax - xMin + 1), int(yMax - yMin + 1)),2);
|
|
// std::string regionImg = "/nfsroot/001_3559/results/sear_" + std::to_string(trackState.FRN) + ".jpg";
|
|
// cv::imwrite(regionImg, img);
|
|
|
|
cv::Size model_sz = {out_sz, out_sz};
|
|
cv::Mat resizedPatch(out_sz, out_sz, CV_8UC3);
|
|
cv::resize(zCrop, resizedPatch, model_sz, 0, 0, cv::INTER_LINEAR);
|
|
|
|
return resizedPatch;
|
|
}
|
|
|
|
|
|
void DaSiamRPNtracker::showFloatInputs(float* input, int size, int diff)
|
|
{
|
|
printf("******* Inputs total size:{%d} **********\n",size);
|
|
int lenN = 1;
|
|
for (int i = 0; i < size; ++i)
|
|
{
|
|
printf ("%.3f ",input[i]);
|
|
if (i%(diff) == 0 and i!=0){
|
|
printf("\n#[%d]#\n",lenN);
|
|
lenN++;
|
|
}
|
|
}
|
|
printf("******* Inputs showDataEnd: **********\n");
|
|
}
|
|
|
|
void DaSiamRPNtracker::showTrackerTimes(double timeEsp)
|
|
{
|
|
tkTime_.updateTimes++;
|
|
if (timeEsp > tkTime_.timeThd)
|
|
{
|
|
tkTime_.expandTimes++;
|
|
}
|
|
if (timeEsp > tkTime_.maxTime)
|
|
tkTime_.maxTime = timeEsp;
|
|
if (timeEsp < tkTime_.minTime)
|
|
tkTime_.minTime = timeEsp;
|
|
if (tkTime_.updateTimes!=0)
|
|
tkTime_.expandRate = (tkTime_.expandTimes*1.0)/tkTime_.updateTimes;
|
|
//printf ("*** CurrentUpdateTime:{%.2f}(ms) maxTime:{%.2f} minTime:{%.2f} expand [%.2f](ms)->[%d]/[%d] = rate:[%.5f]\n",
|
|
// timeEsp,tkTime_.maxTime,tkTime_.minTime,tkTime_.timeThd,tkTime_.expandTimes,tkTime_.updateTimes,tkTime_.expandRate);
|
|
}
|
|
|
|
void DaSiamRPNtracker::draw_rect(cv::Mat frame, cv::Rect targetBox, int type)
|
|
{
|
|
cv::Point pt1, pt2;
|
|
int tw, th;
|
|
{
|
|
pt1.x = int(targetBox.x);
|
|
pt1.y = int(targetBox.y);
|
|
pt2.x = int(targetBox.x + targetBox.width);
|
|
pt2.y = int(targetBox.y + targetBox.height);
|
|
tw = int(targetBox.width);
|
|
th = int(targetBox.height);
|
|
}
|
|
|
|
if (tw * th != 0){
|
|
if (type == 0)
|
|
cv::rectangle(frame, pt1, pt2, cv::Scalar(255, 0, 0), 3); //blue
|
|
if (type == 1)
|
|
cv::rectangle(frame, pt1, pt2, cv::Scalar(0, 255, 0), 3); // green
|
|
if (type == 2)
|
|
cv::rectangle(frame, pt1, pt2, cv::Scalar(0, 0, 255), 3); //red
|
|
if (type == 3)
|
|
cv::rectangle(frame, pt1, pt2, cv::Scalar(0, 255, 255), 3); //yellow
|
|
}
|
|
}
|
|
|
|
|
|
gud_rect_t DaSiamRPNtracker::LinearSpeed_TracePredict(std::deque<gud_rect_t> &trace)
|
|
{
|
|
int diff_x = 0;
|
|
int diff_y = 0;
|
|
int dirct_x = 0;
|
|
int dirct_y = 0;
|
|
|
|
int t_num = trace.size();
|
|
if (t_num < 2)
|
|
return trace.at(0);
|
|
|
|
gud_rect_t start_rect = trace.at(0);
|
|
gud_rect_t end_rect = trace.at(t_num-1);
|
|
|
|
int start_cx = start_rect.cx;
|
|
int start_cy = start_rect.cy;
|
|
int end_cx = end_rect.cx;
|
|
int end_cy = end_rect.cy;
|
|
|
|
for (int i = 1; i < t_num; ++i) {
|
|
gud_rect_t tag_old = trace.at(i-1);
|
|
gud_rect_t tag_new = trace.at(i);
|
|
int ocx = tag_old.cx;
|
|
int ocy = tag_old.cy;
|
|
int ncx = tag_new.cx;
|
|
int ncy = tag_new.cy;
|
|
|
|
diff_x += abs(ncx - ocx);
|
|
diff_y += abs(ncy - ocy);
|
|
|
|
if (abs(ncx - ocx)!=0){
|
|
if ((ncx - ocx) > 0){
|
|
dirct_x++;
|
|
}else{
|
|
dirct_x--;
|
|
}
|
|
}
|
|
if (abs(ncy - ocy)!=0){
|
|
if ((ncy - ocy) > 0){
|
|
dirct_y++;
|
|
}else{
|
|
dirct_y--;
|
|
}
|
|
}
|
|
}
|
|
if (dirct_x!=0){
|
|
if (dirct_x > 0)
|
|
dirct_x = 1;
|
|
else
|
|
dirct_x = -1;
|
|
}
|
|
if (dirct_y!=0){
|
|
if (dirct_y > 0)
|
|
dirct_y = 1;
|
|
else
|
|
dirct_y = -1;
|
|
}
|
|
|
|
float vx = dirct_x * diff_x * 1.0 / (t_num - 1);
|
|
float vy = dirct_y * diff_y * 1.0 / (t_num - 1);
|
|
|
|
#if (DEBUG_PRINT == 1)
|
|
gud_rect_t pre_rect;
|
|
pre_rect.cx = end_rect.cx + (int)(ceil(vx));
|
|
pre_rect.cy = end_rect.cy + (int)(ceil(vy));
|
|
pre_rect.w = end_rect.w;
|
|
pre_rect.h = end_rect.h;
|
|
printf ("\n#### linearspeed_by_trace X:%f Y:%f preCX:%d preCY:%d\n",vx,vy,pre_rect.cx,pre_rect.cy);
|
|
#endif
|
|
return end_rect;
|
|
}
|
|
|
|
int DaSiamRPNtracker::calRectIOU(gud_rect_t re1, gud_rect_t re2)
|
|
{
|
|
int iou = 0;
|
|
int re1_ul_x = static_cast<int>(re1.cx - re1.w/2);
|
|
int re1_ul_y = static_cast<int>(re1.cy - re1.h/2);
|
|
int re1_lr_x = re1_ul_x + re1.w;
|
|
int re1_lr_y = re1_ul_y + re1.h;
|
|
|
|
int re2_ul_x = static_cast<int>(re2.cx - re2.w/2);
|
|
int re2_ul_y = static_cast<int>(re2.cy - re2.h/2);
|
|
int re2_lr_x = re2_ul_x + re2.w;
|
|
int re2_lr_y = re2_ul_y + re2.h;
|
|
|
|
int max_left = MAX(re1_ul_x, re2_ul_x);
|
|
int min_right = MIN(re1_lr_x, re2_lr_x);
|
|
int max_up = MAX(re1_ul_y, re2_ul_y);
|
|
int min_down = MIN(re1_lr_y, re2_lr_y);
|
|
|
|
if (min_right - max_left < 0 || min_down - max_up < 0)
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
else
|
|
{
|
|
int overflow_area = (min_right - max_left) * (min_down - max_up);
|
|
int re1_area = (re1_lr_x - re1_ul_x) * (re1_lr_y - re1_ul_y);
|
|
int re2_area = (re2_lr_x - re2_ul_x) * (re2_lr_y - re2_ul_y);
|
|
if (overflow_area!=0 and re1_area!=0){
|
|
iou = static_cast<int>(overflow_area * 100 / (re1_area + re2_area - overflow_area));
|
|
return iou;
|
|
}
|
|
else
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
void DaSiamRPNtracker::reid_jsonReader(const char *reid_path, gud_reid_config_t *reid_config)
|
|
{
|
|
/******************* OT_Json File Read *******************/
|
|
|
|
FILE *fp;
|
|
cJSON *json;
|
|
fp = fopen(reid_path, "rb");
|
|
fseek(fp, 0, SEEK_END);
|
|
long len = ftell(fp);
|
|
fseek(fp, 0, SEEK_SET);
|
|
char *content = (char *) malloc(sizeof(char) * (len + 1));
|
|
//printf("***** 0jsonRead okokokok*****\n");
|
|
fread(content, 1, len, fp);
|
|
fclose(fp);
|
|
|
|
/******************* Reid_Json File Read *******************/
|
|
json = cJSON_Parse(content);
|
|
|
|
if (json == NULL) {
|
|
cout << "cJSON is readed failed!\n";
|
|
exit(1);
|
|
}
|
|
|
|
cJSON *json_input_n ,*json_input_c, *json_input_w, *json_input_h,*json_cycleup;
|
|
cJSON *json_output_n ,*json_output_c, *json_output_w, *json_output_h,*json_upratio;
|
|
cJSON *json_sim_thd, *json_feat_len, *json_gallery_num, *json_model;
|
|
|
|
json_input_n = cJSON_GetObjectItem(json, "input_n");
|
|
json_input_c = cJSON_GetObjectItem(json, "input_c");
|
|
json_input_w = cJSON_GetObjectItem(json, "input_w");
|
|
json_input_h = cJSON_GetObjectItem(json, "input_h");
|
|
json_output_n = cJSON_GetObjectItem(json, "output_n");
|
|
json_output_c = cJSON_GetObjectItem(json, "output_c");
|
|
json_output_w = cJSON_GetObjectItem(json, "output_w");
|
|
json_output_h = cJSON_GetObjectItem(json, "output_h");
|
|
|
|
json_cycleup = cJSON_GetObjectItem(json, "cycle_update");
|
|
json_upratio = cJSON_GetObjectItem(json, "update_ratio");
|
|
|
|
json_sim_thd = cJSON_GetObjectItem(json, "sim_thd");
|
|
json_feat_len = cJSON_GetObjectItem(json, "feat_len");
|
|
json_gallery_num = cJSON_GetObjectItem(json, "gallery_num");
|
|
json_model = cJSON_GetObjectItem(json, "model");
|
|
|
|
reid_config->input_n = json_input_n->valueint;
|
|
reid_config->input_c = json_input_c->valueint;
|
|
reid_config->input_w = json_input_w->valueint;
|
|
reid_config->input_h = json_input_h->valueint;
|
|
|
|
reid_config->output_n = json_output_n->valueint;
|
|
reid_config->output_c = json_output_c->valueint;
|
|
reid_config->output_w = json_output_w->valueint;
|
|
reid_config->output_h = json_output_h->valueint;
|
|
|
|
reid_config->cycle_update = json_cycleup->valueint;
|
|
reid_config->update_ratio = json_upratio->valuedouble;
|
|
|
|
reid_config->sim_thd = json_sim_thd->valuedouble;
|
|
reid_config->feat_len = json_feat_len->valueint;
|
|
reid_config->gallery_num = json_gallery_num->valueint;
|
|
reid_config->model = json_model->valuestring;
|
|
#if 1
|
|
printf("*****GUD_ALG_REID jsonReader is started*****\n");
|
|
printf ("net_w:%d net_h:%d feat_len:%d g_num:%d sim_thd:%f\n",reid_config->input_w,reid_config->input_h,reid_config->feat_len,
|
|
reid_config->gallery_num, reid_config->sim_thd);
|
|
printf("cycle_update:[%d] update_ratio[%.2f] model:%s\n",reid_config->cycle_update,reid_config->update_ratio,reid_config->model);
|
|
printf("*****GUD_ALG_REID jsonReader is END*****\n");
|
|
#endif
|
|
}
|
|
|