Refactor vision config to use Factors interface

Introduced a Factors interface with ObjFactors and StdDevFactors implementations to generalize camera configuration parameters. Updated Vision, VisionConstants, and object detection logic to use the new factors structure, enabling more flexible and descriptive configuration for both standard deviation and object confidence weights.

Author: FarhanJ2

src/main/java/org/steelhawks/subsystems/vision/Vision.java

@@ -200,8 +200,8 @@ public void periodic() {
                     angularStdDev *= ANGULAR_STD_DEV_MEGATAG2_FACTOR;
                 }
                 if (cameraIndex < VisionConstants.getCameraConfig().length) {
-                    linearStdDev *= getCameraConfig()[cameraIndex].stdDevFactors();
-                    angularStdDev *= getCameraConfig()[cameraIndex].stdDevFactors();
+                    linearStdDev *= getCameraConfig()[cameraIndex].factors().getFactors()[0];
+                    angularStdDev *= getCameraConfig()[cameraIndex].factors().getFactors()[1];
                 }
                 if (useQuestNav && !Robot.isFirstRun()) {
                     assert questNav != null;

src/main/java/org/steelhawks/subsystems/vision/VisionConstants.java

@@ -28,11 +28,60 @@ public class VisionConstants {
         6, 7, 8, 9, 10, 11, 18, 19, 20, 21, 22
     };
 
+    public interface Factors {
+        default Double[] getFactors() {
+            return null;
+        }
+
+        class ObjFactors implements Factors {
+            private final Double[] factors;
+
+            /**
+             * Only used for Limelight, to calculate confidence score.
+             *
+             * @param w1 How much the angle matters to confidence
+             * @param w2 How much the shape matters to confidence
+             * @param w3 How much the area matters to confidence
+             */
+            public ObjFactors(double w1, double w2, double w3) {
+                factors = new Double[] { w1, w2, w3 };
+            }
+
+            public ObjFactors() {
+                this(0.0, 0.0, 0.0);
+            }
+
+            @Override
+            public Double[] getFactors() {
+                return factors;
+            }
+        }
+
+        class StdDevFactors implements Factors {
+            private final Double[] factors;
+
+            public StdDevFactors(double stdDevLinear, double stdDevAngular) {
+                factors = new Double[] {
+                    stdDevLinear, stdDevAngular
+                };
+            }
+
+            public StdDevFactors(double stdDevLinear) {
+                this(stdDevLinear, stdDevLinear);
+            }
+
+            @Override
+            public Double[] getFactors() {
+                return factors;
+            }
+        }
+    }
+
     /**
-     * @param stdDevFactors Standard deviation multipliers for each camera (Adjust to trust some cameras more than others)
+     * @param factors Standard deviation multipliers for each camera (Adjust to trust some cameras more than others) or for calculating object confidence
      */
     public record CameraConfig(
-        String name, Transform3d robotToCamera, double stdDevFactors, VisionConstants.CameraConfig.CameraType cameraType) {
+        String name, Transform3d robotToCamera, Factors factors, VisionConstants.CameraConfig.CameraType cameraType) {
         public enum CameraType {
             LIMELIGHT,
             PHOTON,
@@ -55,7 +104,7 @@ public enum CameraType {
                     Units.degreesToRadians(0.058),
                     Units.degreesToRadians(-15),
                     Units.degreesToRadians(-15))),
-            1.0,
+            new Factors.StdDevFactors(1.0),
             CameraType.PHOTON
         ),
 
@@ -75,7 +124,7 @@ public enum CameraType {
                     Units.degreesToRadians(0),
                     Units.degreesToRadians(-15),
                     Units.degreesToRadians(30))),
-            1.0,
+            new Factors.StdDevFactors(1.0),
             CameraType.PHOTON
         ),
 
@@ -93,7 +142,7 @@ public enum CameraType {
                     Units.degreesToRadians(0),
                     Units.degreesToRadians(0),
                     Units.degreesToRadians(-120))),
-            8.0,
+            new Factors.StdDevFactors(8.0),
             CameraType.PHOTON
         ),
 
@@ -111,7 +160,7 @@ public enum CameraType {
                     Units.degreesToRadians(0),
                     Units.degreesToRadians(0),
                     Units.degreesToRadians(90))),
-            9.0,
+            new Factors.StdDevFactors(9.0),
             CameraType.PHOTON
         )
     };
@@ -120,7 +169,7 @@ public enum CameraType {
         new CameraConfig(
             "limelight",
             Constants.fromOnshapeCoordinates(4.469, 9.261, 21.578, 15.0, -20.0, 0.0),
-            0.0,
+            new Factors.ObjFactors(0.5, 0.3, 0.2),
             CameraType.LIMELIGHT
         )
     };
@@ -129,7 +178,7 @@ public enum CameraType {
         new CameraConfig(
             "limelight-coral",
             new Transform3d(),
-            3.0,
+            new Factors.StdDevFactors(3.0),
             CameraType.LIMELIGHT
         )
     };
@@ -138,13 +187,13 @@ public enum CameraType {
         new CameraConfig(
             "limelight-shooter",
             new Transform3d(),
-            1.2,
+            new Factors.StdDevFactors(1.2),
             CameraType.LIMELIGHT
         ),
         new CameraConfig(
             "limelight",
             new Transform3d(),
-            1.3,
+            new Factors.StdDevFactors(1.3),
             CameraType.LIMELIGHT
         )
     };

src/main/java/org/steelhawks/subsystems/vision/objdetect/ObjectVision.java

@@ -67,9 +67,9 @@ public static double calcConfidence(ObjectVisionIO.DetectionInfo d, int cameraIn
         double txAvg = (d.tx()[0] + d.tx()[1] + d.tx()[2] + d.tx()[3]) / 4.0;
         double angleScore = Math.cos(txAvg); // roughly favors smaller offsets
 
-        double w1 = 0.5; // how much the area matters to confidence
-        double w2 = 0.3; // how much the shape matters to confidence
-        double w3 = 0.2; // how much the angle matters to confidence
+        double w1 = VisionConstants.getCameraConfig()[cameraIndex].factors().getFactors()[0];
+        double w2 = VisionConstants.getCameraConfig()[cameraIndex].factors().getFactors()[1];
+        double w3 = VisionConstants.getCameraConfig()[cameraIndex].factors().getFactors()[2];
 
         return MathUtil.clamp(Constants.loggedValue(logName + "w1", w1 * areaScore) +
             Constants.loggedValue(logName + "w2", w2 * ratio) +

src/main/java/org/steelhawks/subsystems/vision/objdetect/ObjectVisionIOLimelight.java

@@ -46,7 +46,7 @@ public void updateInputs(ObjectVisionIOInputs inputs) {
                 new ObjectObservation(
                     camIndex,
                     new DetectionInfo(detection),
-                    0.0, // TODO 0.0 for limelight, photon vision might just give you a confidence value in code but im not sure confirm and fix
+                    0.0, // 0.0 because limelight doesn't give you a confidence score, we calculate it ourselves
                     timestamp // timestamp in seconds
                 )
             );