unnest conditionals and add constants

team401 · Dec 3, 2024 · 28a704a · 28a704a
1 parent dd69da8
commit 28a704a
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 7 deletions.
diff --git a/vision/src/main/java/coppercore/vision/Camera.java b/vision/src/main/java/coppercore/vision/Camera.java
@@ -45,16 +45,15 @@ public Matrix<N3, N1> getLatestVariance() {
 
         if (numTags == 0) {
             return stdDev;
-        } else {
-            if (numTags > 1) {
+        } else if (numTags > 1) {
                 stdDev = CoreVisionConstants.multiTagStdDev;
-            } else if (numTags == 1 && avgDistanceFromTarget > 4) {
+        } else if (numTags == 1 && avgDistanceFromTarget > CoreVisionConstants.singleTagDistanceCutoff) {
                 return CoreVisionConstants.rejectionStdDev;
-            }
-
-            // distance based variance
-            stdDev = stdDev.times(1 + (Math.pow(avgDistanceFromTarget, 2) / 30));
         }
+
+        // distance based variance
+        stdDev = stdDev.times(1 + (Math.pow(avgDistanceFromTarget, 2) / CoreVisionConstants.distanceFactor));
+
         return stdDev;
     }
 }
diff --git a/vision/src/main/java/coppercore/vision/CoreVisionConstants.java b/vision/src/main/java/coppercore/vision/CoreVisionConstants.java
@@ -18,4 +18,6 @@ public final class CoreVisionConstants {
     public static final Matrix<N3, N1> multiTagStdDev = VecBuilder.fill(0.5, 0.5, 0.5);
     public static final Matrix<N3, N1> rejectionStdDev =
             VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
+    public static final double singleTagDistanceCutoff = 4.0;
+    public static final double distanceFactor = 30.0;
 }