commit 728ae2238d5ce96e0fc418b2a6cc7d7686d7b7e0
author Yash Chainani <yashchainani28@gmail.com> Sat Feb 04 19:48:12 2023 -0800
committer Yash Chainani <yashchainani28@gmail.com> Wed Feb 08 20:19:27 2023 -0800
tree 1498f07d8e3a1691f0b7c0216d3b127679023db4
parent 7c1939b561a5af78de0b271945af77677854efe4

Write april tag image coordinates to flatbuffer

Signed-off-by: Yash Chainani <yashchainani28@gmail.com>
Change-Id: I073e362b58dc05fb917d9ed5ad0d923cbdf7da6e

y2023/vision/aprilrobotics.cc

diff --git a/y2023/vision/aprilrobotics.cc b/y2023/vision/aprilrobotics.cc
index d20a247..fbbc01d 100644
--- a/y2023/vision/aprilrobotics.cc
+++ b/y2023/vision/aprilrobotics.cc
@@ -19,7 +19,9 @@
                         HandleImage(image_color_mat, eof);
                       }),
       target_map_sender_(
-          event_loop->MakeSender<frc971::vision::TargetMap>("/camera")) {
+          event_loop->MakeSender<frc971::vision::TargetMap>("/camera")),
+      april_debug_sender_(
+          event_loop->MakeSender<y2023::vision::AprilDebug>("/camera")) {
   tag_family_ = tag16h5_create();
   tag_detector_ = apriltag_detector_create();
 
@@ -120,6 +122,10 @@
 
   std::vector<std::pair<apriltag_detection_t, apriltag_pose_t>> results;
 
+  std::vector<flatbuffers::Offset<AprilCorners>> corners_vector;
+
+  auto builder = april_debug_sender_.MakeBuilder();
+
   for (int i = 0; i < zarray_size(detections); i++) {
     apriltag_detection_t *det;
     zarray_get(detections, i, &det);
@@ -155,9 +161,35 @@
                                                before_pose_estimation)
                      .count()
               << " seconds for pose estimation";
+
+      std::vector<Point> corner_points;
+
+      corner_points.emplace_back(det->p[0][0], det->p[0][1]);
+      corner_points.emplace_back(det->p[1][0], det->p[1][1]);
+      corner_points.emplace_back(det->p[2][0], det->p[2][1]);
+      corner_points.emplace_back(det->p[3][0], det->p[3][1]);
+
+      auto corner_points_fbs =
+          builder.fbb()->CreateVectorOfStructs(corner_points);
+
+      AprilCorners::Builder april_corners_builder =
+          builder.MakeBuilder<AprilCorners>();
+
+      april_corners_builder.add_id(det->id);
+      april_corners_builder.add_points(corner_points_fbs);
+
+      corners_vector.emplace_back(april_corners_builder.Finish());
     }
   }
 
+  auto corners_vector_fbs = builder.fbb()->CreateVector(corners_vector);
+
+  AprilDebug::Builder april_debug_builder = builder.MakeBuilder<AprilDebug>();
+
+  april_debug_builder.add_corners(corners_vector_fbs);
+
+  builder.CheckOk(builder.Send(april_debug_builder.Finish()));
+
   apriltag_detections_destroy(detections);
 
   const aos::monotonic_clock::time_point end_time = aos::monotonic_clock::now();