From 10472b7be45c5c66b5e8b36165d554f3a6643ca5 Mon Sep 17 00:00:00 2001 From: Veselin Dikov Date: Sun, 22 Mar 2026 15:32:07 -0700 Subject: [PATCH] Roll-forward vision --- .../java/com/team2813/RobotContainer.java | 13 +- .../team2813/subsystems/vision/Vision.java | 178 ++++++++++-------- .../subsystems/vision/VisionConstants.java | 20 +- .../team2813/subsystems/vision/VisionIO.java | 13 +- .../vision/VisionIOPhotonVision.java | 24 +-- .../vision/VisionIOPhotonVisionSim.java | 21 +-- 6 files changed, 136 insertions(+), 133 deletions(-) diff --git a/src/main/java/com/team2813/RobotContainer.java b/src/main/java/com/team2813/RobotContainer.java index 080c4628..703be287 100644 --- a/src/main/java/com/team2813/RobotContainer.java +++ b/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() {}); diff --git a/src/main/java/com/team2813/subsystems/vision/Vision.java b/src/main/java/com/team2813/subsystems/vision/Vision.java index 3de2308a..a143e514 100644 --- a/src/main/java/com/team2813/subsystems/vision/Vision.java +++ b/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 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 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 robotPoses, + List robotPosesAccepted, + List 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(); + } } diff --git a/src/main/java/com/team2813/subsystems/vision/VisionConstants.java b/src/main/java/com/team2813/subsystems/vision/VisionConstants.java index 37f406af..9866643e 100644 --- a/src/main/java/com/team2813/subsystems/vision/VisionConstants.java +++ b/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 } diff --git a/src/main/java/com/team2813/subsystems/vision/VisionIO.java b/src/main/java/com/team2813/subsystems/vision/VisionIO.java index 5ce7cab2..013439c8 100644 --- a/src/main/java/com/team2813/subsystems/vision/VisionIO.java +++ b/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) {} } diff --git a/src/main/java/com/team2813/subsystems/vision/VisionIOPhotonVision.java b/src/main/java/com/team2813/subsystems/vision/VisionIOPhotonVision.java index ed73ba6f..a002b8fd 100644 --- a/src/main/java/com/team2813/subsystems/vision/VisionIOPhotonVision.java +++ b/src/main/java/com/team2813/subsystems/vision/VisionIOPhotonVision.java @@ -7,7 +7,7 @@ package com.team2813.subsystems.vision; -import static com.team2813.subsystems.vision.VisionConstants.aprilTagLayout; +import static com.team2813.subsystems.vision.VisionConstants.APRIL_TAG_LAYOUT; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; @@ -77,14 +77,13 @@ public void updateInputs(VisionIOInputs inputs) { robotPose, // 3D pose estimate multitagResult.estimatedPose.ambiguity, // Ambiguity multitagResult.fiducialIDsUsed.size(), // Tag count - totalTagDistance / result.targets.size(), // Average tag distance - PoseObservationType.PHOTONVISION)); // Observation type + totalTagDistance / result.targets.size())); // Average tag distance } else if (!result.targets.isEmpty()) { // Single tag result var target = result.targets.get(0); // Calculate robot pose - var tagPose = aprilTagLayout.getTagPose(target.fiducialId); + var tagPose = APRIL_TAG_LAYOUT.getTagPose(target.fiducialId); if (tagPose.isPresent()) { Transform3d fieldToTarget = new Transform3d(tagPose.get().getTranslation(), tagPose.get().getRotation()); @@ -97,14 +96,15 @@ public void updateInputs(VisionIOInputs inputs) { tagIds.add((short) target.fiducialId); // Add observation - poseObservations.add( - new PoseObservation( - result.getTimestampSeconds(), // Timestamp - robotPose, // 3D pose estimate - target.poseAmbiguity, // Ambiguity - 1, // Tag count - cameraToTarget.getTranslation().getNorm(), // Average tag distance - PoseObservationType.PHOTONVISION)); // Observation type + if (!result.targets.isEmpty()) { + poseObservations.add( + new PoseObservation( + result.getTimestampSeconds(), // Timestamp + robotPose, // 3D pose estimate + target.poseAmbiguity, // Ambiguity + 1, // Tag count + cameraToTarget.getTranslation().getNorm())); // Average tag distance + } } } } diff --git a/src/main/java/com/team2813/subsystems/vision/VisionIOPhotonVisionSim.java b/src/main/java/com/team2813/subsystems/vision/VisionIOPhotonVisionSim.java index d1ef0ff8..cfd99637 100644 --- a/src/main/java/com/team2813/subsystems/vision/VisionIOPhotonVisionSim.java +++ b/src/main/java/com/team2813/subsystems/vision/VisionIOPhotonVisionSim.java @@ -7,48 +7,35 @@ package com.team2813.subsystems.vision; -import static com.team2813.subsystems.vision.VisionConstants.aprilTagLayout; +import static com.team2813.subsystems.vision.VisionConstants.APRIL_TAG_LAYOUT; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Transform3d; -import java.util.function.Supplier; import org.photonvision.simulation.PhotonCameraSim; import org.photonvision.simulation.SimCameraProperties; import org.photonvision.simulation.VisionSystemSim; /** IO implementation for physics sim using PhotonVision simulator. */ public class VisionIOPhotonVisionSim extends VisionIOPhotonVision { - private static VisionSystemSim visionSim; - - private final Supplier poseSupplier; private final PhotonCameraSim cameraSim; /** * Creates a new VisionIOPhotonVisionSim. * * @param name The name of the camera. - * @param poseSupplier Supplier for the robot pose to use in simulation. + * @param visionSim The simulated vision system to add the camera to. */ public VisionIOPhotonVisionSim( - String name, Transform3d robotToCamera, Supplier poseSupplier) { + String name, Transform3d robotToCamera, VisionSystemSim visionSim) { super(name, robotToCamera); - this.poseSupplier = poseSupplier; - - // Initialize vision sim - if (visionSim == null) { - visionSim = new VisionSystemSim("main"); - visionSim.addAprilTags(aprilTagLayout); - } // Add sim camera var cameraProperties = new SimCameraProperties(); - cameraSim = new PhotonCameraSim(camera, cameraProperties, aprilTagLayout); + cameraSim = new PhotonCameraSim(camera, cameraProperties, APRIL_TAG_LAYOUT); visionSim.addCamera(cameraSim, robotToCamera); } @Override public void updateInputs(VisionIOInputs inputs) { - visionSim.update(poseSupplier.get()); super.updateInputs(inputs); } }