| milind-u | 9219598 | 2022-01-22 20:29:31 -0800 | [diff] [blame^] | 1 | #include "y2022/vision/target_estimator.h" |
| 2 | |
| 3 | namespace y2022::vision { |
| 4 | |
| 5 | TargetEstimateT TargetEstimator::EstimateTargetLocation( |
| 6 | cv::Point2i blob_point, const cv::Mat &intrinsics, |
| 7 | const cv::Mat &extrinsics) { |
| 8 | const cv::Point2d focal_length(intrinsics.at<double>(0, 0), |
| 9 | intrinsics.at<double>(1, 1)); |
| 10 | const cv::Point2d offset(intrinsics.at<double>(0, 2), |
| 11 | intrinsics.at<double>(1, 2)); |
| 12 | |
| 13 | // Blob pitch in camera reference frame |
| 14 | const double blob_pitch = |
| 15 | std::atan(static_cast<double>(-(blob_point.y - offset.y)) / |
| 16 | static_cast<double>(focal_length.y)); |
| 17 | const double camera_height = extrinsics.at<double>(2, 3); |
| 18 | // Depth from camera to blob |
| 19 | const double depth = (kTapeHeight - camera_height) / std::tan(blob_pitch); |
| 20 | |
| 21 | TargetEstimateT target; |
| 22 | target.angle_to_target = |
| 23 | std::atan2(static_cast<double>(blob_point.x - offset.x), |
| 24 | static_cast<double>(focal_length.x)); |
| 25 | target.distance = |
| 26 | (depth / std::cos(target.angle_to_target)) + kUpperHubRadius; |
| 27 | |
| 28 | return target; |
| 29 | } |
| 30 | |
| 31 | } // namespace y2022::vision |