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

frc971/vision/target_map.fbs

diff --git a/frc971/vision/target_map.fbs b/frc971/vision/target_map.fbs
index e635760..de79744 100644
--- a/frc971/vision/target_map.fbs
+++ b/frc971/vision/target_map.fbs
@@ -63,6 +63,13 @@
   // End-of-frame timestamp for the frame with tag detections.
   // (for use case 2.).
   monotonic_timestamp_ns:int64 (id: 2);
+
+  // Number of april tags rejected (cumulative) because
+  // of low decision margin (affected by lighting).
+  // We do the decision margin rejection in aprilrobotics
+  // so we don't have an excessive amount of random target
+  // detections (for use case 2).
+  rejections:uint64 (id: 3);
 }
 
 root_type TargetMap;

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

y2023/vision/aprilrobotics.h

diff --git a/y2023/vision/aprilrobotics.h b/y2023/vision/aprilrobotics.h
index bf9265b..fab2d30 100644
--- a/y2023/vision/aprilrobotics.h
+++ b/y2023/vision/aprilrobotics.h
@@ -31,6 +31,11 @@
     double distortion_factor;
   };
 
+  struct DetectionResult {
+    std::vector<Detection> detections;
+    size_t rejections;
+  };
+
   AprilRoboticsDetector(aos::EventLoop *event_loop,
                         std::string_view channel_name);
   ~AprilRoboticsDetector();
@@ -43,8 +48,8 @@
   // Helper function to store detection points in vector of Point2f's
   std::vector<cv::Point2f> MakeCornerVector(const apriltag_detection_t *det);
 
-  std::vector<Detection> DetectTags(cv::Mat image,
-                                    aos::monotonic_clock::time_point eof);
+  DetectionResult DetectTags(cv::Mat image,
+                             aos::monotonic_clock::time_point eof);
 
   const std::optional<cv::Mat> extrinsics() const { return extrinsics_; }
   const cv::Mat intrinsics() const { return intrinsics_; }
@@ -78,6 +83,8 @@
   frc971::vision::ImageCallback image_callback_;
   aos::Sender<frc971::vision::TargetMap> target_map_sender_;
   aos::Sender<foxglove::ImageAnnotations> image_annotations_sender_;
+
+  size_t rejections_;
 };
 
 }  // namespace vision