From 65b3a156f1d6abf9d392da3aaa2791e7ef7d1e19 Mon Sep 17 00:00:00 2001
From: Mason Vogt <mvog2501@mpsedu.org>
Date: Sat, 13 Apr 2024 10:19:25 -0500
Subject: [PATCH] [Robot] Adjust camera trust

---
 .../robot/subsystems/swerve/SwerveEstimator.java      |  8 +++++---
 .../robot/subsystems/tagtracker/TagTrackerCamera.java |  8 +++++++-
 .../robot/subsystems/tagtracker/TagTrackerInput.java  | 11 +++++++++--
 3 files changed, 21 insertions(+), 6 deletions(-)

diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveEstimator.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveEstimator.java
index 070d161..fb442ee 100644
--- a/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveEstimator.java
+++ b/Robot/src/main/java/com/swrobotics/robot/subsystems/swerve/SwerveEstimator.java
@@ -24,7 +24,7 @@
 
 public final class SwerveEstimator {
     //private static final double[] STATE_STD_DEVS = {0.003, 0.003, 0.0002};
-    private static final double[] STATE_STD_DEVS = {0.005, 0.005, 0.01};
+    private static final double[] STATE_STD_DEVS = {0.005, 0.005, 0.001};
     private static final double HISTORY_TIME = 0.3;
 
     private static final double INITIAL_ANGLE_STDDEV = 0.2; // Really trust it for beginning of match
@@ -61,7 +61,8 @@ public SwerveEstimator(FieldInfo field) {
                                     // Compensate for mounting position
                                     .transformBy(new Transform3d(new Translation3d(halfFrameL - 0.046, -halfFrameW + 0.12, 0.19).unaryMinus(), new Rotation3d()));
                         },
-                        captureProps),
+                        captureProps,
+                        5), // meters
 
                 new TagTrackerInput.CameraInfo(
                         "zoom",
@@ -72,7 +73,8 @@ public SwerveEstimator(FieldInfo field) {
                                     // Compensate for mounting position
                                     .transformBy(new Transform3d(new Translation3d(halfFrameL - 0.16, halfFrameW - 0.133, 0.39).unaryMinus(), new Rotation3d()));
                         },
-                        captureProps)
+                        captureProps,
+                        Double.POSITIVE_INFINITY) // Trust at all distances
         );
 
         latestPose = new Pose2d();
diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/tagtracker/TagTrackerCamera.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/tagtracker/TagTrackerCamera.java
index 7b87824..c87e80e 100644
--- a/Robot/src/main/java/com/swrobotics/robot/subsystems/tagtracker/TagTrackerCamera.java
+++ b/Robot/src/main/java/com/swrobotics/robot/subsystems/tagtracker/TagTrackerCamera.java
@@ -89,14 +89,16 @@ public String toString() {
     private final String name;
 //    private final Transform3d toRobotTransform;
     private final Function<Pose3d, Pose3d> cameraToRobot;
+    private final double maxTrustDistance;
 
     private final TagTrackerCameraIO io;
     private final TagTrackerCameraIO.Inputs inputs;
 
-    public TagTrackerCamera(String name, NetworkTable table, Function<Pose3d, Pose3d> cameraToRobot, CameraCaptureProperties captureProps) {
+    public TagTrackerCamera(String name, NetworkTable table, Function<Pose3d, Pose3d> cameraToRobot, CameraCaptureProperties captureProps, double maxTrustDistance) {
         this.name = name;
 //        this.toRobotTransform = toRobotTransform;
         this.cameraToRobot = cameraToRobot;
+        this.maxTrustDistance = maxTrustDistance;
 
         io = new NTCameraIO(table);
         inputs = new TagTrackerCameraIO.Inputs();
@@ -108,6 +110,10 @@ public Function<Pose3d, Pose3d> getToRobotTransform() {
         return cameraToRobot;
     }
 
+    public double getMaxTrustDistance() {
+        return maxTrustDistance;
+    }
+
     public List<EstimateInput> getEstimates() {
         io.updateInputs(inputs);
         Logger.processInputs("TagTracker/Camera/" + name, inputs);
diff --git a/Robot/src/main/java/com/swrobotics/robot/subsystems/tagtracker/TagTrackerInput.java b/Robot/src/main/java/com/swrobotics/robot/subsystems/tagtracker/TagTrackerInput.java
index fbf40a1..5e4f15e 100644
--- a/Robot/src/main/java/com/swrobotics/robot/subsystems/tagtracker/TagTrackerInput.java
+++ b/Robot/src/main/java/com/swrobotics/robot/subsystems/tagtracker/TagTrackerInput.java
@@ -20,13 +20,15 @@ public static final class CameraInfo {
         public final String name;
         public final Function<Pose3d, Pose3d> cameraToRobot;
         public final CameraCaptureProperties captureProps;
+        public final double maxTrustDistance;
 //        public final Pose3d robotRelPose;
 
-        public CameraInfo(String name, Function<Pose3d, Pose3d> cameraToRobot, CameraCaptureProperties captureProps) {
+        public CameraInfo(String name, Function<Pose3d, Pose3d> cameraToRobot, CameraCaptureProperties captureProps, double maxTrustDistance) {
             this.name = name;
             this.cameraToRobot = cameraToRobot;
 //            this.robotRelPose = robotRelPose;
             this.captureProps = captureProps;
+            this.maxTrustDistance = maxTrustDistance;
         }
     }
 
@@ -66,7 +68,8 @@ public TagTrackerInput(FieldInfo field, CameraInfo... infos) {
                     info.name,
                     camerasTable.getSubTable(info.name),
                     info.cameraToRobot,
-                    info.captureProps);
+                    info.captureProps,
+                    info.maxTrustDistance);
         }
     }
 
@@ -130,6 +133,10 @@ else if (errB < errA * AMBIGUITY_THRESHOLD)
                 }
                 double avgTagDist = totalTagDist / tagCount;
 
+                // Ignore if too far away for camera to be good
+                if (avgTagDist > camera.getMaxTrustDistance())
+                    continue;
+
                 // Trust farther away tags less and frames with more tags more
                 double xyStdDev = XY_STD_DEV_COEFF * MathUtil.square(avgTagDist) / tagCount;
                 double thetaStdDev = THETA_STD_DEV_COEFF * MathUtil.square(avgTagDist) / tagCount;