Refactor object detection to use confidence calculation

Introduces a new confidence calculation for object detections using area, shape, and angle metrics, and filters observations based on a tunable confidence threshold. Updates the ObjectVisionIO interfaces and implementations to support new detection info structures and camera indices, and modifies constructors and usages accordingly.

Author: FarhanJ2

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

@@ -192,8 +192,8 @@ public static ObjectVisionIO[] getObjIO() {
         for (int i = 0; i < config.length; i++) {
             if (RobotBase.isReal()) {
                 switch (config[i].cameraType) {
-                    case LIMELIGHT -> io[i] = new ObjectVisionIOLimelight(config[i].name);
-                    case PHOTON -> io[i] = new ObjectVisionIOPhoton(config[i].name, config[i].robotToCamera);
+                    case LIMELIGHT -> io[i] = new ObjectVisionIOLimelight(config[i].name, i);
+                    case PHOTON -> io[i] = new ObjectVisionIOPhoton(config[i].name, i, config[i].robotToCamera);
                 }
             } else if (Constants.getRobot() == Constants.RobotType.SIMBOT && !RobotBase.isReal()) {
                 io[i] = new ObjectVisionSim(

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

@@ -1,13 +1,16 @@
 package org.steelhawks.subsystems.vision.objdetect;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.*;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import org.littletonrobotics.junction.Logger;
+import org.steelhawks.Constants;
 import org.steelhawks.FieldConstants;
 import org.steelhawks.RobotContainer;
 import org.steelhawks.subsystems.vision.VisionConstants;
+import org.steelhawks.util.LoggedTunableNumber;
 
 import java.util.*;
 import java.util.stream.Collectors;
@@ -17,6 +20,11 @@ public class ObjectVision extends SubsystemBase {
     private static final double coralOverlap = 0.5; // meters
     private static final double coralMaxAge = 10.0; // seconds
 
+    private static final LoggedTunableNumber maxArea =
+        new LoggedTunableNumber("ObjectVision/MaxArea", 20.0);
+    private static final LoggedTunableNumber confidenceThreshold =
+        new LoggedTunableNumber("ObjectVision/ConfidenceThreshold", 0.3);
+
     private final ObjectVisionIO[] io;
     private final ObjectVisionIOInputsAutoLogged[] inputs;
 
@@ -26,7 +34,7 @@ record CoralPose(
 
     private final FieldObject2d coralObjects = FieldConstants.FIELD_2D.getObject("Corals");
     private final ArrayList<ObjectVisionIO.ObjectObservation> allObservations = new ArrayList<>();
-    private Set<CoralPose> coralPoses = new HashSet<>();
+    private final Set<CoralPose> coralPoses = new HashSet<>();
 
     public ObjectVision() {
         this.io = VisionConstants.getObjIO();
@@ -36,24 +44,39 @@ public ObjectVision() {
         }
     }
 
-    @Override
-    public void periodic() {
-        for (int i = 0; i < inputs.length; i++) {
-            io[i].updateInputs(inputs[i]);
-            Logger.processInputs("ObjectVision/" + io[i].getName(), inputs[i]);
-            allObservations.addAll(Arrays.asList(inputs[i].observations));
-        }
-        allObservations.stream()
-            .sorted(Comparator.comparingDouble(ObjectVisionIO.ObjectObservation::timestamp))
-            .forEach(this::addCoralObservationToPose);
-
-        coralObjects.setPoses(
-            coralPoses.stream()
-                .map(coral -> new Pose2d(coral.translation, new Rotation2d())) // zero rotation
-                .collect(Collectors.toList()));
-        allObservations.clear();
+    /*
+     * NEED TO TUNE MAX AREA
+     * NEED TO TUNE W1, W2, W3
+     * Log values to tune properly
+     */
+    public static double calcConfidence(ObjectVisionIO.DetectionInfo d, int cameraIndex) {
+        String logName = "ObjectVision/" +
+            Objects.requireNonNull(VisionConstants.getObjDetectConfig())[cameraIndex].name() + "/";
+
+        // areaScore: normalized 0–1
+        double areaScore = Math.min(d.area() / maxArea.get(), 1.0);
+
+        // shapeScore: 0 = skewed, 1 = rectangle
+        double width = Math.abs(d.tx()[0] - d.tx()[1]);
+        double height = Math.abs(d.ty()[0] - d.ty()[2]);
+        double diag1 = Math.hypot(d.tx()[0] - d.tx()[2], d.ty()[0] - d.ty()[2]);
+        double diag2 = Math.hypot(d.tx()[1] - d.tx()[3], d.ty()[1] - d.ty()[3]);
+        double ratio = Math.min(diag1, diag2) / Math.max(diag1, diag2); // 0–1, shapeScore
+
+        // angleScore: penalize extreme horizontal angles
+        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
+
+        return MathUtil.clamp(Constants.loggedValue(logName + "w1", w1 * areaScore) +
+            Constants.loggedValue(logName + "w2", w2 * ratio) +
+                Constants.loggedValue(logName + "w3", w3 * angleScore), 0.0, 1.0);
     }
 
+
     private void addCoralObservationToPose(ObjectVisionIO.ObjectObservation observation) {
         double now = Timer.getFPGATimestamp();
         Optional<Pose2d> oldWheelOdomPose = RobotContainer.s_Swerve.getPoseAtTime(observation.timestamp());
@@ -67,7 +90,8 @@ private void addCoralObservationToPose(ObjectVisionIO.ObjectObservation observat
         // robot actually moved according to the wheel odometry since the observation
         Pose2d fieldToRobot =
             estimatedPose.transformBy(new Transform2d(wheelOdometryPose, oldWheelOdomPose.get()));
-        Pose3d robotToCamera = new Pose3d(0.0, 0.0, 0.0, new Rotation3d());
+        Transform3d robotToCamera =
+            Objects.requireNonNull(VisionConstants.getObjDetectConfig())[observation.camIndex()].robotToCamera();
 
         // find object midpoint
         double tx = (observation.tx()[2] + observation.tx()[3]) / 2.0;
@@ -86,23 +110,43 @@ private void addCoralObservationToPose(ObjectVisionIO.ObjectObservation observat
         double lateralCorrection = 1.0 / Math.cos(-tx); // correction for horizontal angle to get lateral distance
         double cameraToObjectNorm = forwardDistance * lateralCorrection;
 
-        Pose2d fieldToCamera = fieldToRobot.transformBy(new Transform2d(robotToCamera.toPose2d().toMatrix()));
+        Transform2d robotToCamera2d =
+            new Transform2d(
+                robotToCamera.getTranslation().toTranslation2d(),
+                robotToCamera.getRotation().toRotation2d());
+        Pose2d fieldToCamera = fieldToRobot.transformBy(robotToCamera2d);
         Pose2d fieldToCoral =
             fieldToCamera
                 .transformBy(new Transform2d(Translation2d.kZero, new Rotation2d(-tx)))
                 .transformBy(
                     new Transform2d(new Translation2d(cameraToObjectNorm, 0), Rotation2d.kZero));
         CoralPose coralPose = new CoralPose(fieldToCoral.getTranslation(), observation.timestamp());
         // delete coral once close enough to robot and deletes coral that has existed longer than 10 seconds
-        coralPoses =
-            coralPoses.stream()
-                .filter(
-                    (c) -> c.translation.getDistance(fieldToCoral.getTranslation()) > coralOverlap)
-                .filter((c) -> now - c.timestamp <= coralMaxAge)
-                .collect(Collectors.toSet());
+        coralPoses.removeIf(
+            c -> c.translation.getDistance(fieldToCoral.getTranslation()) <= coralOverlap
+                || now - c.timestamp > coralMaxAge);
         coralPoses.add(coralPose);
     }
 
+    @Override
+    public void periodic() {
+        for (int i = 0; i < inputs.length; i++) {
+            io[i].updateInputs(inputs[i]);
+            Logger.processInputs("ObjectVision/" + io[i].getName(), inputs[i]);
+            allObservations.addAll(Arrays.asList(inputs[i].observations));
+        }
+        allObservations.stream()
+            .filter(o -> calcConfidence(o.info(), o.camIndex()) >= confidenceThreshold.get())
+            .sorted(Comparator.comparingDouble(ObjectVisionIO.ObjectObservation::timestamp))
+            .forEach(this::addCoralObservationToPose);
+
+        coralObjects.setPoses(
+            coralPoses.stream()
+                .map(coral -> new Pose2d(coral.translation, new Rotation2d())) // zero rotation
+                .collect(Collectors.toList()));
+        allObservations.clear();
+    }
+
     public void reset() {
         coralPoses.clear();
     }

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

@@ -1,23 +1,43 @@
 package org.steelhawks.subsystems.vision.objdetect;
 
 import org.littletonrobotics.junction.AutoLog;
+import org.steelhawks.util.LimelightHelpers;
 
 public interface ObjectVisionIO {
 
     record ObjectObservation(
-        String label,
-        double confidence,
+        int camIndex,
+        int classId,
+        DetectionInfo info,
+        double confidence, // TODO if Photon doesnt give you a confidence val just remove this and calc on your own
         double[] tx,
         double[] ty,
         double area,
         double timestamp
     ) {}
 
+    record DetectionInfo(
+        int classId,      // Class of the object
+        double area,      // Target area (e.g., ta from Limelight)
+        double[] tx,      // Corner X positions
+        double[] ty       // Corner Y positions
+    ) {
+        public DetectionInfo(LimelightHelpers.RawDetection raw) {
+            this(
+                raw.classId,
+                raw.ta,
+                new double[] {raw.corner0_X, raw.corner1_X, raw.corner2_X, raw.corner3_X},
+                new double[] {raw.corner0_Y, raw.corner1_Y, raw.corner2_Y, raw.corner3_Y}
+            );
+        }
+    }
+
+
     @AutoLog
     class ObjectVisionIOInputs {
         public boolean connected = false;
         public ObjectObservation latestTargetObservation =
-            new ObjectObservation("", 0.0, new double[0], new double[0], 0.0, 0.0);
+            new ObjectObservation(0, 0, new DetectionInfo(0, 0.0, new double[0], new double[0]), 0.0, new double[0], new double[0], 0.0, 0.0);
         public ObjectObservation[] observations = new ObjectObservation[0];
     }
 

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

@@ -12,14 +12,16 @@
 public class ObjectVisionIOLimelight implements ObjectVisionIO {
 
     private final String name;
+    private final int camIndex;
     private final DoubleSubscriber latencySubscriber;
     private final DoubleSubscriber txSubscriber;
     private final DoubleSubscriber tySubscriber;
     private final DoubleSubscriber taSubscriber; // target area
     private final IntegerSubscriber targetValidSubscriber;
 
-    public ObjectVisionIOLimelight(String name) {
+    public ObjectVisionIOLimelight(String name, int camIndex) {
         this.name = name;
+        this.camIndex = camIndex;
         var table = NetworkTableInstance.getDefault().getTable(name);
         latencySubscriber = table.getDoubleTopic("tl").subscribe(0.0);
         txSubscriber = table.getDoubleTopic("tx").subscribe(0.0);
@@ -38,8 +40,6 @@ public void updateInputs(ObjectVisionIOInputs inputs) {
             LimelightHelpers.getRawDetections(name);
 
         for (LimelightHelpers.RawDetection detection : detections) {
-            String label =
-                detection.classId == 0 ? "coral" : "other";
             // pack corners into tx ty arrays
             double[] tx = {
                 detection.corner0_X,
@@ -53,12 +53,14 @@ public void updateInputs(ObjectVisionIOInputs inputs) {
                 detection.corner2_Y,
                 detection.corner3_Y
             };
-            double confidence = detection.ta; // make an algo later
             double limelightLatencySec = latencySubscriber.get() / 1000.0;
             double timestamp = RobotController.getFPGATime() / 1e6 - limelightLatencySec;
             observations.add(
                 new ObjectObservation(
-                    label, confidence, tx, ty,
+                    camIndex, detection.classId,
+                    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
+                    tx, ty,
                     detection.ta,
                     timestamp // timestamp in seconds
                 )

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

@@ -1,14 +1,21 @@
 package org.steelhawks.subsystems.vision.objdetect;
 
 import edu.wpi.first.math.geometry.Transform3d;
+import org.photonvision.PhotonCamera;
 
 public class ObjectVisionIOPhoton implements ObjectVisionIO {
 
     private final String name;
+    private final int camIndex;
     private final Transform3d robotToCamera;
 
-    public ObjectVisionIOPhoton(String name, Transform3d robotToCamera) {
+    private final PhotonCamera camera;
+
+    public ObjectVisionIOPhoton(String name, int camIndex, Transform3d robotToCamera) {
         this.name = name;
+        this.camIndex = camIndex;
         this.robotToCamera = robotToCamera;
+
+        camera = new PhotonCamera(name);
     }
 }