commit 99b1a765c96de9d35cc94396da72bde8ee89a9c4
author milind-u <milind.upadhyay@gmail.com> Sun Mar 12 16:48:32 2023 -0700
committer milind-u <milind.upadhyay@gmail.com> Wed Mar 15 20:57:36 2023 -0700
tree 5704b9a71e47ebe31e85e04a486e60de3235a28d
parent 6e0d865e1c5beb14fdc5e2620f8f3483a47bf929

Count decision margin rejections in aprilrobotics

That way we'll easily be able to tell if we need to adjust the threshold
because of lighting. Next step is adding this output from each pi
to the webpage.

Signed-off-by: milind-u <milind.upadhyay@gmail.com>
Change-Id: I20491dab30850c0a2115bcdf6ef1dbec8153e673

y2023/vision/aprilrobotics.cc

diff --git a/y2023/vision/aprilrobotics.cc b/y2023/vision/aprilrobotics.cc
index 45b0cf9..95ad541 100644
--- a/y2023/vision/aprilrobotics.cc
+++ b/y2023/vision/aprilrobotics.cc
@@ -36,7 +36,8 @@
       target_map_sender_(
           event_loop->MakeSender<frc971::vision::TargetMap>("/camera")),
       image_annotations_sender_(
-          event_loop->MakeSender<foxglove::ImageAnnotations>("/camera")) {
+          event_loop->MakeSender<foxglove::ImageAnnotations>("/camera")),
+      rejections_(0) {
   tag_family_ = tag16h5_create();
   tag_detector_ = apriltag_detector_create();
 
@@ -89,17 +90,18 @@
                                         aos::monotonic_clock::time_point eof) {
   image_size_ = image_grayscale.size();
 
-  std::vector<Detection> detections = DetectTags(image_grayscale, eof);
+  DetectionResult result = DetectTags(image_grayscale, eof);
 
   auto builder = target_map_sender_.MakeBuilder();
   std::vector<flatbuffers::Offset<frc971::vision::TargetPoseFbs>> target_poses;
-  for (const auto &detection : detections) {
+  for (const auto &detection : result.detections) {
     target_poses.emplace_back(BuildTargetPose(detection, builder.fbb()));
   }
   const auto target_poses_offset = builder.fbb()->CreateVector(target_poses);
   auto target_map_builder = builder.MakeBuilder<frc971::vision::TargetMap>();
   target_map_builder.add_target_poses(target_poses_offset);
   target_map_builder.add_monotonic_timestamp_ns(eof.time_since_epoch().count());
+  target_map_builder.add_rejections(result.rejections);
   builder.CheckOk(builder.Send(target_map_builder.Finish()));
 }
 
@@ -177,7 +179,7 @@
   return corner_points;
 }
 
-std::vector<AprilRoboticsDetector::Detection> AprilRoboticsDetector::DetectTags(
+AprilRoboticsDetector::DetectionResult AprilRoboticsDetector::DetectTags(
     cv::Mat image, aos::monotonic_clock::time_point eof) {
   const aos::monotonic_clock::time_point start_time =
       aos::monotonic_clock::now();
@@ -273,6 +275,8 @@
                                      .pose = pose,
                                      .pose_error = pose_error,
                                      .distortion_factor = distortion_factor});
+    } else {
+      rejections_++;
     }
   }
 
@@ -292,7 +296,7 @@
   VLOG(1) << "Took " << chrono::duration<double>(end_time - start_time).count()
           << " seconds to detect overall";
 
-  return results;
+  return {.detections = results, .rejections = rejections_};
 }
 
 }  // namespace vision