From 3c8463d71ebcaa13bf22b1a719b99e16f038075a Mon Sep 17 00:00:00 2001 From: Jack Lingle Date: Thu, 12 Dec 2024 12:03:32 -0500 Subject: [PATCH] add ambiguity factor for pose rejection --- vision/src/main/java/coppercore/vision/VisionLocalizer.java | 1 + 1 file changed, 1 insertion(+) diff --git a/vision/src/main/java/coppercore/vision/VisionLocalizer.java b/vision/src/main/java/coppercore/vision/VisionLocalizer.java index 9ce3c12..f87e4a2 100644 --- 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()