| 289 | } |
| 290 | |
| 291 | void |
| 292 | Tracker::createDistanceMatrix() |
| 293 | { |
| 294 | distance_matrix_ = cv::Mat_<double>(tracks_.size(), detections_.size()); |
| 295 | int track = 0; |
| 296 | for(std::list<Track*>::const_iterator it = tracks_.begin(), |
| 297 | end = tracks_.end(); it != end; it++) |
| 298 | { |
| 299 | //double x, y, height, vx, vz; |
| 300 | Track* t = *it; |
| 301 | //t->predict(x, y, height, vx, vz); |
| 302 | int measure = 0; |
| 303 | for(std::vector<open_ptrack::detection::Detection>::iterator dit = detections_.begin(); dit != detections_.end(); dit++) |
| 304 | { |
| 305 | double detector_likelihood; |
| 306 | |
| 307 | // Compute detector likelihood: |
| 308 | if (detector_likelihood_) |
| 309 | { |
| 310 | detector_likelihood = dit->getConfidence(); |
| 311 | // detector_likelihood = log((dit->getConfidence() + 3) / 6); |
| 312 | } |
| 313 | else |
| 314 | { |
| 315 | detector_likelihood = 0; |
| 316 | } |
| 317 | |
| 318 | // Compute motion likelihood: |
| 319 | double motion_likelihood = t->getMahalanobisDistance( |
| 320 | dit->getWorldCentroid()(0), |
| 321 | dit->getWorldCentroid()(1), |
| 322 | dit->getSource()->getTime()); |
| 323 | |
| 324 | // Compute joint likelihood and put it in the distance matrix: |
| 325 | |
| 326 | distance_matrix_(track, measure++) = likelihood_weights_[0] * detector_likelihood + likelihood_weights_[1] * motion_likelihood; |
| 327 | |
| 328 | // Remove NaN and inf: |
| 329 | if (std::isnan(distance_matrix_(track, measure-1)) | (not std::isfinite(distance_matrix_(track, measure-1)))) |
| 330 | distance_matrix_(track, measure-1) = 2*gate_distance_; |
| 331 | |
| 332 | // std::cout << (*it)->getId() << ": " << "Motion likelihood: " << likelihood_weights_[0] * motion_likelihood << std::endl; |
| 333 | // if (detector_likelihood_) |
| 334 | // std::cout << (*it)->getId() << ": " << "Detector likelihood: " << likelihood_weights_[1] * dit->getConfidence() << std::endl; |
| 335 | // std::cout << (*it)->getId() << ": " << "JOINT LIKELIHOOD: " << distance_matrix_(track, measure-1) << std::endl; |
| 336 | |
| 337 | /*ROS_INFO("%d(%f, %f) = %f", t->getId(), |
| 338 | dit->getWorldCentroid()(0), |
| 339 | dit->getWorldCentroid()(1), |
| 340 | distance_matrix_(track, measure - 1));*/ |
| 341 | } |
| 342 | track++; |
| 343 | } |
| 344 | |
| 345 | // std::cout << "Distance matrix:" << std::endl; |
| 346 | // for(int row = 0; row < distance_matrix_.rows; row++) |
| 347 | // { |
| 348 | // for(int col = 0; col < distance_matrix_.cols; col++) |
nothing calls this directly
no test coverage detected