| 272 | } |
| 273 | |
| 274 | void |
| 275 | Tracker3D::createDistanceMatrix() |
| 276 | { |
| 277 | distance_matrix_ = cv::Mat_<double>(tracks_.size(), detections_.size()); |
| 278 | int track = 0; |
| 279 | for(std::list<Track3D*>::const_iterator it = tracks_.begin(), |
| 280 | end = tracks_.end(); it != end; it++) |
| 281 | { |
| 282 | //double x, y, height, vx, vz; |
| 283 | Track3D* t = *it; |
| 284 | //t->predict(x, y, height, vx, vz); |
| 285 | int measure = 0; |
| 286 | for(std::vector<open_ptrack::detection::Detection>::iterator dit = detections_.begin(); dit != detections_.end(); dit++) |
| 287 | { |
| 288 | double detector_likelihood; |
| 289 | |
| 290 | // Compute detector likelihood: |
| 291 | if (detector_likelihood_) |
| 292 | { |
| 293 | detector_likelihood = dit->getConfidence(); |
| 294 | // detector_likelihood = log((dit->getConfidence() + 3) / 6); |
| 295 | } |
| 296 | else |
| 297 | { |
| 298 | detector_likelihood = 0; |
| 299 | } |
| 300 | |
| 301 | // Compute motion likelihood: |
| 302 | double motion_likelihood = t->getMahalanobisDistance( |
| 303 | dit->getWorldCentroid()(0), |
| 304 | dit->getWorldCentroid()(1), |
| 305 | dit->getWorldCentroid()(2), |
| 306 | dit->getSource()->getTime()); |
| 307 | |
| 308 | // Compute joint likelihood and put it in the distance matrix: |
| 309 | |
| 310 | distance_matrix_(track, measure++) = likelihood_weights_[0] * detector_likelihood + likelihood_weights_[1] * motion_likelihood; |
| 311 | |
| 312 | // Remove NaN and inf: |
| 313 | if (std::isnan(distance_matrix_(track, measure-1)) | (not std::isfinite(distance_matrix_(track, measure-1)))) |
| 314 | distance_matrix_(track, measure-1) = 2*gate_distance_; |
| 315 | |
| 316 | // std::cout << (*it)->getId() << ": " << "Motion likelihood: " << likelihood_weights_[0] * motion_likelihood << std::endl; |
| 317 | // if (detector_likelihood_) |
| 318 | // std::cout << (*it)->getId() << ": " << "Detector likelihood: " << likelihood_weights_[1] * dit->getConfidence() << std::endl; |
| 319 | // std::cout << (*it)->getId() << ": " << "JOINT LIKELIHOOD: " << distance_matrix_(track, measure-1) << std::endl; |
| 320 | |
| 321 | /*ROS_INFO("%d(%f, %f) = %f", t->getId(), |
| 322 | dit->getWorldCentroid()(0), |
| 323 | dit->getWorldCentroid()(1), |
| 324 | distance_matrix_(track, measure - 1));*/ |
| 325 | } |
| 326 | track++; |
| 327 | } |
| 328 | |
| 329 | // std::cout << "Distance matrix:" << std::endl; |
| 330 | // for(int row = 0; row < distance_matrix_.rows; row++) |
| 331 | // { |
nothing calls this directly
no test coverage detected