From 32a35867c5b18ac22a3bf0c4c0ed4c5e28cf7890 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Sun, 22 Feb 2026 14:47:13 -0500 Subject: [PATCH] multiple cameras --- .../frc/robot/constants/GameConstants.java | 5 ++- .../robot/subsystems/ApriltagSubsystem.java | 40 +++++++++---------- .../swervedrive/SwerveSubsystem.java | 4 +- .../estimation/FilterablePoseManager.java | 24 ++++++----- .../vision/estimation/PoseEstimator.java | 5 ++- .../vision/estimation/PoseManager.java | 9 +++-- 6 files changed, 51 insertions(+), 36 deletions(-) diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index f7a9e708..c099c1b6 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -163,7 +163,10 @@ public enum Mode { public static final double HORIZONTAL_FOV = Units.degreesToRadians(110); // radians; TODO: Change Later public static final double VERTICAL_FOV = Units.degreesToRadians(90); // radians; TODO: Change Later - public static final Transform3d ROBOT_TO_CAMERA = new Transform3d(0,0,0, new Rotation3d(0,0,0)); // TODO: change Later + public static final Transform3d[] ROBOT_TO_CAMERA = {new Transform3d(0,0,0, new Rotation3d(0,0,0)), + new Transform3d(0,0,0, new Rotation3d(0,0,Math.PI/2)), + new Transform3d(0,0,0, new Rotation3d(0,0,Math.PI)), + new Transform3d(0,0,0, new Rotation3d(0,0,3*Math.PI/2))}; // TODO: change Later public static final double AVERAGE_CAM_LATENCY = 0; // seconds; TODO: change Later public static final double AVERAGE_CAM_LATENCY_STD_DEV = 0; // seconds; TODO: change Later public static final double AVERAGE_PIR_LATENCY = 20; //ms diff --git a/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java b/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java index dcd41142..166cabb0 100644 --- a/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java @@ -1,12 +1,10 @@ package frc.robot.subsystems; import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.apriltags.*; @@ -74,23 +72,25 @@ public void periodic() { } } public void simReadings() { - for (Apriltag tag: Apriltag.values()) { - Pose3d cameraPos = new Pose3d(robotPoseSupplier.get()).transformBy(Constants.ROBOT_TO_CAMERA); - if (tag.canSee(cameraPos,Constants.HORIZONTAL_FOV, Constants.VERTICAL_FOV)) { - Pose3d adjPose = tag.getPose().relativeTo(cameraPos); - double cosIncidenceAngle = (-adjPose.getX()*Math.cos(adjPose.getRotation().getZ())-adjPose.getY()*Math.sin(adjPose.getRotation().getZ()))/(adjPose.getTranslation().getNorm()); - double distance = tag.getTranslation().getDistance(cameraPos.getTranslation()); - if (distance/cosIncidenceAngle < Constants.MAX_VISION_DISTANCE.get()) { - VisionMeasurement measurement = new VisionMeasurement(new Pose2d(), tag.getTagInfo(),distance,0); - Vector stdDevs = estimator.getVisionTruster().calculateTrust(measurement, cameraPos); - double readingX = robotPoseSupplier.get().getX()+ random.nextGaussian()*stdDevs.get(0); - double readingY = robotPoseSupplier.get().getY()+ random.nextGaussian()*stdDevs.get(1); - double readingYaw = robotPoseSupplier.get().getRotation().getDegrees()+ random.nextGaussian()*stdDevs.get(2); - Pose2d readingPos = new Pose2d(readingX,readingY,Rotation2d.fromDegrees(readingYaw)); - distance = readingPos.getTranslation().getDistance(tag.getPose().toPose2d().getTranslation()); - if(BasicVisionFilter.inBounds(readingPos)) { - io.addReading(new ApriltagReading(readingX, readingY, readingYaw, - distance, tag.number(), Constants.AVERAGE_CAM_LATENCY + random.nextGaussian() * Constants.AVERAGE_CAM_LATENCY_STD_DEV, Logger.getTimestamp() / 1000.0)); + for (Transform3d cameraTransform: Constants.ROBOT_TO_CAMERA) { + for (Apriltag tag : Apriltag.values()) { + Pose3d cameraPos = new Pose3d(robotPoseSupplier.get()).transformBy(cameraTransform); + if (tag.canSee(cameraPos, Constants.HORIZONTAL_FOV, Constants.VERTICAL_FOV)) { + Pose3d adjPose = tag.getPose().relativeTo(cameraPos); + double cosIncidenceAngle = (-adjPose.getX() * Math.cos(adjPose.getRotation().getZ()) - adjPose.getY() * Math.sin(adjPose.getRotation().getZ())) / (adjPose.getTranslation().getNorm()); + double distance = tag.getTranslation().getDistance(cameraPos.getTranslation()); + if (distance / cosIncidenceAngle < Constants.MAX_VISION_DISTANCE.get()) { + VisionMeasurement measurement = new VisionMeasurement(new Pose2d(), tag.getTagInfo(), distance, 0); + Vector stdDevs = estimator.getVisionTruster().calculateTrust(measurement, cameraPos); + double readingX = robotPoseSupplier.get().getX() + random.nextGaussian() * stdDevs.get(0); + double readingY = robotPoseSupplier.get().getY() + random.nextGaussian() * stdDevs.get(1); + double readingYaw = robotPoseSupplier.get().getRotation().getDegrees() + random.nextGaussian() * stdDevs.get(2); + Pose2d readingPos = new Pose2d(readingX, readingY, Rotation2d.fromDegrees(readingYaw)); + distance = readingPos.getTranslation().getDistance(tag.getPose().toPose2d().getTranslation()); + if (BasicVisionFilter.inBounds(readingPos)) { + io.addReading(new ApriltagReading(readingX, readingY, readingYaw, + distance, tag.number(), Constants.AVERAGE_CAM_LATENCY + random.nextGaussian() * Constants.AVERAGE_CAM_LATENCY_STD_DEV, Logger.getTimestamp() / 1000.0)); + } } } } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index e3b251a8..125e7864 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -335,8 +335,8 @@ public double getError() { public double getAverageError(){ return poseError.stream().mapToDouble(record -> record.error).average().orElse(0); } - public Pose3d getCameraPose() { - return new Pose3d(getSimulationPose()).transformBy(Constants.ROBOT_TO_CAMERA); + public Pose3d getCameraPose(int camera) { + return new Pose3d(getSimulationPose()).transformBy(Constants.ROBOT_TO_CAMERA[camera]); } public Pose2d getSimulationPose() { diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/FilterablePoseManager.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/FilterablePoseManager.java index 8a668baf..63d0a145 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/FilterablePoseManager.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/FilterablePoseManager.java @@ -29,6 +29,10 @@ private record MeasurementRecord(Apriltag tag, double timestamp, FilterResult re private final VisionFilter filter; private final ConcurrentLinkedDeque lastSecondMeasurements = new ConcurrentLinkedDeque<>(); + private final List validMeasurementsPose = new ArrayList<>(); + private final List invalidMeasurementsPose = new ArrayList<>();; + private final List acceptedTagsPose = new ArrayList<>(); + private final List log = new ArrayList<>(); public FilterablePoseManager( PoseDeviation PoseDeviation, @@ -57,19 +61,15 @@ public FilterablePoseManager( } @Override - public void processQueue() { + public void processQueue(int camera) { double currentTime = Logger.getTimestamp()/1000000.0; double oneSecondAgo = currentTime - 1.0; lastSecondMeasurements.removeIf(record -> record.timestamp < oneSecondAgo); - List log = new ArrayList<>(); - List validMeasurementsPose = new ArrayList<>(); - List invalidMeasurementsPose = new ArrayList<>();; - List acceptedTagsPose = new ArrayList<>(); for (Map.Entry> queueEntry : visionMeasurementQueueMap.entrySet()) { int tagId = queueEntry.getKey(); Queue queue = queueEntry.getValue(); LinkedHashMap filteredData = - filter.filter(queue, drivebase.getCameraPose()); + filter.filter(queue, drivebase.getCameraPose(camera)); queue.clear(); for (Map.Entry filterEntry : filteredData.entrySet()) { VisionMeasurement v = filterEntry.getKey(); @@ -78,11 +78,10 @@ public void processQueue() { lastSecondMeasurements.add(new MeasurementRecord(Apriltag.of(tagId), v.timeOfMeasurement(),r)); switch (r) { case ACCEPTED -> { - setVisionSTD(visionTruster.calculateTrust(v, drivebase.getCameraPose())); + setVisionSTD(visionTruster.calculateTrust(v, drivebase.getCameraPose(camera))); validMeasurementsPose.add(v.measurement()); addVisionMeasurement(v); acceptedTagsPose.add(Apriltag.of(tagId).getPose()); - } case NOT_PROCESSED -> queue.add(v); case REJECTED -> { @@ -91,6 +90,10 @@ public void processQueue() { } } } + + + } + public void log() { Logger.recordOutput("Apriltag/acceptedMeasurementsPose", validMeasurementsPose.toArray(Pose2d[]::new)); Logger.recordOutput("Apriltag/rejectedMeasurementsPose", invalidMeasurementsPose.toArray(Pose2d[]::new)); Logger.recordOutput("Apriltag/numberAcceptedLastSecond", lastSecondMeasurements.stream().filter(record -> record.result == FilterResult.ACCEPTED).count()); @@ -98,8 +101,11 @@ public void processQueue() { Logger.recordOutput("Apriltag/numberRejectedLastSecond", lastSecondMeasurements.stream().filter(record -> record.result == FilterResult.REJECTED).count()); Logger.recordOutput("Apriltag/acceptedTagPose", acceptedTagsPose.toArray(Pose3d[]::new)); Logger.recordOutput("Apriltag/Log", log.toArray(VisionLog[]::new)); + validMeasurementsPose.clear(); + invalidMeasurementsPose.clear(); + acceptedTagsPose.clear(); + log.clear(); } - public VisionTruster getVisionTruster() { return visionTruster; } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java index 069afe12..60c163df 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java @@ -135,7 +135,10 @@ private void updateVision(int... invalidApriltagNumbers) { } long end = System.currentTimeMillis(); Logger.recordOutput("RegisteringVisionTimeMillis", end - start); - poseManager.processQueue(); + for (int i=0; i new LinkedList<>()).add(measurement); + while (visionMeasurementQueueMap.computeIfAbsent(tagId,k -> new LinkedList<>()).size() >= 3) { + visionMeasurementQueueMap.get(tagId).poll(); + } + visionMeasurementQueueMap.get(tagId).add(measurement); } // override for filtering - public void processQueue() { + public void processQueue(int camera) { for (Queue queue : visionMeasurementQueueMap.values()) { VisionMeasurement m = queue.poll(); while (m != null) { - setVisionSTD(visionTruster.calculateTrust(m, drivebase.getCameraPose())); + setVisionSTD(visionTruster.calculateTrust(m, drivebase.getCameraPose(camera))); addVisionMeasurement(m); m = queue.poll(); }