setup code for trig pose estimation (vision pose estimator cleanup too)

src/main/java/frc/robot/Constants.java

@@ -52,14 +52,14 @@ public static class Ports {
   }
 
   public static class FieldConstants {
-    public static final AprilTagFieldLayout fieldLayout =
+    public static final AprilTagFieldLayout tagLayout =
         AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape);
 
     public static final Translation2d reefCenter =
         new Translation2d(Inches.of(176.75).in(Meters), Inches.of(158.5).in(Meters));
 
     public static final Translation2d fieldCenter =
-        new Translation2d(fieldLayout.getFieldLength() / 2, fieldLayout.getFieldWidth() / 2);
+        new Translation2d(tagLayout.getFieldLength() / 2, tagLayout.getFieldWidth() / 2);
 
     public static final Translation2d humanCenter =
         new Translation2d(Inches.of(47.93).in(Meter), Inches.of(158.28).in(Meters));

src/main/java/frc/robot/subsystems/Swerve.java

@@ -54,6 +54,7 @@
 import frc.robot.utils.VisionPoseEstimator;
 import frc.robot.utils.VisionPoseEstimator.VisionPoseEstimate;
 import java.util.ArrayList;
+import java.util.Arrays;
 import java.util.HashSet;
 import java.util.List;
 import java.util.Optional;
@@ -165,6 +166,8 @@ public class Swerve extends TunerSwerveDrivetrain implements Subsystem, SelfChec
   private final List<VisionPoseEstimate> _acceptedEstimates = new ArrayList<>();
   private final List<VisionPoseEstimate> _rejectedEstimates = new ArrayList<>();
 
+  private final List<VisionPoseEstimate> _allEstimates = new ArrayList<>();
+
   private final Set<Pose3d> _detectedTags = new HashSet<>();
 
   private final VisionSystemSim _visionSystemSim;
@@ -223,7 +226,7 @@ public Swerve(
       startSimThread();
 
       _visionSystemSim = new VisionSystemSim("Vision System Sim");
-      _visionSystemSim.addAprilTags(FieldConstants.fieldLayout);
+      _visionSystemSim.addAprilTags(FieldConstants.tagLayout);
 
       _cameras.forEach(cam -> _visionSystemSim.addCamera(cam.getCameraSim(), cam.robotToCam));
     } else {
@@ -515,6 +518,44 @@ public Command driveTo(Pose2d goalPose) {
         .withName("Drive To");
   }
 
+  /**
+   * Returns the robot's field-relative pose using trig. This estimate uses the side-view and
+   * top-view right triangles formed between the robot and the specified tag to estimate the robot's
+   * pose. For this to work, a tag must by in view of one of the {@link VisionPoseEstimator}s.
+   *
+   * @param tagId The tag id to use for the trig estimate.
+   * @return The trig estimate (which also gets latency compensated). If the tag doesn't exist or
+   *     isn't in view of any of the cameras, the normal pose estimator pose is returned as a
+   *     default.
+   */
+  public Pose2d getTrigPose(int tagId) {
+    if (FieldConstants.tagLayout.getTagPose(tagId).isEmpty()) return getPose();
+
+    VisionPoseEstimate[] tagIsVisible =
+        _allEstimates.stream()
+            .filter(e -> Arrays.asList(e.detectedTags()).contains((Object) tagId))
+            .toArray(VisionPoseEstimate[]::new);
+
+    if (tagIsVisible.length == 0) return getPose();
+
+    VisionPoseEstimate visionEstimate = tagIsVisible[0];
+
+    Pose3d tagPose = FieldConstants.tagLayout.getTagPose(tagId).get();
+
+    double distance = visionEstimate.avgTagDistance();
+
+    double tx = visionEstimate.tx();
+    double ty = visionEstimate.ty();
+
+    // TODO: trig over here using the above values
+    Pose2d trigPose = getPose();
+
+    Pose2d oldPose =
+        samplePoseAt(Utils.fpgaToCurrentTime(visionEstimate.timestamp())).orElse(getPose());
+
+    return trigPose.transformBy(getPose().minus(oldPose));
+  }
+
   /** Wrapper for getting estimated pose. */
   public Pose2d getPose() {
     return getState().Pose;
@@ -543,6 +584,8 @@ private void updateVisionPoseEstimates() {
     _acceptedEstimates.clear();
     _rejectedEstimates.clear();
 
+    _allEstimates.clear();
+
     _detectedTags.clear();
 
     for (VisionPoseEstimator cam : _cameras) {
@@ -555,13 +598,16 @@ private void updateVisionPoseEstimates() {
           (estimate) -> {
             // add all detected tag poses
             for (int id : estimate.detectedTags()) {
-              FieldConstants.fieldLayout.getTagPose(id).ifPresent(pose -> _detectedTags.add(pose));
+              FieldConstants.tagLayout.getTagPose(id).ifPresent(pose -> _detectedTags.add(pose));
             }
 
             // add robot poses to their corresponding arrays
             if (estimate.isValid()) _acceptedEstimates.add(estimate);
             else _rejectedEstimates.add(estimate);
           });
+
+      _allEstimates.addAll(_acceptedEstimates);
+      _allEstimates.addAll(_rejectedEstimates);
     }
   }
 

src/main/java/frc/robot/utils/VisionPoseEstimator.java

@@ -117,7 +117,7 @@ public record VisionPoseEstimate(
       /** The detected tag ids in this measurement. */
       int[] detectedTags,
 
-      /** The average distance from the tag(s) (-1 when no tags). */
+      /** The average distance from the tag(s) in 3D space (-1 when no tags). */
       double avgTagDistance,
 
       /**
@@ -127,7 +127,13 @@ public record VisionPoseEstimate(
       double[] stdDevs,
 
       /** Whether this estimate passed the filter or not. */
-      boolean isValid) {
+      boolean isValid,
+
+      /** The yaw to the center of the tag when there is one tag. */
+      double tx,
+
+      /** The pitch to the center of the tag when there is one tag. */
+      double ty) {
     /**
      * Used for sorting a list of vision pose estimates, first the timestamps are sorted (from
      * smallest to highest), then the standard deviations at the same timestamp are sorted if
@@ -164,7 +170,7 @@ public record VisionPoseEstimate(
   public static VisionPoseEstimator buildFromConstants(
       VisionPoseEstimatorConstants camConstants, Function<Double, Rotation2d> gyroAtTime) {
     return buildFromConstants(
-        camConstants, NetworkTableInstance.getDefault(), FieldConstants.fieldLayout, gyroAtTime);
+        camConstants, NetworkTableInstance.getDefault(), FieldConstants.tagLayout, gyroAtTime);
   }
 
   /**
@@ -250,6 +256,8 @@ private void logNewEstimate(VisionPoseEstimate estimate) {
     DogLog.log(_estimateLogPath + "Average Tag Distance", estimate.avgTagDistance);
     DogLog.log(_estimateLogPath + "Std Devs", estimate.stdDevs);
     DogLog.log(_estimateLogPath + "Is Valid", estimate.isValid);
+    DogLog.log(_estimateLogPath + "TX", estimate.tx);
+    DogLog.log(_estimateLogPath + "TY", estimate.ty);
   }
 
   /**
@@ -273,6 +281,8 @@ private VisionPoseEstimate processEstimate(EstimatedRobotPose estimate, Rotation
     double avgTagDistance = 0;
     double[] stdDevs = new double[] {-1, -1, -1};
     boolean isValid = false;
+    double tx = -10000;
+    double ty = -10000;
 
     estimate.targetsUsed.forEach(
         t -> t.getDetectedCorners().forEach(c -> detectedCorners.add(new Translation2d(c.x, c.y))));
@@ -286,6 +296,9 @@ private VisionPoseEstimate processEstimate(EstimatedRobotPose estimate, Rotation
 
       ambiguity = target.getPoseAmbiguity();
 
+      tx = target.getYaw();
+      ty = target.getPitch();
+
       Pose3d betterReprojPose = tagPose.transformBy(target.getBestCameraToTarget().inverse());
       Pose3d worseReprojPose = tagPose.transformBy(target.getAlternateCameraToTarget().inverse());
 
@@ -360,7 +373,9 @@ private VisionPoseEstimate processEstimate(EstimatedRobotPose estimate, Rotation
         detectedTags,
         avgTagDistance,
         stdDevs,
-        isValid);
+        isValid,
+        tx,
+        ty);
   }
 
   /** Reads from the camera and generates an array of new latest {@link VisionPoseEstimate}(s). */

src/test/java/frc/robot/VisionPoseEstimatorTest.java

@@ -323,7 +323,9 @@ public void estimateSort() {
               new int[] {1},
               1.2,
               new double[] {0.3, 0.1, 0.2},
-              true));
+              true,
+              -1000,
+              -1000));
     }
 
     newEstimates.add(
@@ -339,7 +341,9 @@ public void estimateSort() {
               4, 3, 5
             }, // these are worse std devs, so it should come before the better estimate @ timestamp
             // = 2s
-            true));
+            true,
+            -1000,
+            -1000));
 
     newEstimates.sort(VisionPoseEstimate.sorter);