#include "track.h"
|
Track::Track()
|
{
|
|
}
|
Track::~Track()
|
{
|
|
}
|
Track::Track(KAL_MEAN& mean, KAL_COVA& covariance, uint64_t track_id, int n_init, int max_age, const FEATURE& feature,char* img_time)
|
{
|
this->mean = mean;
|
this->covariance = covariance;
|
this->track_id = track_id;
|
this->hits = 1;
|
this->age = 1;
|
this->time_since_update = 0;
|
this->state = TrackState::Tentative;
|
features = FEATURESS(1, 128);
|
features.row(0) = feature;//features.rows() must = 0;
|
|
this->_n_init = n_init;
|
this->_max_age = max_age;
|
this->init_t = true;
|
this->isCurrent = true;
|
|
//初始化徘徊参数
|
this->last_time = 0;
|
// gettimeofday(&this->last_s_time,0); // 徘徊计时开启
|
this->last_s_time = char_2_unix(img_time);
|
std::cout<< "==track img time=%s：" << img_time << std::endl;
|
|
this->isWander = false;
|
|
//初始化跌倒的参数
|
this->rate = 100;
|
this->isFall = false;
|
this->first_Fall = true;
|
this->rateScale = 0;
|
this->minRate = 0;
|
this->center_point.x = 0;
|
this->center_point.y = 0;
|
this->fall_total_time = 0;
|
this->hisMinRate = 100;
|
this->human_h = 0;
|
this->human_w = 0;
|
|
|
//初始化奔跑的参数
|
this->last_point.x = -1;
|
this->last_point.y = -1;
|
this->is_runing_p = true;
|
this->is_runing_t = true;
|
this->isRuning = false;
|
this->sum_velocity = 0;
|
this->confidence = 0;
|
this->run_rate = 0;
|
this->last_rate = 0;
|
this->rate_area = 0.0;
|
|
this->is_hat = false;
|
this->hatScore = 0;
|
this->headScore = 0;
|
this->helmetScore = 0;
|
|
this->is_mask=false;
|
this->maskScore = 0;
|
|
this->is_smoke=false;
|
this->smokeScore = 0;
|
|
// 添加当前的xy坐标
|
// memset(&this->start,0,sizeof(struct this->timeval));
|
// memset(&this->stop,0,sizeof(struct this->timeval));
|
// memset(&this->diff,0,sizeof(struct this->timeval));
|
}
|
|
void Track::predit(KalmFilter *kf)
|
{
|
/*Propagate the state distribution to the current time step using a
|
Kalman filter prediction step.
|
|
Parameters
|
----------
|
kf : kalman_filter.KalmFilter
|
The Kalman filter.
|
*/
|
|
kf->predict(this->mean, this->covariance);
|
this->age += 1;
|
this->time_since_update += 1;
|
}
|
|
void Track::update(KalmFilter * const kf, const DETECTION_ROW& detection, char* img_time)
|
{
|
// 判断消失三秒后删除该轨迹
|
if(this->init_t){
|
// gettimeofday(&this->start,0);
|
this->start = char_2_unix(img_time);
|
std::cout << "tracks start:: " << this->start << std::endl;
|
this->init_t = false;
|
}
|
|
// gettimeofday(&this->stop,0);
|
this->stop = char_2_unix(img_time);
|
// this->time_substract(&this->diff, &this->start, &this->stop);
|
this->diff = this->stop - this->start;
|
std::cout << "tracks curr time:: " << this->stop << " tracks curr diff:: "<<(double)this->diff <<std::endl;
|
|
// this->sum_timer = (double)((diff.tv_sec*1000000 + diff.tv_usec)/1000000);
|
this->sum_timer = (double)(this->diff)/1000;
|
printf("----Track update time------%f\n\n\n\n\n\n", this->sum_timer);
|
// std::cout<< this->sum_timer << "::" << MAX_OUT_TIME<<std::endl;
|
|
if(this->sum_timer > MAX_OUT_TIME)
|
{
|
this->state = TrackState::Deleted;
|
this->_max_age = 0;
|
this->last_time = 0;
|
|
// gettimeofday(&this->last_s_time,0); // 重新徘徊计时（这是离开不到三秒都算是徘徊）
|
this->last_s_time = char_2_unix(img_time);
|
std::cout<< "wander start "<< this->last_s_time << " : " << img_time <<std::endl;
|
// std::cout<< "--------------------------delete -----------------------";
|
}
|
// gettimeofday(&this->start,0);
|
this->start = char_2_unix(img_time);
|
|
// 计算徘徊的时长
|
// gettimeofday(&this->last_e_time,0);
|
this->last_e_time = char_2_unix(img_time);
|
std::cout<< "wander end "<< this->last_e_time << " : " << img_time <<std::endl;
|
// this->time_substract(&this->last_diff_time,&this->last_s_time,&this->last_e_time);
|
this->last_diff_time = this->last_e_time - this->last_s_time;
|
// this->last_time = (int)(this->last_diff_time.tv_sec+this->last_diff_time.tv_usec/1000000);
|
this->last_time = (int)(this->last_diff_time/1000);
|
std::cout<< "wander time "<< this->last_time << "::" << this->last_diff_time << "last_s_time" << this->last_s_time << " e: "<< this->last_e_time <<std::endl;
|
|
// 奔跑记录单帧时间
|
if(this->is_runing_t)
|
{
|
// gettimeofday(&this->single_s_time, 0);
|
this->single_s_time = char_2_unix(img_time);
|
|
this->is_runing_t = false;
|
}else{
|
// this->time_substract(&this->single_diff_time,&this->single_s_time,&this->last_e_time);
|
this->single_diff_time = this->last_e_time - this->single_s_time;
|
// this->single_time = (int)(this->single_diff_time.tv_sec*1000 + this->single_diff_time.tv_usec/1000);
|
this->single_time = (int)(this->single_diff_time);
|
// gettimeofday(&this->single_s_time, 0);
|
this->single_s_time = char_2_unix(img_time);
|
}
|
|
|
KAL_DATA pa = kf->update(this->mean, this->covariance, detection.to_xyah());
|
this->mean = pa.first;
|
this->covariance = pa.second;
|
|
// 添加当前的xy坐标
|
this->xywh = detection.tlwh;
|
this->confidence = (this->confidence*4+detection.confidence)/5;
|
|
if(this->is_hat)
|
{
|
this->is_hat = detection.is_hat;
|
this->hatScore =(int)((this->hatScore*9 + detection.hatScore)/10);
|
this->helmetScore =(int)((this->helmetScore*9 + detection.helmetScore)/10);
|
this->headScore =(int)((this->headScore*9 + detection.headScore)/10);
|
}else
|
{
|
this->is_hat = detection.is_hat;
|
this->hatScore = detection.hatScore;
|
this->helmetScore = detection.helmetScore;
|
this->headScore = detection.headScore;
|
}
|
|
|
this->is_mask = detection.is_mask;
|
this->maskScore = detection.maskScore;
|
this->is_smoke = detection.is_smoke;
|
this->smokeScore = detection.smokeScore;
|
/***yolo 直接测跌倒代码更新 s*/
|
if(detection.obj_id==0)
|
{
|
this->fall_total_time = 0 ;
|
this->first_Fall = true;
|
this->rateScale = 0;
|
this->isRuning = false;
|
}else if(detection.obj_id==1)
|
{
|
/**-- start计算累积跌倒时间*/
|
if(this->first_Fall) //是否开始计算跌倒时间
|
{
|
this->first_Fall = false;
|
// gettimeofday(&this->fall_s_time,0);
|
this->fall_s_time = char_2_unix(img_time);
|
}
|
// gettimeofday(&this->fall_e_time,0);
|
this->fall_e_time = char_2_unix(img_time);
|
// this->time_substract(&this->fall_diff_time,&this->fall_s_time,&this->fall_e_time); //距离最小比例的时间
|
this->fall_diff_time = this->fall_e_time - this->fall_s_time;
|
// this->fall_total_time = (int)(this->fall_diff_time.tv_sec + this->fall_diff_time.tv_usec/1000000);
|
this->fall_total_time = (int)(this->fall_diff_time/1000);
|
|
/** -- end计算累积跌倒时间*/
|
|
this->rateScale = detection.confidence;
|
this->isRuning = false;
|
|
}else if(detection.obj_id==2)
|
{
|
this->isRuning = true;
|
}
|
/***yolo 直接测跌倒代码更新 end*/
|
|
featuresAppendOne(detection.feature);
|
// this->features.row(features.rows()) = detection.feature;
|
this->hits += 1;
|
this->time_since_update = 0;
|
if(this->state == TrackState::Tentative && this->hits >= this->_n_init) {
|
this->state = TrackState::Confirmed;
|
}
|
this->isCurrent = true;
|
}
|
|
bool Track::mark_missed(char* img_time)
|
{
|
if(this->init_t)
|
{
|
// gettimeofday(&this->start,0);
|
this->start = char_2_unix(img_time);
|
this->init_t = false;
|
}
|
// gettimeofday(&this->stop,0);
|
this->stop = char_2_unix(img_time);
|
|
// this->time_substract(&this->diff,&this->start,&this->stop);
|
this->diff = this->stop - this->start;
|
|
// this->sum_timer = (double)((diff.tv_sec*1000000 + diff.tv_usec)/1000000);
|
this->sum_timer = (double)(this->diff)/1000;
|
|
if(this->state == TrackState::Tentative) {
|
this->state = TrackState::Deleted;
|
return true;
|
} else if(this->time_since_update > this->_max_age) {
|
this->state = TrackState::Deleted;
|
return true;
|
} else if(this->sum_timer > MAX_OUT_TIME)
|
{
|
this->state = TrackState::Deleted;
|
this->_max_age = 0;
|
return true;
|
}
|
this->isCurrent = false;
|
return false;
|
}
|
|
bool Track::is_confirmed()
|
{
|
return this->state == TrackState::Confirmed;
|
}
|
|
bool Track::is_deleted()
|
{
|
return this->state == TrackState::Deleted;
|
}
|
|
bool Track::is_tentative()
|
{
|
return this->state == TrackState::Tentative;
|
}
|
|
DETECTBOX Track::to_tlwh()
|
{
|
DETECTBOX ret = mean.leftCols(4);
|
ret(2) *= ret(3);
|
ret.leftCols(2) -= (ret.rightCols(2)/2);
|
return ret;
|
}
|
|
DETECTBOX Track::to_xywh()
|
{
|
return this->xywh;
|
}
|
|
void Track::featuresAppendOne(const FEATURE &f)
|
{
|
int size = this->features.rows();
|
FEATURESS newfeatures = FEATURESS(size+1, 128);
|
newfeatures.block(0, 0, size, 128) = this->features;
|
newfeatures.row(size) = f;
|
features = newfeatures;
|
// printf("--features size::---%d---%d--\n",size,features.rows());
|
}
|
|
int Track::time_substract(struct timeval *result, struct timeval *begin, struct timeval *end)
|
{
|
if(begin->tv_sec > end->tv_sec) return -1;
|
if((begin->tv_sec == end->tv_sec) && (begin->tv_usec > end->tv_usec)) return -2;
|
result->tv_sec = (end->tv_sec - begin->tv_sec);
|
result->tv_usec = (end->tv_usec - begin->tv_usec);
|
|
if(result->tv_usec < 0)
|
{
|
result->tv_sec--;
|
result->tv_usec += 1000000;
|
}
|
return 0;
|
|
}
|
