//Update the position of the person when info from the detection is available
void Person::update(tf::Point pos, Time time, bool img_detection)
{
double dt = time.sec - last_update_time_.sec;
kf_->predict(dt);
kf_->update(pos.x(), pos.y(), time);
last_update_time_ = time;
life_time_ = (img_detection) ? 5 : 3;
return;
}
Person::Person(tf::Point pos, Time time) : id_(++next_id), life_time_(3)
{
kf_ = new PersonKF;
kf_->init(pos.x(), pos.y(), 0.0, 0.0, time);
last_update_time_ = time;
return;
}
void Person::correct(tf::Point pos, Time time)
{
updateTime(time,last_correct_time_);
last_correct_time_ = time;
last_predict_time_ = time;
//Prediction is run before correction
kf_->predict();
//This prevents the filter from getting stuck in a state when no corrections are executed
kf_->statePre.copyTo(kf_->statePost);
kf_->errorCovPre.copyTo(kf_->errorCovPost);
setData(kf_->correct((Mat_<float>(2,1) << pos.x(), pos.y())), time);
return;
}