vision performance tweak

MillenniumFalcons · Feb 15, 2024 · f8b2ce1 · f8b2ce1
1 parent d23a4b3
commit f8b2ce1
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 4 deletions.
diff --git a/src/main/java/team3647/frc2024/util/AprilTagPhotonVision.java b/src/main/java/team3647/frc2024/util/AprilTagPhotonVision.java
@@ -46,13 +46,17 @@ public static AprilTagId getId(int id) {
 
     public Optional<VisionMeasurement> QueueToInputs() {
         var update = photonPoseEstimator.update();
+        var result = this.getLatestResult();
         if (update.isEmpty()) {
             return Optional.empty();
         }
-        var visionPose = update.get().estimatedPose.toPose2d();
-        final var stdDevs =
-                VecBuilder.fill(0.05, 0.05, 0.05)
-                        .times(GeomUtil.distance(drivePose.get(), visionPose));
+        var targetDistance =
+                GeomUtil.distanceSquared(
+                        result.getBestTarget()
+                                .getBestCameraToTarget()
+                                .getTranslation()
+                                .toTranslation2d());
+        final var stdDevs = VecBuilder.fill(0.03, 0.03, 0.03).times(targetDistance);
         VisionMeasurement measurement =
                 VisionMeasurement.fromEstimatedRobotPose(update.get(), stdDevs);
         return Optional.of(measurement);

diff --git a/src/main/java/team3647/lib/GeomUtil.java b/src/main/java/team3647/lib/GeomUtil.java
@@ -2,16 +2,32 @@
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
 
 public class GeomUtil {
     public static double distance(Pose2d pose1, Pose2d pose2) {
         final var pose = pose1.minus(pose2);
         return Math.sqrt(Math.pow(pose.getX(), 2) + Math.pow(pose.getY(), 2));
     }
 
+    public static double distanceSquared(Pose2d pose1, Pose2d pose2) {
+        final var pose = pose1.minus(pose2);
+        return Math.pow(pose.getX(), 2) + Math.pow(pose.getY(), 2);
+    }
+
     public static double distance(Transform2d transform) {
         var distancSquared = Math.pow(transform.getX(), 2) + Math.pow(transform.getY(), 2);
         var distance = Math.sqrt(distancSquared);
         return distance;
     }
+
+    public static double distanceSquared(Transform2d transform) {
+        var distancSquared = Math.pow(transform.getX(), 2) + Math.pow(transform.getY(), 2);
+        return distancSquared;
+    }
+
+    public static double distanceSquared(Translation2d transform) {
+        var distancSquared = Math.pow(transform.getX(), 2) + Math.pow(transform.getY(), 2);
+        return distancSquared;
+    }
 }