Send original corner points, not undistorted ones.
Change-Id: I7152edb0c08f6eec98f6d5f92e1ff8d05cfcc5a6
Signed-off-by: Jim Ostrowski <yimmy13@gmail.com>
diff --git a/y2023/vision/aprilrobotics.cc b/y2023/vision/aprilrobotics.cc
index 7ccde65..c9c29e8 100644
--- a/y2023/vision/aprilrobotics.cc
+++ b/y2023/vision/aprilrobotics.cc
@@ -18,13 +18,12 @@
std::string_view channel_name)
: calibration_data_(event_loop),
ftrace_(),
- image_callback_(
- event_loop, channel_name,
- [&](cv::Mat image_color_mat,
- const aos::monotonic_clock::time_point eof) {
- HandleImage(image_color_mat, eof);
- },
- chrono::milliseconds(5)),
+ image_callback_(event_loop, channel_name,
+ [&](cv::Mat image_color_mat,
+ const aos::monotonic_clock::time_point eof) {
+ HandleImage(image_color_mat, eof);
+ },
+ chrono::milliseconds(5)),
target_map_sender_(
event_loop->MakeSender<frc971::vision::TargetMap>("/camera")),
image_annotations_sender_(
@@ -154,6 +153,7 @@
std::vector<std::pair<apriltag_detection_t, apriltag_pose_t>> results;
+ std::vector<std::vector<cv::Point2f>> orig_corners_vector;
std::vector<std::vector<cv::Point2f>> corners_vector;
auto builder = image_annotations_sender_.MakeBuilder();
@@ -179,6 +179,15 @@
info.cx = intrinsics_.at<double>(0, 2);
info.cy = intrinsics_.at<double>(1, 2);
+ // Store out the original, pre-undistortion corner points for sending
+ std::vector<cv::Point2f> orig_corner_points;
+ orig_corner_points.emplace_back(det->p[0][0], det->p[0][1]);
+ orig_corner_points.emplace_back(det->p[1][0], det->p[1][1]);
+ orig_corner_points.emplace_back(det->p[2][0], det->p[2][1]);
+ orig_corner_points.emplace_back(det->p[3][0], det->p[3][1]);
+
+ orig_corners_vector.emplace_back(orig_corner_points);
+
UndistortDetection(det);
apriltag_pose_t pose;
@@ -207,8 +216,8 @@
}
}
- const auto annotations_offset =
- frc971::vision::BuildAnnotations(eof, corners_vector, 5.0, builder.fbb());
+ const auto annotations_offset = frc971::vision::BuildAnnotations(
+ eof, orig_corners_vector, 5.0, builder.fbb());
builder.CheckOk(builder.Send(annotations_offset));
apriltag_detections_destroy(detections);