Merge remote-tracking branch 'origin/vision' into develop

lasarobotics · Feb 27, 2025 · c8c291c · c8c291c
2 parents fdb0ef3 + 2bfe34d
commit c8c291c
Showing 1 changed file with 11 additions and 4 deletions.
diff --git a/src/main/java/org/lasarobotics/vision/AprilTagCamera.java b/src/main/java/org/lasarobotics/vision/AprilTagCamera.java
@@ -18,6 +18,7 @@
 import org.photonvision.PhotonPoseEstimator.PoseStrategy;
 import org.photonvision.simulation.PhotonCameraSim;
 import org.photonvision.simulation.SimCameraProperties;
+import org.photonvision.targeting.PhotonTrackedTarget;
 
 import edu.wpi.first.apriltag.AprilTag;
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
@@ -196,22 +197,28 @@ private void run() {
 
         // Get distance to closest tag
         var closestTagDistance = Units.Meters.of(100.0);
+        PhotonTrackedTarget closestTag = null;
         // Number of tags in range
         int numOfTagsInRange = 0;
         // Loop through all targets used for this estimate
         for (var target : estimatedRobotPose.targetsUsed) {
           // Get distance to tag
           var tagDistance = Units.Meters.of(target.getBestCameraToTarget().getTranslation().getNorm());
           // Check if tag distance is closest yet
-          if (tagDistance.lte(closestTagDistance)) closestTagDistance = tagDistance;
+          if (tagDistance.lte(closestTagDistance)) {
+            closestTagDistance = tagDistance;
+            closestTag = target;
+          }
           // Increment number of tags in range if applicable
           if (tagDistance.lte(MAX_TAG_DISTANCE)) numOfTagsInRange++;
         }
 
-        // Ignore if tags are too far
+        // Ignore if tags are too far or if the single tag within range is too ambiguous
         if (numOfTagsInRange < 2 && estimatedRobotPose.targetsUsed.size() > 1) {
-          m_latestResult.set(null);
-          return;
+          if (closestTag == null || closestTag.getPoseAmbiguity() > APRILTAG_POSE_AMBIGUITY_THRESHOLD) {
+            m_latestResult.set(null);
+            return;
+          }
         }
 
         // Calculate standard deviation