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::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::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;
}
void
Tracker::createDistanceMatrix()
{
distance_matrix_ = cv::Mat_<double>(tracks_.size(), detections_.size());
int track = 0;
for(std::list<Track*>::iterator it = tracks_.begin(); it != tracks_.end(); it++)
{
//double x, y, height, vx, vz;
Track* t = *it;
//t->predict(x, y, height, vx, vz);
int measure = 0;
for(std::vector<open_ptrack::detection::Detection>::iterator dit = detections_.begin(); dit != detections_.end(); dit++)
{
double detector_likelihood;
// Compute detector likelihood:
if (detector_likelihood_)
{
detector_likelihood = dit->getConfidence();
// detector_likelihood = log((dit->getConfidence() + 3) / 6);
}
else
{
detector_likelihood = 0;
}
// Compute motion likelihood:
double motion_likelihood = t->getMahalanobisDistance(
dit->getWorldCentroid()(0),
dit->getWorldCentroid()(1),
dit->getSource()->getTime());
// Compute joint likelihood and put it in the distance matrix:
distance_matrix_(track, measure++) = likelihood_weights_[0] * detector_likelihood + likelihood_weights_[1] * motion_likelihood;
//std::cout << (*it)->getId() << ": " << "Motion likelihood: " << motionWeight * motionLikelihood << std::endl;
//if (detector_likelihood_)
// std::cout << (*it)->getId() << ": " << "Detector likelihood: " << detectorWeight * dit->getConfidence() << std::endl;
//std::cout << (*it)->getId() << ": " << "JOINT LIKELIHOOD: " << distance_matrix_(track, measure-1) << std::endl;
/*ROS_INFO("%d(%f, %f) = %f", t->getId(),
dit->getWorldCentroid()(0),
dit->getWorldCentroid()(1),
distance_matrix_(track, measure - 1));*/
}
track++;
}
// std::cout << "Distance matrix:" << std::endl;
// for(int row = 0; row < distance_matrix_.rows; row++)
// {
// for(int col = 0; col < distance_matrix_.cols; col++)
// {
// std::cout.width(8);
// std::cout << distance_matrix_(row,col) << ",";
// }
// std::cout << std::endl;
// }
// std::cout << std::endl;
}
