Stick blob results into a flatbuffer
subscibed to this in viewer
Signed-off-by: Henry Speiser <henry@speiser.net>
Change-Id: I5ea9a3b8ab31ea7c8e9351469ba80de9c6cc3d2a
Signed-off-by: Henry Speiser <henry@speiser.net>
diff --git a/y2022/vision/target_estimator.cc b/y2022/vision/target_estimator.cc
index c51eb08..1ac3d55 100644
--- a/y2022/vision/target_estimator.cc
+++ b/y2022/vision/target_estimator.cc
@@ -2,9 +2,10 @@
namespace y2022::vision {
-TargetEstimateT TargetEstimator::EstimateTargetLocation(
- cv::Point2i blob_point, const cv::Mat &intrinsics,
- const cv::Mat &extrinsics) {
+void TargetEstimator::EstimateTargetLocation(cv::Point2i centroid,
+ const cv::Mat &intrinsics,
+ const cv::Mat &extrinsics,
+ TargetEstimate::Builder *builder) {
const cv::Point2d focal_length(intrinsics.at<double>(0, 0),
intrinsics.at<double>(1, 1));
const cv::Point2d offset(intrinsics.at<double>(0, 2),
@@ -12,20 +13,19 @@
// Blob pitch in camera reference frame
const double blob_pitch =
- std::atan(static_cast<double>(-(blob_point.y - offset.y)) /
+ std::atan(static_cast<double>(-(centroid.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),
+ double angle_to_target =
+ std::atan2(static_cast<double>(centroid.x - offset.x),
static_cast<double>(focal_length.x));
- target.distance =
- (depth / std::cos(target.angle_to_target)) + kUpperHubRadius;
+ double distance = (depth / std::cos(angle_to_target)) + kUpperHubRadius;
- return target;
+ builder->add_angle_to_target(angle_to_target);
+ builder->add_distance(distance);
}
} // namespace y2022::vision