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