void
Tracker::createCostMatrix()
{
cost_matrix_ = distance_matrix_.clone();
for(int i = 0; i < distance_matrix_.rows; i++)
{
for(int j = 0; j < distance_matrix_.cols; j++)
{
if(distance_matrix_(i, j) > gate_distance_)
cost_matrix_(i, j) = 1000000.0;
}
}
// std::cout << "Munkres input matrix:" << std::endl;
// for(int row = 0; row < cost_matrix_.rows; row++)
// {
// for(int col = 0; col < cost_matrix_.cols; col++)
// {
// std::cout.width(8);
// std::cout << cost_matrix_(row,col) << ",";
// }
// std::cout << std::endl;
// }
// std::cout << std::endl;
}
void
Tracker::fillUnassociatedDetections()
{
// Fill a list with detections not associated to any track:
if(cost_matrix_.cols == 0 && detections_.size() > 0)
{
for(size_t measure = 0; measure < detections_.size(); measure++)
unassociated_detections_.push_back(detections_[measure]);
}
else
{
for(int measure = 0; measure < cost_matrix_.cols; measure++)
{
bool associated = false;
for(int track = 0; track < cost_matrix_.rows; track++)
{
if(cost_matrix_(track, measure) == 0.0)
{
if(distance_matrix_(track, measure) > gate_distance_)
break;
associated = true;
}
}
if(!associated/* && detections_[measure].getConfidence() > min_confidence_*/)
{
unassociated_detections_.push_back(detections_[measure]);
}
}
}
}
void
Tracker::updateDetectedTracks()
{
// std::cout << "Munkres output matrix:" << std::endl;
// for(int row = 0; row < cost_matrix_.rows; row++) {
// for(int col = 0; col < cost_matrix_.cols; col++) {
// std::cout.width(1);
// std::cout << cost_matrix_(row,col) << ",";
// }
// std::cout << std::endl;
// }
// std::cout << std::endl;
// Iterate over every track:
int track = 0;
for(std::list<open_ptrack::tracking::Track*>::iterator it = tracks_.begin(); it != tracks_.end(); it++)
{
bool updated = false;
open_ptrack::tracking::Track* t = *it;
for(int measure = 0; measure < cost_matrix_.cols; measure++)
{
// If a detection<->track association has been found:
if(cost_matrix_(track, measure) == 0.0 && distance_matrix_(track, measure) <= gate_distance_)
{
open_ptrack::detection::Detection& d = detections_[measure];
// If the detection has enough confidence in the current frame or in a recent past:
// if ((t->getLowConfidenceConsecutiveFrames() < 10) || ((d.getConfidence() - 0.5) > min_confidence_detections_))
if ((t->getLowConfidenceConsecutiveFrames() < 10) || (d.getConfidence() > ((min_confidence_ + min_confidence_detections_)/2)))
{
// Update track with the associated detection:
bool first_update = false;
t->update(d.getWorldCentroid()(0), d.getWorldCentroid()(1), d.getWorldCentroid()(2),d.getHeight(),
d.getDistance(), //distance_matrix_(track, measure),
d.getConfidence(), min_confidence_, min_confidence_detections_,
d.getSource(), first_update);
t->setVisibility(d.isOccluded() ? Track::OCCLUDED : Track::VISIBLE);
updated = true;
break;
}
else
{
std::cout << "Id: " << t->getId() << ", lowConfConsFrames: " << t->getLowConfidenceConsecutiveFrames() << ", newConf: " << d.getConfidence()<< std::endl;
}
}
}
if(!updated)
{
if(t->getVisibility() != Track::NOT_VISIBLE)
{
t->setVisibility(Track::NOT_VISIBLE);
//t->update();
}
}
track++;
}
// std::cout << std::endl;
}
