Merge "Adding check that pixels lie within padded border of image left/right"
diff --git a/y2023/vision/aprilrobotics.cc b/y2023/vision/aprilrobotics.cc
index 3ad6671..c2e063a 100644
--- a/y2023/vision/aprilrobotics.cc
+++ b/y2023/vision/aprilrobotics.cc
@@ -157,6 +157,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();
@@ -189,6 +190,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;
@@ -217,8 +227,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);