add ambiguity factor for pose rejection

team401 · Dec 12, 2024 · 3c8463d · 3c8463d
1 parent 577c460
commit 3c8463d
Showing 1 changed file with 1 addition and 0 deletions.
diff --git a/vision/src/main/java/coppercore/vision/VisionLocalizer.java b/vision/src/main/java/coppercore/vision/VisionLocalizer.java
@@ -110,6 +110,7 @@ private boolean shouldRejectPose(VisionIO.PoseObservation observation) {
                 || Math.abs(observation.pose().getZ())
                         > CoreVisionConstants.maxZCutoff // Must have realistic Z coordinate
                 || observation.averageTagDistance() > CoreVisionConstants.maxAcceptedDistanceMeters
+                || observation.ambiguity() > 0.3
                 // Must be within the field boundaries
                 || observation.pose().getX() < 0.0
                 || observation.pose().getX() > aprilTagLayout.getFieldLength()