From 558cb4c957f112587d19703156ba3360759a55b7 Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Fri, 20 Mar 2026 17:49:41 -0400 Subject: [PATCH 1/2] done --- .../frc/robot/apriltags/SimApriltagIO.java | 15 ++++---- .../robot/apriltags/TCPApriltagServer.java | 11 +++--- .../frc/robot/constants/GameConstants.java | 2 ++ .../estimation/FilterablePoseManager.java | 34 +++++++++++-------- .../vision/estimation/PoseEstimator.java | 6 ++-- .../vision/estimation/PoseManager.java | 22 +++++++----- .../vision/truster/BasicVisionFilter.java | 15 ++++---- .../vision/truster/ConstantVisionTruster.java | 3 +- .../vision/truster/LinearVisionTruster.java | 3 +- .../vision/truster/SquareVisionTruster.java | 11 ++++-- .../vision/truster/VisionMeasurement.java | 4 ++- .../vision/truster/VisionTruster.java | 3 +- 12 files changed, 78 insertions(+), 51 deletions(-) diff --git a/src/main/java/frc/robot/apriltags/SimApriltagIO.java b/src/main/java/frc/robot/apriltags/SimApriltagIO.java index 256c6f29..22d1a895 100644 --- a/src/main/java/frc/robot/apriltags/SimApriltagIO.java +++ b/src/main/java/frc/robot/apriltags/SimApriltagIO.java @@ -1,5 +1,6 @@ package frc.robot.apriltags; +import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.numbers.N3; @@ -38,12 +39,12 @@ public void simReadings() { for (Apriltag tag : Apriltag.values()) { Pose3d cameraPos = new Pose3d(pose).transformBy(Constants.ROBOT_TO_CAMERA); if (ObjectUtils.canSee(tag.getPose(), 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 (cosIncidenceAngle!=0 && distance / cosIncidenceAngle < Constants.MAX_VISION_DISTANCE_SIMULATION) { - VisionMeasurement measurement = new VisionMeasurement(new Pose2d(), distance, 0); - Vector stdDevs = truster.calculateTrust(measurement); + Translation3d adjPose2 = cameraPos.relativeTo(tag.getPose()).getTranslation(); + double distance = adjPose2.getNorm(); + double distanceTimesCosIncidenceAngle = adjPose2.getX(); + if (distanceTimesCosIncidenceAngle!=0 && distance*distance/distanceTimesCosIncidenceAngle < Constants.MAX_VISION_DISTANCE_SIMULATION) { + VisionMeasurement measurement = new VisionMeasurement(new Pose2d(), distance, 0, 0, 0); + Vector stdDevs = truster.calculateTrust(measurement, cameraPos); double readingX = pose.getX() + random.nextGaussian() * stdDevs.get(0); double readingY = pose.getY() + random.nextGaussian() * stdDevs.get(1); double readingYaw = pose.getRotation().getDegrees() + random.nextGaussian() * stdDevs.get(2); @@ -51,7 +52,7 @@ public void simReadings() { distance = readingPos.getTranslation().getDistance(tag.getPose().toPose2d().getTranslation()); if (BasicVisionFilter.inBounds(readingPos)) { addReading(new ApriltagReading(readingX, readingY, readingYaw, - distance,0,tag.number(), Constants.AVERAGE_CAM_LATENCY + random.nextGaussian() * Constants.AVERAGE_CAM_LATENCY_STD_DEV, stdDevs.get(0),Logger.getTimestamp() / 1000.0)); + distance,Math.acos(distanceTimesCosIncidenceAngle/distance),tag.number(), Constants.AVERAGE_CAM_LATENCY + random.nextGaussian() * Constants.AVERAGE_CAM_LATENCY_STD_DEV, stdDevs.get(0),Logger.getTimestamp() / 1000.0)); } } } diff --git a/src/main/java/frc/robot/apriltags/TCPApriltagServer.java b/src/main/java/frc/robot/apriltags/TCPApriltagServer.java index 62819099..4e31327e 100644 --- a/src/main/java/frc/robot/apriltags/TCPApriltagServer.java +++ b/src/main/java/frc/robot/apriltags/TCPApriltagServer.java @@ -3,6 +3,7 @@ import edu.wpi.first.wpilibj.Timer; import java.io.DataInputStream; import java.io.IOException; +import frc.robot.constants.Constants; public class TCPApriltagServer extends TCPServer { @@ -21,7 +22,7 @@ protected ApriltagReading extractFromStream(DataInputStream stream) throws IOExc double poseYaw = -1; double distanceToTag = -1; double cameraToTagAngle = -1; - double timestamp = -1; + double latency = -1; double stdDev = -1; int apriltagNumber = -1; double now = 0; @@ -31,18 +32,18 @@ protected ApriltagReading extractFromStream(DataInputStream stream) throws IOExc && distanceToTag == -1 && cameraToTagAngle == -1 && apriltagNumber == -1 - && timestamp == -1 + && latency == -1 && stdDev == -1) { posX = stream.readDouble(); posY = stream.readDouble(); poseYaw = stream.readDouble(); distanceToTag = stream.readDouble(); cameraToTagAngle = stream.readDouble(); - timestamp = stream.readDouble(); + latency = stream.readDouble(); stdDev = stream.readDouble(); apriltagNumber = stream.readInt(); - now = Timer.getFPGATimestamp() * 1000; + now = Timer.getFPGATimestamp() * 1000-Constants.AVERAGE_PIR_LATENCY-latency; } - return new ApriltagReading(posX, posY, poseYaw, distanceToTag, cameraToTagAngle, apriltagNumber, timestamp, stdDev, now); + return new ApriltagReading(posX, posY, poseYaw, distanceToTag, cameraToTagAngle, apriltagNumber, latency, stdDev, now); } } diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index ebba02b8..2bf1fe88 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -160,4 +160,6 @@ public enum Mode { 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 MAX_VISION_DISTANCE_SIMULATION = 6; + public static final double AVERAGE_PIR_LATENCY = 20; //ms + public static final double VISION_STD_THRESHOLD = 0.25; } 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 8e8a3cef..7cb386a5 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 @@ -1,5 +1,6 @@ package frc.robot.subsystems.swervedrive.vision.estimation; +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; @@ -57,23 +58,26 @@ public FilterablePoseManager( public void processQueue() { List validMeasurementsPose = new ArrayList<>(); List invalidMeasurementsPose = new ArrayList<>(); - - LinkedHashMap filteredData = + for (Map.Entry> queueEntry : visionMeasurementQueueMap.entrySet()) { + Queue visionMeasurementQueue = queueEntry.getValue(); + LinkedHashMap filteredData = filter.filter(visionMeasurementQueue); - visionMeasurementQueue.clear(); - for (Map.Entry filterEntry : filteredData.entrySet()) { - VisionMeasurement v = filterEntry.getKey(); - FilterResult r = filterEntry.getValue(); - switch (r) { - case ACCEPTED -> { - setVisionSTD(visionTruster.calculateTrust(v)); - validMeasurementsPose.add(v.measurement()); - addVisionMeasurement(v); + visionMeasurementQueue.clear(); - } - case NOT_PROCESSED -> visionMeasurementQueue.add(v); - case REJECTED -> { - invalidMeasurementsPose.add(v.measurement()); + for (Map.Entry filterEntry : filteredData.entrySet()) { + VisionMeasurement v = filterEntry.getKey(); + FilterResult r = filterEntry.getValue(); + switch (r) { + case ACCEPTED -> { + setVisionSTD(VecBuilder.fill(v.standardDeviation(), v.standardDeviation(), v.standardDeviation())); + validMeasurementsPose.add(v.measurement()); + addVisionMeasurement(v); + + } + case NOT_PROCESSED -> visionMeasurementQueue.add(v); + case REJECTED -> { + invalidMeasurementsPose.add(v.measurement()); + } } } } 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 dc80d1af..3d214499 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 @@ -145,6 +145,8 @@ private VisionMeasurement getVisionMeasurement(double[] pos, int index) { double serverTime = apriltagSystem.getIO().getInputs().serverTime[index]; //double timestamp = 0; // latency is not right we are assuming zero double timestamp = apriltagSystem.getIO().getInputs().timestamp[index]; + double stdDev = apriltagSystem.getIO().getInputs().stdDev[index]; + int tagId = apriltagSystem.getIO().getInputs().apriltagNumber[index]; Pose2d visionPose; if (getEstimatedPose()!=null) { visionPose = new Pose2d(pos[0], pos[1], getEstimatedPose().getRotation()); @@ -152,7 +154,7 @@ private VisionMeasurement getVisionMeasurement(double[] pos, int index) { visionPose = new Pose2d(pos[0], pos[1], poseManager.getRotation()); } double distanceFromTag = apriltagSystem.getIO().getInputs().distanceToTag[index]; - return new VisionMeasurement(visionPose, distanceFromTag, timestamp/1000); + return new VisionMeasurement(visionPose, distanceFromTag, timestamp/1000, stdDev, tagId); } /** @@ -206,7 +208,7 @@ public FilterablePoseManager getPoseManager() { public void addMockVisionMeasurement() { poseManager.registerVisionMeasurement( - new VisionMeasurement(getEstimatedPose(), 0, Logger.getTimestamp() / 1e6)); + new VisionMeasurement(getEstimatedPose(), 0, Logger.getTimestamp() / 1e6, 0, 0 )); } public VisionTruster getVisionTruster() { return poseManager.getVisionTruster(); diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseManager.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseManager.java index 9658b8a8..0da14841 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseManager.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseManager.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.swervedrive.vision.estimation; +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.Rotation2d; @@ -13,6 +14,7 @@ import frc.robot.subsystems.swervedrive.vision.truster.VisionTruster; import org.littletonrobotics.junction.Logger; +import java.util.LinkedHashMap; import java.util.LinkedList; import java.util.Optional; import java.util.Queue; @@ -24,7 +26,7 @@ public class PoseManager { private final TimeInterpolatableBuffer estimatedPoseBuffer; //private final SwerveDrivePoseEstimator poseEstimator; - protected final Queue visionMeasurementQueue = new LinkedList<>(); + protected final LinkedHashMap> visionMeasurementQueueMap = new LinkedHashMap<>(); private final SwerveSubsystem drivebase; protected final VisionTruster visionTruster; @@ -67,19 +69,21 @@ public void registerVisionMeasurement(VisionMeasurement measurement) { if (measurement == null) { return; } - while (visionMeasurementQueue.size() >= 3) { - visionMeasurementQueue.poll(); + while (visionMeasurementQueueMap.computeIfAbsent(measurement.tagId(),k -> new LinkedList<>()).size() >= 3) { + visionMeasurementQueueMap.get(measurement.tagId()).poll(); } - visionMeasurementQueue.add(measurement); + visionMeasurementQueueMap.get(measurement.tagId()).add(measurement); } // override for filtering public void processQueue() { - VisionMeasurement m = visionMeasurementQueue.poll(); - while (m != null) { - setVisionSTD(visionTruster.calculateTrust(m)); - addVisionMeasurement(m); - m = visionMeasurementQueue.poll(); + for (Queue visionMeasurementQueue : visionMeasurementQueueMap.values()) { + VisionMeasurement m = visionMeasurementQueue.poll(); + while (m != null) { + setVisionSTD(VecBuilder.fill(m.standardDeviation(), m.standardDeviation(), m.standardDeviation())); + addVisionMeasurement(m); + m = visionMeasurementQueue.poll(); + } } } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/BasicVisionFilter.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/BasicVisionFilter.java index 388715c1..6fe93a95 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/BasicVisionFilter.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/BasicVisionFilter.java @@ -54,7 +54,7 @@ public LinkedHashMap filter( Pose2d vision1Pose = getVisionPose(m1); Pose2d vision2Pose = getVisionPose(m2); boolean valid1 = - filterVision(vision1Pose, vision2Pose, m1.timeOfMeasurement(), m2.timeOfMeasurement()); + filterVision(m1,m2); resultMap.put(m1, valid1 ? FilterResult.ACCEPTED : FilterResult.REJECTED); m1 = measurements.poll(); m2 = measurements.peek(); @@ -63,19 +63,22 @@ public LinkedHashMap filter( return resultMap; } - private boolean filterVision(Pose2d m1Pose, Pose2d m2Pose, double m1Time, double m2Time) { - Optional odomPoseAtVis1 = poseBuffer.getSample(m1Time); - Optional odomPoseAtVis2 = poseBuffer.getSample(m2Time); + private boolean filterVision(VisionMeasurement m1, VisionMeasurement m2) { + Optional odomPoseAtVis1 = poseBuffer.getSample(m1.timeOfMeasurement()); + Optional odomPoseAtVis2 = poseBuffer.getSample(m1.timeOfMeasurement()); if (odomPoseAtVis1.isEmpty() || odomPoseAtVis2.isEmpty()) { return false; } - if (!inBounds(m1Pose) || !inBounds(m2Pose)) { + if (!inBounds(m1.measurement()) || !inBounds(m2.measurement())) { return false; } double odomDiff1To2 = odomPoseAtVis1.get().getTranslation().getDistance(odomPoseAtVis2.get().getTranslation()); - double visionDiff1To2 = m1Pose.getTranslation().getDistance(m2Pose.getTranslation()); + double visionDiff1To2 = m1.measurement().getTranslation().getDistance(m2.measurement().getTranslation()); double diff = Math.abs(odomDiff1To2 - visionDiff1To2); + if (m1.standardDeviation() > Constants.VISION_STD_THRESHOLD) { + return false; + } return Math.abs(diff) <= Constants.VISION_CONSISTENCY_THRESHOLD; } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/ConstantVisionTruster.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/ConstantVisionTruster.java index 3017ccb1..fe133bbe 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/ConstantVisionTruster.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/ConstantVisionTruster.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.swervedrive.vision.truster; import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.numbers.N3; @@ -11,7 +12,7 @@ public ConstantVisionTruster(Vector initialSTD) { } @Override - public Vector calculateTrust(VisionMeasurement measurement) { + public Vector calculateTrust(VisionMeasurement measurement, Pose3d cameraPose) { return initialSTD; } } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/LinearVisionTruster.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/LinearVisionTruster.java index e7f75ea6..78e6f094 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/LinearVisionTruster.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/LinearVisionTruster.java @@ -2,6 +2,7 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.numbers.N3; public class LinearVisionTruster extends DistanceVisionTruster { @@ -14,7 +15,7 @@ public LinearVisionTruster(Vector initialSTD, double slopeSTD) { } @Override - public Vector calculateTrust(VisionMeasurement measurement) { + public Vector calculateTrust(VisionMeasurement measurement, Pose3d cameraPose) { double std = measurement.distanceFromTag() * slopeSTD; return initialSTD.plus(VecBuilder.fill(std, std, std)); } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/SquareVisionTruster.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/SquareVisionTruster.java index 7a925fc8..98d59a89 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/SquareVisionTruster.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/SquareVisionTruster.java @@ -2,7 +2,10 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.numbers.N3; +import frc.robot.utils.Apriltag; public class SquareVisionTruster extends DistanceVisionTruster { @@ -15,8 +18,10 @@ public SquareVisionTruster(Vector initialSTD, double constant) { } @Override - public Vector calculateTrust(VisionMeasurement measurement) { - double std = Math.pow(measurement.distanceFromTag() - 0.4572, 2) * constant; - return initialSTD.plus(VecBuilder.fill(std, std, std)); + public Vector calculateTrust(VisionMeasurement measurement, Pose3d cameraPose) { + Translation3d adjPose2 = cameraPose.relativeTo(Apriltag.of(measurement.tagId()).getPose()).getTranslation(); + double distanceTimesCosIncidenceAngle = adjPose2.getX()/adjPose2.getNorm(); + double std = Math.pow(measurement.distanceFromTag(), 2) * constant / distanceTimesCosIncidenceAngle; + return initialSTD.plus(VecBuilder.fill(std, std, std*10000)); } } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionMeasurement.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionMeasurement.java index c4913a8b..9e009908 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionMeasurement.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionMeasurement.java @@ -1,6 +1,8 @@ package frc.robot.subsystems.swervedrive.vision.truster; +import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.numbers.N3; import frc.robot.utils.Apriltag; /** @@ -9,4 +11,4 @@ * @param distanceFromTag distance (meters) estimated robot pose was from the tag * @param timeOfMeasurement time when the pose was measured (seconds) */ -public record VisionMeasurement(Pose2d measurement, double distanceFromTag, double timeOfMeasurement) {} +public record VisionMeasurement(Pose2d measurement, double distanceFromTag, double timeOfMeasurement, double standardDeviation, int tagId) {} diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionTruster.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionTruster.java index 7ce7c3d4..36279178 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionTruster.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionTruster.java @@ -1,8 +1,9 @@ package frc.robot.subsystems.swervedrive.vision.truster; import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.numbers.N3; public interface VisionTruster { - Vector calculateTrust(VisionMeasurement measurement); + Vector calculateTrust(VisionMeasurement measurement, Pose3d cameraPose); } From 40ae88de322358c808982f4c0cbd6f4d211bc1fa Mon Sep 17 00:00:00 2001 From: Lev Strougov <62769580+Levercpu@users.noreply.github.com> Date: Fri, 20 Mar 2026 18:02:46 -0400 Subject: [PATCH 2/2] done --- src/main/java/frc/robot/RobotContainer.java | 3 ++- src/main/java/frc/robot/apriltags/SimApriltagIO.java | 2 +- src/main/java/frc/robot/constants/GameConstants.java | 3 ++- .../vision/estimation/FilterablePoseManager.java | 11 +++++++++++ .../swervedrive/vision/estimation/PoseEstimator.java | 2 +- .../swervedrive/vision/truster/BasicVisionFilter.java | 2 -- 6 files changed, 17 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 08f31379..eb6de6ef 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -72,6 +72,7 @@ //import frc.robot.subsystems.TiltSubsystem; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import frc.robot.subsystems.swervedrive.vision.truster.ConstantVisionTruster; +import frc.robot.subsystems.swervedrive.vision.truster.SquareVisionTruster; import frc.robot.subsystems.swervedrive.vision.truster.VisionTruster; import frc.robot.utils.logging.io.gyro.RealGyroIo; import frc.robot.utils.logging.io.gyro.ThreadedGyro; @@ -122,7 +123,7 @@ public class RobotContainer { private AutoFactory autoFactory; private static AutoRoutine straightRoutine; private static AutoTrajectory straightTrajectory; - private final VisionTruster truster = new ConstantVisionTruster(visionMeasurementStdDevs2); + private final VisionTruster truster = new SquareVisionTruster(visionMeasurementStdDevs2, Constants.VISION_STD_DEV_CONST); // Replace with CommandPS4Controller or CommandJoystick if needed // new CommandXboxController(OperatorConstants.kDriverControllerPort);private diff --git a/src/main/java/frc/robot/apriltags/SimApriltagIO.java b/src/main/java/frc/robot/apriltags/SimApriltagIO.java index 22d1a895..21c7bda0 100644 --- a/src/main/java/frc/robot/apriltags/SimApriltagIO.java +++ b/src/main/java/frc/robot/apriltags/SimApriltagIO.java @@ -43,7 +43,7 @@ public void simReadings() { double distance = adjPose2.getNorm(); double distanceTimesCosIncidenceAngle = adjPose2.getX(); if (distanceTimesCosIncidenceAngle!=0 && distance*distance/distanceTimesCosIncidenceAngle < Constants.MAX_VISION_DISTANCE_SIMULATION) { - VisionMeasurement measurement = new VisionMeasurement(new Pose2d(), distance, 0, 0, 0); + VisionMeasurement measurement = new VisionMeasurement(new Pose2d(), distance, 0, 0, tag.number()); Vector stdDevs = truster.calculateTrust(measurement, cameraPos); double readingX = pose.getX() + random.nextGaussian() * stdDevs.get(0); double readingY = pose.getY() + random.nextGaussian() * stdDevs.get(1); diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 2bf1fe88..7c422e03 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -36,7 +36,7 @@ public enum Mode { public static final boolean ENABLE_LOGGING = true; //Debugs - public static final boolean DEBUG = false; + public static final boolean DEBUG = true; public static final boolean ARM_DEBUG = true; //Joystick @@ -162,4 +162,5 @@ public enum Mode { public static final double MAX_VISION_DISTANCE_SIMULATION = 6; public static final double AVERAGE_PIR_LATENCY = 20; //ms public static final double VISION_STD_THRESHOLD = 0.25; + public static final double VISION_STD_DEV_CONST = 1.0/148.0; // TODO: Change later } 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 7cb386a5..cc8bb629 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 @@ -7,6 +7,7 @@ import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.numbers.N3; +import frc.robot.constants.Constants; import frc.robot.constants.GameConstants; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import frc.robot.subsystems.swervedrive.vision.truster.FilterResult; @@ -27,6 +28,7 @@ */ public class FilterablePoseManager extends PoseManager { private final VisionFilter filter; + private List acceptedTagPoses = new ArrayList<>(); public FilterablePoseManager( PoseDeviation PoseDeviation, @@ -58,6 +60,9 @@ public FilterablePoseManager( public void processQueue() { List validMeasurementsPose = new ArrayList<>(); List invalidMeasurementsPose = new ArrayList<>(); + if (Constants.currentMode == Constants.Mode.SIM) { + acceptedTagPoses = new ArrayList<>(); + } for (Map.Entry> queueEntry : visionMeasurementQueueMap.entrySet()) { Queue visionMeasurementQueue = queueEntry.getValue(); LinkedHashMap filteredData = @@ -72,6 +77,9 @@ public void processQueue() { setVisionSTD(VecBuilder.fill(v.standardDeviation(), v.standardDeviation(), v.standardDeviation())); validMeasurementsPose.add(v.measurement()); addVisionMeasurement(v); + if (Constants.currentMode == Constants.Mode.SIM) { + acceptedTagPoses.add(Apriltag.of(v.tagId()).getPose()); + } } case NOT_PROCESSED -> visionMeasurementQueue.add(v); @@ -83,6 +91,9 @@ public void processQueue() { } Logger.recordOutput("Apriltag/numberAccepted", validMeasurementsPose.size()); Logger.recordOutput("Apriltag/numberRejected", invalidMeasurementsPose.size()); + if (Constants.currentMode == Constants.Mode.SIM) { + Logger.recordOutput("Apriltag/acceptedTags", acceptedTagPoses.toArray(Pose3d[]::new)); + } } public VisionTruster getVisionTruster() { 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 3d214499..0f4708c8 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 @@ -51,7 +51,7 @@ public class PoseEstimator { private static final double visionStdRateOfChange = 1; /* standard deviation of vision readings, the lower the numbers arm, the more we trust vision */ - public static final Vector visionMeasurementStdDevs2 = VecBuilder.fill(0.3, 0.3, 100); + public static final Vector visionMeasurementStdDevs2 = VecBuilder.fill(0,0,100); private final FilterablePoseManager poseManager; public PoseEstimator( diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/BasicVisionFilter.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/BasicVisionFilter.java index 6fe93a95..22c82c02 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/BasicVisionFilter.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/BasicVisionFilter.java @@ -51,8 +51,6 @@ public LinkedHashMap filter( Filter poses ------------------------------------------------------- */ - Pose2d vision1Pose = getVisionPose(m1); - Pose2d vision2Pose = getVisionPose(m2); boolean valid1 = filterVision(m1,m2); resultMap.put(m1, valid1 ? FilterResult.ACCEPTED : FilterResult.REJECTED);