[FEATURE]: Add 6328 single-tag pose estimation

src/main/java/frc/robot/RobotContainer.java

@@ -58,6 +58,7 @@ public class RobotContainer {
           (drivetrain instanceof DrivetrainSim)
               ? ((DrivetrainSim) drivetrain)::getActualPose
               : drivetrain::getPose,
+          () -> drivetrain.getPose().getRotation(),
           visionEst ->
               drivetrain.addVisionMeasurement(
                   visionEst.estimate().estimatedPose.toPose2d(),

src/main/java/frc/robot/subsystems/vision/Camera.java

@@ -5,11 +5,14 @@
 import edu.wpi.first.epilogue.NotLogged;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.wpilibj.Timer;
 import frc.robot.RobotConstants;
 import frc.robot.subsystems.vision.VisionConstants.CameraConfig;
+import java.util.function.Supplier;
 import org.photonvision.EstimatedRobotPose;
 import org.photonvision.PhotonCamera;
 import org.photonvision.PhotonPoseEstimator;
@@ -24,27 +27,52 @@ public class Camera {
 
   private final PhotonCamera camera;
 
-  private final PhotonPoseEstimator poseEstimator;
+  private final Supplier<Rotation2d> robotHeadingSupplier;
+
+  private final PhotonPoseEstimator multiTagEstimator;
+
+  private final PhotonPoseEstimator singleTagEstimator;
 
   // for logging
-  private VisionEstimate latestValidEstimate;
+  private VisionEstimate latestMultiTagEstimate;
 
-  public Camera(CameraConfig config, PhotonCamera camera) {
+  private VisionEstimate latestSingleTagEstimate;
+
+  public Camera(
+      CameraConfig config, PhotonCamera camera, Supplier<Rotation2d> robotHeadingSupplier) {
     this.name = config.cameraName();
 
     this.usage = config.usage();
 
     this.camera = camera;
 
-    this.poseEstimator =
+    this.robotHeadingSupplier = robotHeadingSupplier;
+
+    this.multiTagEstimator =
         new PhotonPoseEstimator(
             RobotConstants.kAprilTagFieldLayout,
             PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR,
             config.robotToCamera());
+
+    this.singleTagEstimator =
+        new PhotonPoseEstimator(
+            RobotConstants.kAprilTagFieldLayout,
+            PoseStrategy.PNP_DISTANCE_TRIG_SOLVE,
+            config.robotToCamera());
+  }
+
+  public VisionEstimate getLatestMultiTagEstimate() {
+    return latestMultiTagEstimate;
+  }
+
+  public VisionEstimate getLatestSingleTagEstimate() {
+    return latestSingleTagEstimate;
   }
 
   @NotLogged
   public VisionEstimate tryLatestEstimate() {
+    singleTagEstimator.addHeadingData(Timer.getFPGATimestamp(), robotHeadingSupplier.get());
+
     if (!camera.isConnected()) return null;
 
     final var unreadResults = camera.getAllUnreadResults();
@@ -55,27 +83,63 @@ public VisionEstimate tryLatestEstimate() {
 
     if (!latestResult.hasTargets()) return null;
 
-    final var estimate = poseEstimator.update(latestResult);
-
-    return estimate
-        .filter(
-            poseEst ->
-                VisionConstants.kAllowedFieldArea.contains(
-                    poseEst.estimatedPose.getTranslation().toTranslation2d()))
-        .map(
-            photonEst -> {
-              final var visionEst =
-                  new VisionEstimate(photonEst, calculateStdDevs(photonEst), name, usage);
-              latestValidEstimate = visionEst;
-              return visionEst;
-            })
-        .orElse(null);
+    final var multiTagEstimate =
+        multiTagEstimator
+            .update(latestResult)
+            .filter(
+                poseEst ->
+                    VisionConstants.kAllowedFieldArea.contains(
+                        poseEst.estimatedPose.getTranslation().toTranslation2d()))
+            .map(
+                photonEst -> {
+                  final var visionEst =
+                      new VisionEstimate(
+                          photonEst,
+                          calculateStdDevs(photonEst, EstimateType.MULTI_TAG),
+                          name,
+                          usage,
+                          EstimateType.MULTI_TAG);
+
+                  latestMultiTagEstimate = visionEst;
+
+                  return visionEst;
+                })
+            .orElse(null);
+
+    final var singleTagEstimate =
+        singleTagEstimator
+            .update(latestResult)
+            .filter(
+                poseEst ->
+                    VisionConstants.kAllowedFieldArea.contains(
+                        poseEst.estimatedPose.getTranslation().toTranslation2d()))
+            .map(
+                photonEst -> {
+                  final var visionEst =
+                      new VisionEstimate(
+                          photonEst,
+                          calculateStdDevs(photonEst, EstimateType.SINGLE_TAG),
+                          name,
+                          usage,
+                          EstimateType.SINGLE_TAG);
+
+                  latestSingleTagEstimate = visionEst;
+
+                  return visionEst;
+                })
+            .orElse(null);
+
+    return switch (latestResult.targets.size()) {
+      case 1 -> singleTagEstimate;
+      default -> multiTagEstimate;
+    };
   }
 
   // could be absolute nonsense, open to tuning constants for each robot camera config
   // assumes `result` has targets
   @NotLogged
-  private Matrix<N3, N1> calculateStdDevs(EstimatedRobotPose visionPoseEstimate) {
+  private Matrix<N3, N1> calculateStdDevs(
+      EstimatedRobotPose visionPoseEstimate, EstimateType estimateType) {
     // weighted average by ambiguity
     final double avgTargetDistance =
         visionPoseEstimate.targetsUsed.stream()
@@ -91,12 +155,17 @@ private Matrix<N3, N1> calculateStdDevs(EstimatedRobotPose visionPoseEstimate) {
                 .reduce(0.0, Double::sum)
             / (visionPoseEstimate.targetsUsed.size());
 
+    final double estimateTypeMultiplier =
+        (estimateType == EstimateType.SINGLE_TAG) ? VisionConstants.kSingleTagStdDevMultiplier : 1;
+
     final double translationStdDev =
-        VisionConstants.kTranslationStdDevCoeff
+        estimateTypeMultiplier
+            * VisionConstants.kTranslationStdDevCoeff
             * Math.pow(avgTargetDistance, 3)
             / Math.pow(visionPoseEstimate.targetsUsed.size(), 3);
     final double rotationStdDev =
-        VisionConstants.kRotationStdDevCoeff
+        estimateTypeMultiplier
+            * VisionConstants.kRotationStdDevCoeff
             * Math.pow(avgTargetDistance, 3)
             / Math.pow(visionPoseEstimate.targetsUsed.size(), 3);
 

src/main/java/frc/robot/subsystems/vision/EstimateType.java

@@ -0,0 +1,10 @@
+/* (C) Robolancers 2025 */
+package frc.robot.subsystems.vision;
+
+import edu.wpi.first.epilogue.Logged;
+
+@Logged
+public enum EstimateType {
+  SINGLE_TAG,
+  MULTI_TAG
+}

src/main/java/frc/robot/subsystems/vision/Vision.java

@@ -3,6 +3,7 @@
 
 import edu.wpi.first.epilogue.Logged;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.RobotBase;
 import frc.robot.util.VirtualSubsystem;
 import java.util.function.Consumer;
@@ -17,15 +18,17 @@ public class Vision extends VirtualSubsystem {
 
   public static Vision create(
       Supplier<Pose2d> robotPoseSupplier,
+      Supplier<Rotation2d> robotHeadingSupplier,
       Consumer<VisionEstimate> visionDataConsumer,
       Consumer<VisionEstimate> reefVisionDataConsumer) {
     return RobotBase.isReal()
         ? new Vision(
-            new VisionIOReal(VisionConstants.kCameraConfigs),
+            new VisionIOReal(robotHeadingSupplier, VisionConstants.kCameraConfigs),
             visionDataConsumer,
             reefVisionDataConsumer)
         : new Vision(
-            new VisionIOSim(robotPoseSupplier, VisionConstants.kCameraConfigs),
+            new VisionIOSim(
+                robotPoseSupplier, robotHeadingSupplier, VisionConstants.kCameraConfigs),
             visionDataConsumer,
             reefVisionDataConsumer);
   }
@@ -44,11 +47,13 @@ public void periodic() {
     final var latestEstimates = io.getLatestEstimates();
 
     for (final var est : latestEstimates) {
-      visionDataConsumer.accept(est);
-
-      if (est.sourceType() == CameraUsage.REEF) {
-        reefVisionDataConsumer.accept(est);
+      switch (est.estimateType()) {
+        case MULTI_TAG -> visionDataConsumer.accept(est);
+        case SINGLE_TAG -> { // Java 21 when clauses would be nice
+          if (est.sourceType() == CameraUsage.REEF) reefVisionDataConsumer.accept(est);
+        }
       }
+      ;
     }
   }
 }

src/main/java/frc/robot/subsystems/vision/VisionConstants.java

@@ -21,6 +21,9 @@ public class VisionConstants {
   public static final double kTranslationStdDevCoeff = 1e-1;
   public static final double kRotationStdDevCoeff = 1e-1;
 
+  // TODO: tune in sim, represents (to some extent) how much more single tag estimates are trusted
+  public static final double kSingleTagStdDevMultiplier = 1e-2;
+
   public static record CameraCalibration(
       int resolutionWidth,
       int resolutionHeight,

src/main/java/frc/robot/subsystems/vision/VisionEstimate.java

@@ -15,4 +15,5 @@ public record VisionEstimate(
     EstimatedRobotPose estimate,
     Matrix<N3, N1> stdDevs,
     String sourceName,
-    CameraUsage sourceType) {}
+    CameraUsage sourceType,
+    EstimateType estimateType) {}

src/main/java/frc/robot/subsystems/vision/VisionIOReal.java

@@ -2,20 +2,24 @@
 package frc.robot.subsystems.vision;
 
 import edu.wpi.first.epilogue.Logged;
+import edu.wpi.first.math.geometry.Rotation2d;
 import frc.robot.subsystems.vision.VisionConstants.CameraConfig;
 import java.util.List;
 import java.util.Objects;
+import java.util.function.Supplier;
 import java.util.stream.Stream;
 import org.photonvision.PhotonCamera;
 
 @Logged
 public class VisionIOReal implements VisionIO {
   private final List<Camera> cameras;
 
-  public VisionIOReal(CameraConfig... configs) {
+  public VisionIOReal(Supplier<Rotation2d> robotHeadingSupplier, CameraConfig... configs) {
     cameras =
         Stream.of(configs)
-            .map(config -> new Camera(config, new PhotonCamera(config.cameraName())))
+            .map(
+                config ->
+                    new Camera(config, new PhotonCamera(config.cameraName()), robotHeadingSupplier))
             .toList();
   }
 

src/main/java/frc/robot/subsystems/vision/VisionIOSim.java

@@ -4,6 +4,7 @@
 import edu.wpi.first.epilogue.Logged;
 import edu.wpi.first.epilogue.NotLogged;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import frc.robot.RobotConstants;
 import frc.robot.subsystems.vision.VisionConstants.CameraConfig;
 import java.util.ArrayList;
@@ -26,7 +27,10 @@ public class VisionIOSim implements VisionIO {
 
   @NotLogged private final List<VisionTargetSim> targets;
 
-  public VisionIOSim(Supplier<Pose2d> robotPoseSupplier, CameraConfig... configs) {
+  public VisionIOSim(
+      Supplier<Pose2d> robotPoseSupplier,
+      Supplier<Rotation2d> robotHeadingSupplier,
+      CameraConfig... configs) {
     this.sim = new VisionSystemSim("main");
     sim.addAprilTags(RobotConstants.kAprilTagFieldLayout);
 
@@ -37,7 +41,7 @@ public VisionIOSim(Supplier<Pose2d> robotPoseSupplier, CameraConfig... configs)
                   final var camera = new PhotonCamera(config.cameraName());
                   final var cameraSim = new PhotonCameraSim(camera, config.calib().simProperties());
                   sim.addCamera(cameraSim, config.robotToCamera());
-                  return new Camera(config, camera);
+                  return new Camera(config, camera, robotHeadingSupplier);
                 })
             .toList();