Estimate target location from blobs
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: Ib9bfe0dc494b1581236d1f3f57bc65c6687981e5
diff --git a/y2022/vision/target_estimator.cc b/y2022/vision/target_estimator.cc
new file mode 100644
index 0000000..c51eb08
--- /dev/null
+++ b/y2022/vision/target_estimator.cc
@@ -0,0 +1,31 @@
+#include "y2022/vision/target_estimator.h"
+
+namespace y2022::vision {
+
+TargetEstimateT TargetEstimator::EstimateTargetLocation(
+ cv::Point2i blob_point, const cv::Mat &intrinsics,
+ const cv::Mat &extrinsics) {
+ const cv::Point2d focal_length(intrinsics.at<double>(0, 0),
+ intrinsics.at<double>(1, 1));
+ const cv::Point2d offset(intrinsics.at<double>(0, 2),
+ intrinsics.at<double>(1, 2));
+
+ // Blob pitch in camera reference frame
+ const double blob_pitch =
+ std::atan(static_cast<double>(-(blob_point.y - offset.y)) /
+ static_cast<double>(focal_length.y));
+ const double camera_height = extrinsics.at<double>(2, 3);
+ // Depth from camera to blob
+ const double depth = (kTapeHeight - camera_height) / std::tan(blob_pitch);
+
+ TargetEstimateT target;
+ target.angle_to_target =
+ std::atan2(static_cast<double>(blob_point.x - offset.x),
+ static_cast<double>(focal_length.x));
+ target.distance =
+ (depth / std::cos(target.angle_to_target)) + kUpperHubRadius;
+
+ return target;
+}
+
+} // namespace y2022::vision