commit 5ddd515bb44670c6e4da8c77040df543a4330d07
author milind-u <milind.upadhyay@gmail.com> Sat Mar 04 15:19:17 2023 -0800
committer Milind Upadhyay <milind.upadhyay@gmail.com> Sun Mar 05 13:59:55 2023 -0800
tree 3e769bb5094c43f25662fe9dd3ffb42e9a3dbed2
parent 8791beddf07b355c232dea819e6757bebf69e0b4

Filter out large pose errors in target mapping

Following same logic as the localizer.

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

y2023/vision/target_mapping.cc

diff --git a/y2023/vision/target_mapping.cc b/y2023/vision/target_mapping.cc
index c59414b..e399470 100644
--- a/y2023/vision/target_mapping.cc
+++ b/y2023/vision/target_mapping.cc
@@ -32,6 +32,8 @@
              "Use the calibration for a node with this team number");
 DEFINE_string(mcap_output_path, "/tmp/log.mcap", "Log to output.");
 DEFINE_string(pi, "pi1", "Pi name to generate mcap log for; defaults to pi1.");
+DEFINE_double(max_pose_error, 1e-6,
+              "Throw out target poses with a higher pose error than this");
 
 namespace y2023 {
 namespace vision {
@@ -52,12 +54,17 @@
 
 // Add detected apriltag poses relative to the robot to
 // timestamped_target_detections
-void HandleAprilTag(const TargetMap &map,
-                    aos::distributed_clock::time_point pi_distributed_time,
-                    std::vector<DataAdapter::TimestampedDetection>
-                        *timestamped_target_detections,
-                    Eigen::Affine3d extrinsics) {
+void HandleAprilTags(const TargetMap &map,
+                     aos::distributed_clock::time_point pi_distributed_time,
+                     std::vector<DataAdapter::TimestampedDetection>
+                         *timestamped_target_detections,
+                     Eigen::Affine3d extrinsics) {
   for (const auto *target_pose_fbs : *map.target_poses()) {
+    // Skip detections with high pose errors
+    if (target_pose_fbs->pose_error() > FLAGS_max_pose_error) {
+      continue;
+    }
+
     const TargetMapper::TargetPose target_pose =
         PoseUtils::TargetPoseFromFbs(*target_pose_fbs);
 
@@ -82,7 +89,7 @@
   }
 }
 
-// Get images from pi and pass apriltag positions to HandleAprilTag()
+// Get images from pi and pass apriltag positions to HandleAprilTags()
 void HandlePiCaptures(
     aos::EventLoop *detection_event_loop, aos::EventLoop *mapping_event_loop,
     aos::logger::LogReader *reader,
@@ -107,8 +114,8 @@
             ->ToDistributedClock(aos::monotonic_clock::time_point(
                 aos::monotonic_clock::duration(map.monotonic_timestamp_ns())));
 
-    HandleAprilTag(map, pi_distributed_time, timestamped_target_detections,
-                   extrinsics);
+    HandleAprilTags(map, pi_distributed_time, timestamped_target_detections,
+                    extrinsics);
   });
 }