Roll-forward vision

src/main/java/com/team2813/RobotContainer.java

@@ -7,7 +7,7 @@
 
 package com.team2813;
 
-import static com.team2813.subsystems.vision.VisionConstants.aprilTagLayout;
+import static com.team2813.subsystems.vision.VisionConstants.APRIL_TAG_LAYOUT;
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
@@ -110,6 +110,7 @@ public RobotContainer(
         vision =
             new Vision(
                 drive::addVisionMeasurement,
+                () -> {},
                 new VisionIOPhotonVision(
                     VisionConstants.RED_BACK_LEFT_COLOR_CAMERA_NAME,
                     VisionConstants.RED_BACK_LEFT_CAM_FROM_ROBOT),
@@ -141,23 +142,24 @@ public RobotContainer(
         hopper = new Hopper(new HopperIOSim());
 
         VisionSystemSim visionSim = new VisionSystemSim("main");
-        visionSim.addAprilTags(aprilTagLayout);
+        visionSim.addAprilTags(APRIL_TAG_LAYOUT);
 
         vision =
             new Vision(
                 drive::addVisionMeasurement,
+                () -> visionSim.update(drive.getPose()),
                 new VisionIOPhotonVisionSim(
                     VisionConstants.RED_BACK_LEFT_COLOR_CAMERA_NAME,
                     VisionConstants.RED_BACK_LEFT_CAM_FROM_ROBOT,
-                    drive::getPose),
+                    visionSim),
                 new VisionIOPhotonVisionSim(
                     VisionConstants.GREEN_BACK_RIGHT_COLOR_CAMERA_NAME,
                     VisionConstants.GREEN_BACK_RIGHT_CAM_FROM_ROBOT,
-                    drive::getPose),
+                    visionSim),
                 new VisionIOPhotonVisionSim(
                     VisionConstants.BLUE_FRONT_MONO_CAMERA_NAME,
                     VisionConstants.BLUE_FRONT_CAM_FROM_ROBOT,
-                    drive::getPose));
+                    visionSim));
         intakeExtension = new IntakeExtension(new IntakeExtensionIOSim());
         intakeRoller = new IntakeRoller(new IntakeRollerIOSim());
 
@@ -182,6 +184,7 @@ public RobotContainer(
         vision =
             new Vision(
                 drive::addVisionMeasurement,
+                () -> {},
                 new VisionIO() {},
                 new VisionIO() {},
                 new VisionIO() {});

src/main/java/com/team2813/subsystems/vision/Vision.java

@@ -7,9 +7,6 @@
 
 package com.team2813.subsystems.vision;
 
-import static com.team2813.subsystems.vision.VisionConstants.*;
-
-import com.team2813.subsystems.vision.VisionIO.PoseObservationType;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -29,13 +26,12 @@ public class Vision extends SubsystemBase {
   private final VisionIO[] io;
   private final VisionIOInputsAutoLogged[] inputs;
   private final Alert[] disconnectedAlerts;
+  private final Runnable updateVSim;
 
-  // This allows us to disable vision at will in case it is causing issues.
-  private boolean visionEnabled = true;
-
-  public Vision(VisionConsumer consumer, VisionIO... io) {
+  public Vision(VisionConsumer consumer, Runnable updateVSim, VisionIO... io) {
     this.consumer = consumer;
     this.io = io;
+    this.updateVSim = updateVSim;
 
     // Initialize inputs
     this.inputs = new VisionIOInputsAutoLogged[io.length];
@@ -47,8 +43,7 @@ public Vision(VisionConsumer consumer, VisionIO... io) {
     this.disconnectedAlerts = new Alert[io.length];
     for (int i = 0; i < inputs.length; i++) {
       disconnectedAlerts[i] =
-          new Alert(
-              "Vision camera " + Integer.toString(i) + " is disconnected.", AlertType.kWarning);
+          new Alert("Vision camera " + i + " is disconnected.", AlertType.kWarning);
     }
   }
 
@@ -63,11 +58,9 @@ public Rotation2d getTargetX(int cameraIndex) {
 
   @Override
   public void periodic() {
-    visionEnabled = VisionConstants.isVisionEnabled();
-
     for (int i = 0; i < io.length; i++) {
       io[i].updateInputs(inputs[i]);
-      Logger.processInputs("Vision/Camera" + Integer.toString(i), inputs[i]);
+      Logger.processInputs("Vision/Camera" + i, inputs[i]);
     }
 
     // Initialize logging values
@@ -77,6 +70,7 @@ public void periodic() {
     List<Pose3d> allRobotPosesRejected = new LinkedList<>();
 
     // Loop over cameras
+    // TODO: merge the two for loops into one loop, in a future PR
     for (int cameraIndex = 0; cameraIndex < io.length; cameraIndex++) {
       // Update disconnected alert
       disconnectedAlerts[cameraIndex].set(!inputs[cameraIndex].connected);
@@ -89,76 +83,29 @@ public void periodic() {
 
       // Add tag poses
       for (int tagId : inputs[cameraIndex].tagIds) {
-        var tagPose = aprilTagLayout.getTagPose(tagId);
-        if (tagPose.isPresent()) {
-          tagPoses.add(tagPose.get());
-        }
+        var tagPose = VisionConstants.APRIL_TAG_LAYOUT.getTagPose(tagId);
+        tagPose.ifPresent(tagPoses::add);
       }
 
-      // Loop over pose observations
-      for (var observation : inputs[cameraIndex].poseObservations) {
-        // Check whether to reject pose
-        boolean rejectPose =
-            observation.tagCount() == 0 // Must have at least one tag
-                || (observation.tagCount() == 1
-                    && observation.ambiguity() > maxAmbiguity) // Cannot be high ambiguity
-                || Math.abs(observation.pose().getZ())
-                    > maxZError // Must have realistic Z coordinate
-
-                // Must be within the field boundaries
-                || observation.pose().getX() < 0.0
-                || observation.pose().getX() > aprilTagLayout.getFieldLength()
-                || observation.pose().getY() < 0.0
-                || observation.pose().getY() > aprilTagLayout.getFieldWidth();
-
-        // Add pose to log
-        robotPoses.add(observation.pose());
-        if (rejectPose) {
-          robotPosesRejected.add(observation.pose());
-        } else {
-          robotPosesAccepted.add(observation.pose());
-        }
-
-        // Skip if rejected
-        if (rejectPose) {
-          continue;
-        }
-
-        // Calculate standard deviations
-        double stdDevFactor =
-            Math.pow(observation.averageTagDistance(), 2.0) / observation.tagCount();
-        double linearStdDev = linearStdDevBaseline * stdDevFactor;
-        double angularStdDev = angularStdDevBaseline * stdDevFactor;
-        if (observation.type() == PoseObservationType.MEGATAG_2) {
-          linearStdDev *= linearStdDevMegatag2Factor;
-          angularStdDev *= angularStdDevMegatag2Factor;
-        }
-        if (cameraIndex < cameraStdDevFactors.length) {
-          linearStdDev *= cameraStdDevFactors[cameraIndex];
-          angularStdDev *= cameraStdDevFactors[cameraIndex];
-        }
-
-        if (visionEnabled) {
-          // Send vision observation
-          consumer.accept(
-              observation.pose().toPose2d(),
-              observation.timestamp(),
-              VecBuilder.fill(linearStdDev, linearStdDev, angularStdDev));
-        }
-      }
+      // Process observations, sending accepted poses to consumer and adding all poses to log
+      processObservations(
+          cameraIndex,
+          inputs[cameraIndex].poseObservations,
+          robotPoses,
+          robotPosesAccepted,
+          robotPosesRejected,
+          consumer);
 
       // Log camera metadata
       Logger.recordOutput(
-          "Vision/Camera" + Integer.toString(cameraIndex) + "/TagPoses",
-          tagPoses.toArray(new Pose3d[0]));
+          "Vision/Camera" + cameraIndex + "/TagPoses", tagPoses.toArray(new Pose3d[0]));
       Logger.recordOutput(
-          "Vision/Camera" + Integer.toString(cameraIndex) + "/RobotPoses",
-          robotPoses.toArray(new Pose3d[0]));
+          "Vision/Camera" + cameraIndex + "/RobotPoses", robotPoses.toArray(new Pose3d[0]));
       Logger.recordOutput(
-          "Vision/Camera" + Integer.toString(cameraIndex) + "/RobotPosesAccepted",
+          "Vision/Camera" + cameraIndex + "/RobotPosesAccepted",
           robotPosesAccepted.toArray(new Pose3d[0]));
       Logger.recordOutput(
-          "Vision/Camera" + Integer.toString(cameraIndex) + "/RobotPosesRejected",
+          "Vision/Camera" + cameraIndex + "/RobotPosesRejected",
           robotPosesRejected.toArray(new Pose3d[0]));
       allTagPoses.addAll(tagPoses);
       allRobotPoses.addAll(robotPoses);
@@ -176,10 +123,91 @@ public void periodic() {
   }
 
   @FunctionalInterface
-  public static interface VisionConsumer {
-    public void accept(
+  public interface VisionConsumer {
+    void accept(
         Pose2d visionRobotPoseMeters,
         double timestampSeconds,
         Matrix<N3, N1> visionMeasurementStdDevs);
   }
+
+  /**
+   * Processes the observations from each camera, sending accepted observations to the consumer and
+   * logging all observations.
+   *
+   * @param cameraIndex - the index of the camera to process observations for
+   * @param observations - the list of pose observations from the selected camera
+   * @param robotPoses - the list of robot poses to add all observations from selected camera to
+   * @param robotPosesAccepted - the list of robot poses to add accepted observations from selected
+   *     camera to
+   * @param robotPosesRejected - the list of robot poses to add rejected observations from selected
+   *     camera to
+   */
+  private static void processObservations(
+      int cameraIndex,
+      VisionIO.PoseObservation[] observations,
+      List<Pose3d> robotPoses,
+      List<Pose3d> robotPosesAccepted,
+      List<Pose3d> robotPosesRejected,
+      VisionConsumer consumer) {
+    // Loop over pose observations
+    for (var observation : observations) {
+      // Check whether to reject pose
+      boolean rejectPose = shouldRejectPose(observation);
+
+      // Add pose to log
+      robotPoses.add(observation.pose());
+      if (rejectPose) {
+        robotPosesRejected.add(observation.pose());
+      } else {
+        robotPosesAccepted.add(observation.pose());
+      }
+
+      // Skip if rejected
+      if (rejectPose) {
+        continue;
+      }
+
+      // Calculate standard deviations
+      double stdDevFactor =
+          Math.pow(observation.averageTagDistance(), 2.0) / observation.tagCount();
+      double linearStdDev = VisionConstants.LINEAR_STD_DEV_BASELINE * stdDevFactor;
+      double angularStdDev = VisionConstants.ANGULAR_STD_DEV_BASELINE * stdDevFactor;
+      if (cameraIndex < VisionConstants.CAMERA_STD_DEV_FACTORS.length) {
+        linearStdDev *= VisionConstants.CAMERA_STD_DEV_FACTORS[cameraIndex];
+        angularStdDev *= VisionConstants.CAMERA_STD_DEV_FACTORS[cameraIndex];
+      }
+
+      // Send vision observation
+      consumer.accept(
+          observation.pose().toPose2d(),
+          observation.timestamp(),
+          VecBuilder.fill(linearStdDev, linearStdDev, angularStdDev));
+    }
+  }
+
+  /**
+   * Determines whether to reject a vision pose observation based on various criteria such as
+   * ambiguity, Z coordinate, and field boundaries.
+   *
+   * @param observation - the vision pose observation to evaluate for rejection
+   * @return - true if the pose observation should be rejected, false otherwise
+   */
+  private static boolean shouldRejectPose(VisionIO.PoseObservation observation) {
+    return observation.tagCount() == 0 // Must have at least one tag
+        || (observation.tagCount() == 1
+            && observation.ambiguity() > VisionConstants.MAX_AMBIGUITY) // Cannot be high ambiguity
+        || Math.abs(observation.pose().getZ())
+            > VisionConstants.MAX_Z_ERROR // Must have realistic Z coordinate
+
+        // Must be within the field boundaries
+        || observation.pose().getX() < 0.0
+        || observation.pose().getX() > VisionConstants.APRIL_TAG_LAYOUT.getFieldLength()
+        || observation.pose().getY() < 0.0
+        || observation.pose().getY() > VisionConstants.APRIL_TAG_LAYOUT.getFieldWidth();
+  }
+
+  @Override
+  public void simulationPeriodic() {
+    updateVSim.run();
+  }
 }

src/main/java/com/team2813/subsystems/vision/VisionConstants.java

@@ -25,7 +25,7 @@ public class VisionConstants {
   }
 
   // AprilTag layout
-  public static AprilTagFieldLayout aprilTagLayout =
+  public static final AprilTagFieldLayout APRIL_TAG_LAYOUT =
       AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
 
   // Camera names, must match names configured on coprocessor
@@ -53,20 +53,21 @@ public class VisionConstants {
           new Rotation3d(Degrees.of(0), Degrees.of(-17.5), Degrees.of(0)));
 
   // Basic filtering thresholds
-  public static double maxAmbiguity = 0.3;
-  public static double maxZError = 0.75;
+  public static final double MAX_AMBIGUITY = 0.3;
+  public static final double MAX_Z_ERROR = 0.75;
 
   // Standard deviation baselines, for 1 meter distance and 1 tag
   // (Adjusted automatically based on distance and # of tags)
-  public static double linearStdDevBaseline = 0.02; // Meters
-  public static double angularStdDevBaseline = 0.06; // Radians
+  public static final double LINEAR_STD_DEV_BASELINE = 0.02; // Meters
+  public static final double ANGULAR_STD_DEV_BASELINE = 0.06; // Radians
 
   // Standard deviation multipliers for each camera
   // (Adjust to trust some cameras more than others)
-  public static double[] cameraStdDevFactors =
+  public static final double[] CAMERA_STD_DEV_FACTORS =
       new double[] {
         1.0, // Camera 0
-        1.0 // Camera 1
+        1.0, // Camera 1
+        1.0, // Camera 2
       };
 
   /**
@@ -76,9 +77,4 @@ public class VisionConstants {
   public static boolean isVisionEnabled() {
     return Preferences.getBoolean(VISION_ENABLED_NT, false);
   }
-
-  // Multipliers to apply for MegaTag 2 observations
-  public static double linearStdDevMegatag2Factor = 0.5; // More stable than full 3D solve
-  public static double angularStdDevMegatag2Factor =
-      Double.POSITIVE_INFINITY; // No rotation data available
 }

src/main/java/com/team2813/subsystems/vision/VisionIO.java

@@ -26,18 +26,7 @@ public static record TargetObservation(Rotation2d tx, Rotation2d ty) {}
 
   /** Represents a robot pose sample used for pose estimation. */
   public static record PoseObservation(
-      double timestamp,
-      Pose3d pose,
-      double ambiguity,
-      int tagCount,
-      double averageTagDistance,
-      PoseObservationType type) {}
-
-  public static enum PoseObservationType {
-    MEGATAG_1,
-    MEGATAG_2,
-    PHOTONVISION
-  }
+      double timestamp, Pose3d pose, double ambiguity, int tagCount, double averageTagDistance) {}
 
   public default void updateInputs(VisionIOInputs inputs) {}
 }