| 387 | } |
| 388 | |
| 389 | void |
| 390 | Track::draw(bool vertical) |
| 391 | { |
| 392 | cv::Scalar color(int(255.0 * color_(0)), int(255.0 * color_(1)), int(255.0 * color_(2))); |
| 393 | |
| 394 | double _x2, _y2; |
| 395 | tmp_filter_->getState(_x2, _y2); |
| 396 | Eigen::Vector3d centroid2(_x2, _y2, z_); |
| 397 | centroid2 = detection_source_->transformToCam(centroid2); |
| 398 | |
| 399 | if(visibility_ == Track::NOT_VISIBLE) |
| 400 | return; |
| 401 | |
| 402 | double _x, _y; |
| 403 | filter_->getState(_x, _y); |
| 404 | |
| 405 | cv::Scalar darkBlue(130,0,0); |
| 406 | cv::Scalar white(255,255,255); |
| 407 | Eigen::Vector3d centroid(_x, _y, z_ ); |
| 408 | Eigen::Vector3d top(_x, _y, z_ + (height_/2)); |
| 409 | Eigen::Vector3d bottom(_x, _y, z_ - (height_/2)); |
| 410 | |
| 411 | std::vector<Eigen::Vector3d> points; |
| 412 | double delta = height_ / 5.0; |
| 413 | points.push_back(Eigen::Vector3d(_x - delta, _y - delta, z_ - (height_/2))); |
| 414 | points.push_back(Eigen::Vector3d(_x + delta, _y - delta, z_ - (height_/2))); |
| 415 | points.push_back(Eigen::Vector3d(_x + delta, _y + delta, z_ - (height_/2))); |
| 416 | points.push_back(Eigen::Vector3d(_x - delta, _y + delta, z_ - (height_/2))); |
| 417 | points.push_back(Eigen::Vector3d(_x - delta, _y - delta, z_ + (height_/2))); |
| 418 | points.push_back(Eigen::Vector3d(_x + delta, _y - delta, z_ + (height_/2))); |
| 419 | points.push_back(Eigen::Vector3d(_x + delta, _y + delta, z_ + (height_/2))); |
| 420 | points.push_back(Eigen::Vector3d(_x - delta, _y + delta, z_ + (height_/2))); |
| 421 | |
| 422 | //TODO loop for each detection source |
| 423 | |
| 424 | centroid = detection_source_->transformToCam(centroid); |
| 425 | cv::circle(detection_source_->getImage(), cv::Point(centroid(0), centroid(1)), 5, color, 1); |
| 426 | top = detection_source_->transformToCam(top); |
| 427 | bottom = detection_source_->transformToCam(bottom); |
| 428 | for(std::vector<Eigen::Vector3d>::iterator it = points.begin(); it != points.end(); it++) |
| 429 | *it = detection_source_->transformToCam(*it); |
| 430 | |
| 431 | // Draw a paralellepiped around the person: |
| 432 | for(size_t i = 0; i < 4; i++) |
| 433 | { |
| 434 | cv::line(detection_source_->getImage(), cv::Point(points[i](0), points[i](1)), |
| 435 | cv::Point(points[(i + 1) % 4](0), points[(i + 1) % 4](1)), color, |
| 436 | visibility_ == VISIBLE ? 2 : 1, CV_AA); |
| 437 | cv::line(detection_source_->getImage(), cv::Point(points[i + 4](0), points[i + 4](1)), |
| 438 | cv::Point(points[(i + 1) % 4 + 4](0), points[(i + 1) % 4 + 4](1)), color, |
| 439 | visibility_ == VISIBLE ? 2 : 1, CV_AA); |
| 440 | cv::line(detection_source_->getImage(), cv::Point(points[i](0), points[i](1)), |
| 441 | cv::Point(points[i + 4](0), points[i + 4](1)), color, |
| 442 | visibility_ == VISIBLE ? 2 : 1, CV_AA); |
| 443 | } |
| 444 | |
| 445 | std::stringstream ss; |
| 446 | float distance_to_display = float(int(distance_*100))/100; |