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