Estimate target location from blobs
Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: Ib9bfe0dc494b1581236d1f3f57bc65c6687981e5
diff --git a/y2022/vision/target_estimator.h b/y2022/vision/target_estimator.h
new file mode 100644
index 0000000..4988c3c
--- /dev/null
+++ b/y2022/vision/target_estimator.h
@@ -0,0 +1,26 @@
+#ifndef Y2022_VISION_POSE_ESTIMATOR_H_
+#define Y2022_VISION_POSE_ESTIMATOR_H_
+
+#include "opencv2/imgproc.hpp"
+#include "y2022/vision/target_estimate_generated.h"
+
+namespace y2022::vision {
+
+class TargetEstimator {
+ public:
+ // Computes the location of the target.
+ // blob_point is the mean (x, y) of blob pixels.
+ static TargetEstimateT EstimateTargetLocation(cv::Point2i blob_point,
+ const cv::Mat &intrinsics,
+ const cv::Mat &extrinsics);
+
+ private:
+ // Height of the center of the tape (m)
+ static constexpr double kTapeHeight = 2.58 + (0.05 / 2);
+ // Horizontal distance from tape to center of hub (m)
+ static constexpr double kUpperHubRadius = 1.22 / 2;
+};
+
+} // namespace y2022::vision
+
+#endif // Y2022_VISION_POSE_ESTIMATOR_H_