/*!
Adjusts the stochastic model state on the basis of the given
measurement of the model state.
m is usually equal to get_last_feature().
@see Perform_Kalman_Filtering()
*/
void Trace::Correct_Kalman_Filter(BlobPtr m)
{
CvMat* z = cvCreateMat(1,1,CV_32FC1);
//Centroid
cvmSet(z,0,0,m->centroid().x);
cvKalmanCorrect(m_filtFeature.centroid.x, z);
cvmSet(z,0,0,m->centroid().y);
cvKalmanCorrect(m_filtFeature.centroid.y, z);
//Size
cvmSet(z,0,0,m->size());
cvKalmanCorrect(m_filtFeature.size, z);
//Width
cvmSet(z,0,0,m->width());
cvKalmanCorrect(m_filtFeature.width, z);
//Height
cvmSet(z,0,0,m->height());
cvKalmanCorrect(m_filtFeature.height, z);
//Color histogram
for (int i=0; i<Blob::HISTO_SIZE; i++)
{
cvmSet(z,0,0,m->m_color_histogram[i]);
cvKalmanCorrect(m_filtFeature.color_histogram[i], z);
}
cvReleaseMat(&z);
}
void Trace::add_path_node(BlobPtr feature)
{
if (!m_nodes.empty())
m_displacement += (m_nodes.back()->centroid() - feature->centroid()).Norm();
/*BlobPtr measured_point(new Blob(*feature));
m_nodes.push_back(measured_point);*/
m_nodes.push_back(feature);
#ifdef SMOOTH_TRACES_ONLINE
if (get_length() == KALMAN_START_LENGTH)
Initialize_Kalman_Filter();
#else
if (m_x_range.min > feature->centroid().x)
m_x_range.min = feature->centroid().x;
if (m_x_range.max < feature->centroid().x)
m_x_range.max = feature->centroid().x;
if (m_y_range.min > feature->centroid().y)
m_y_range.min = feature->centroid().y;
if (m_y_range.max < feature->centroid().y)
m_y_range.max = feature->centroid().y;
#endif
}
void Trace::SaveChangesToDatabase(vpl::BlobTrackerDBManager& dbm, int target_id)
{
ASSERT(dbm.hasDB());
if (m_dbID < 0)
{
m_dbID = dbm.createTrace(m_id, start_time(), get_first_frame(), target_id);
if (m_dbID < 0)
{
ShowError("Cannot save target to database");
return;
}
}
for (; m_numFeaturesSaved < m_nodes.size(); m_numFeaturesSaved++)
{
BlobPtr f = m_nodes[m_numFeaturesSaved];
dbm.createTraceNode((int)f->centroid().x, (int)f->centroid().y, f->height(), f->width(),
f->timestamp(), m_numFeaturesSaved, m_dbID);
}
}
