Exemplo n.º 1
0
//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;
}
Exemplo n.º 2
0
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;
}
Exemplo n.º 3
0
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;	
}