Vision Logic

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

@@ -62,6 +62,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;
@@ -120,7 +121,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

src/main/java/frc/robot/apriltags/SimApriltagIO.java

@@ -34,12 +34,12 @@ public void simReadings() {
             for (Apriltag tag : Apriltag.values()) {
                 Pose3d cameraPos = new Pose3d(robotPoseSupplier.get().get()).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 (distance / cosIncidenceAngle < Constants.MAX_VISION_DISTANCE_SIMULATION) {
+                    Translation3d adjPose2 = cameraPos.relativeTo(tag.getPose()).getTranslation();
+                    double distance = adjPose2.getNorm();
+                    double distanceTimesCosIncidenceAngle = adjPose2.getX();
+                    if (distanceTimesCosIncidenceAngle < Constants.MAX_VISION_DISTANCE_SIMULATION) {
                         VisionMeasurement measurement = new VisionMeasurement(new Pose2d(), tag.getTagInfo(), distance, 0);
-                        Vector<N3> stdDevs = truster.calculateTrust(measurement);
+                        Vector<N3> stdDevs = truster.calculateTrust(measurement, cameraPos);
                         double readingX = robotPoseSupplier.get().get().getX() + random.nextGaussian() * stdDevs.get(0);
                         double readingY = robotPoseSupplier.get().get().getY() + random.nextGaussian() * stdDevs.get(1);
                         double readingYaw = robotPoseSupplier.get().get().getRotation().getDegrees() + random.nextGaussian() * stdDevs.get(2);

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<ApriltagReading> {
 
@@ -20,24 +21,24 @@ protected ApriltagReading extractFromStream(DataInputStream stream) throws IOExc
         double posY = -1;
         double poseYaw = -1;
         double distanceToTag = -1;
-        double timestamp = -1;
+        double latency = -1;
         int apriltagNumber = -1;
         double now = 0;
         while (posX == -1
                 && posY == -1
                 && poseYaw == -1
                 && distanceToTag == -1
                 && apriltagNumber == -1
-                && timestamp == -1) {
+                && latency == -1) {
             posX = stream.readDouble();
             posY = stream.readDouble();
             poseYaw = stream.readDouble();
             distanceToTag = stream.readDouble();
-            timestamp = stream.readDouble();
+            latency = 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, apriltagNumber, timestamp, now);
+        return new ApriltagReading(posX, posY, poseYaw, distanceToTag, apriltagNumber, latency, now);
     }
 
 }

src/main/java/frc/robot/constants/GameConstants.java

@@ -1,8 +1,11 @@
 package frc.robot.constants;
 
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.RobotBase;
 import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity;
@@ -149,14 +152,19 @@ public enum Mode {
     public static final int ENDGAME_START = 30;
 
     public static final double VISION_CONSISTENCY_THRESHOLD = 0.25; //How close 2 vision measurements have to be (needs to be tuned potentially but seemingly from my testing it also might not be needed)
+    public static final double VISION_STD_THRESHOLD = 0.25;
     public static final boolean ENABLE_VISION = true;
     public static final double POSE_BUFFER_STORAGE_TIME = 2; //how many past measurements are stored in the buffer (might increase if we need further back)
 
     // Vision
-    public static final Transform3d ROBOT_TO_CAMERA = new Transform3d(0,0,0, new Rotation3d(0,0,0)); // TODO: change
+    public static final Transform3d ROBOT_TO_CAMERA = new Transform3d(0,0,0, new Rotation3d(0,0,0));
     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 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 MAX_VISION_DISTANCE_SIMULATION = 3.5;
+    public static final double AVERAGE_PIR_LATENCY = 20; //ms
+    public static final double VISION_SMOOTHER = 1.0;
+    public static final Vector<N3> INITIAL_VISION_STD_DEVS = VecBuilder.fill(0,0,0); // TODO: Change later
+    public static final double VISION_STD_DEV_CONST = 1.0/148.0; // TODO: Change later
 }

src/main/java/frc/robot/subsystems/ApriltagSubsystem.java

@@ -64,7 +64,4 @@ public void periodic() {
         estimator.updatePosition(drivebase.getOdom());
         io.periodic();
     }
-    public Vector<N3> calculateTrust(VisionMeasurement measurement) {
-        return estimator.getVisionTruster().calculateTrust(measurement);
-    }
 }

src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java

@@ -9,6 +9,7 @@
 import static edu.wpi.first.units.Units.Meter;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 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.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
@@ -333,6 +334,13 @@ public void resetOdometry(Pose2d initialHolonomicPose) {
     public Pose2d getPose() {
         return swerveDrive.getPose();
     }
+    public Pose3d getCameraPose() {
+        if (Constants.currentMode == GameConstants.Mode.SIM) {
+            return new Pose3d(getSimulationPose().get()).transformBy(Constants.ROBOT_TO_CAMERA);
+        } else {
+            return new Pose3d(getPose()).transformBy(Constants.ROBOT_TO_CAMERA);
+        }
+    }
     public double getError() {
         return getPose().getTranslation().getDistance((getSimulationPose().get().getTranslation()));
     }
@@ -515,7 +523,7 @@ public SwerveDrive getSwerveDrive() {
         return swerveDrive;
     }
     public void setVariance(Vector<N3> variance){
-        this.variance = variance;
+        this.variance = variance.times(Constants.VISION_SMOOTHER);
     }
     public void addVisionMeasurement(Pose2d pose, double visionTimestamp){
         swerveDrive.addVisionMeasurement(pose, visionTimestamp, variance);

src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/FilterablePoseManager.java

@@ -28,7 +28,10 @@ public class FilterablePoseManager extends PoseManager {
   private record MeasurementRecord(Apriltag tag, double timestamp, FilterResult result) { }
   public record VisionLog(VisionMeasurement measurement, FilterResult result) {}
   private final ConcurrentLinkedDeque<MeasurementRecord> lastSecondMeasurements = new ConcurrentLinkedDeque<>();
-
+  private final List<Pose2d> validMeasurementsPose = new ArrayList<>();
+  private final List<Pose2d> invalidMeasurementsPose = new ArrayList<>();;
+  private final List<Pose3d> acceptedTagsPose = new ArrayList<>();
+  private final List<VisionLog> log = new ArrayList<>();
   public FilterablePoseManager(
       PoseDeviation PoseDeviation,
       SwerveDriveKinematics kinematics,
@@ -60,43 +63,46 @@ public void processQueue() {
     double currentTime = Logger.getTimestamp()/1000000.0;
     double oneSecondAgo = currentTime - 1.0;
     lastSecondMeasurements.removeIf(record -> record.timestamp < oneSecondAgo);
-    List<VisionLog> log = new ArrayList<>();
-    List<Pose2d> validMeasurementsPose = new ArrayList<>();
-    List<Pose2d> invalidMeasurementsPose = new ArrayList<>();;
-    List<Pose3d> acceptedTagsPose = new ArrayList<>();
-
-    LinkedHashMap<VisionMeasurement, FilterResult> filteredData =
-              filter.filter(visionMeasurementQueue);
-    visionMeasurementQueue.clear();
-    for (Map.Entry<VisionMeasurement, FilterResult> filterEntry : filteredData.entrySet()) {
-      VisionMeasurement v = filterEntry.getKey();
-      Apriltag tag = v.tag().tag();
-      FilterResult r = filterEntry.getValue();
-      log.add(new VisionLog(v, r));
-      lastSecondMeasurements.add(new MeasurementRecord(tag, v.timeOfMeasurement(),r));
-      switch (r) {
-        case ACCEPTED -> {
-          setVisionSTD(visionTruster.calculateTrust(v));
-          validMeasurementsPose.add(v.measurement());
-          addVisionMeasurement(v);
-          acceptedTagsPose.add(tag.getPose());
+    for (Map.Entry<Integer, Queue<VisionMeasurement>> queueEntry : visionMeasurementQueueMap.entrySet()) {
+      Queue<VisionMeasurement> visionMeasurementQueue = queueEntry.getValue();
+      LinkedHashMap<VisionMeasurement, FilterResult> filteredData =
+                filter.filter(visionMeasurementQueue,drivebase.getCameraPose());
+      visionMeasurementQueue.clear();
+      for (Map.Entry<VisionMeasurement, FilterResult> filterEntry : filteredData.entrySet()) {
+        VisionMeasurement v = filterEntry.getKey();
+        Apriltag tag = v.tag().tag();
+        FilterResult r = filterEntry.getValue();
+        log.add(new VisionLog(v, r));
+        lastSecondMeasurements.add(new MeasurementRecord(tag, v.timeOfMeasurement(), r));
+        switch (r) {
+          case ACCEPTED -> {
+            setVisionSTD(visionTruster.calculateTrust(v,drivebase.getCameraPose()));
+            validMeasurementsPose.add(v.measurement());
+            addVisionMeasurement(v);
+            acceptedTagsPose.add(tag.getPose());
 
-        }
-        case NOT_PROCESSED -> visionMeasurementQueue.add(v);
-        case REJECTED -> {
-          invalidMeasurementsPose.add(v.measurement());
+          }
+          case NOT_PROCESSED -> visionMeasurementQueue.add(v);
+          case REJECTED -> {
+            invalidMeasurementsPose.add(v.measurement());
+          }
         }
       }
     }
+  }
+  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());
     Logger.recordOutput("Apriltag/numberNotProcessedLastSecond", lastSecondMeasurements.stream().filter(record -> record.result == FilterResult.NOT_PROCESSED).count());
     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;
   }

src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java

@@ -16,10 +16,7 @@
 import frc.robot.constants.Constants;
 import frc.robot.subsystems.ApriltagSubsystem;
 import frc.robot.subsystems.swervedrive.SwerveSubsystem;
-import frc.robot.subsystems.swervedrive.vision.truster.BasicVisionFilter;
-import frc.robot.subsystems.swervedrive.vision.truster.ConstantVisionTruster;
-import frc.robot.subsystems.swervedrive.vision.truster.VisionMeasurement;
-import frc.robot.subsystems.swervedrive.vision.truster.VisionTruster;
+import frc.robot.subsystems.swervedrive.vision.truster.*;
 import frc.robot.utils.Apriltag;
 import frc.robot.utils.math.ArrayUtils;
 
@@ -51,7 +48,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<N3> visionMeasurementStdDevs2 = VecBuilder.fill(0.3, 0.3, 100);
+  public static final Vector<N3> visionMeasurementStdDevs2 = VecBuilder.fill(0, 0, 100);
   private final FilterablePoseManager poseManager;
 
   public PoseEstimator(
@@ -82,11 +79,11 @@ public PoseEstimator(
         TimeInterpolatableBuffer.createBuffer(Constants.POSE_BUFFER_STORAGE_TIME);
     this.poseManager =
         new FilterablePoseManager(
-            visionMeasurementStdDevs2,
+            Constants.INITIAL_VISION_STD_DEVS,
             kinematics,
             drivebase,
             m1Buffer,
-            new BasicVisionFilter(m1Buffer) {
+            new BasicVisionFilter(m1Buffer,truster) {
               @Override
               public Pose2d getVisionPose(VisionMeasurement measurement) {
                 return measurement.measurement();
@@ -129,7 +126,7 @@ private void updateVision(int... invalidApriltagNumbers) {
             && !ArrayUtils.contains(
                 invalidApriltagNumbers, apriltagSystem.getIO().getInputs().apriltagNumber[i])) {
           VisionMeasurement measurement = getVisionMeasurement(pos, i);
-          poseManager.registerVisionMeasurement(measurement);
+          poseManager.registerVisionMeasurement(measurement,measurement.tag().tag().number());
         } else {
           invalidCounter++;
           Logger.recordOutput("Apriltag/ValidationFailureCount", invalidCounter);
@@ -138,7 +135,8 @@ private void updateVision(int... invalidApriltagNumbers) {
     }
     long end = System.currentTimeMillis();
     Logger.recordOutput("RegisteringVisionTimeMillis", end - start);
-    poseManager.processQueue();
+        poseManager.processQueue();
+      poseManager.log();
   }
 
   private VisionMeasurement getVisionMeasurement(double[] pos, int index) {
@@ -155,7 +153,7 @@ private VisionMeasurement getVisionMeasurement(double[] pos, int index) {
    * are sent to the {@link PoseManager} for further processing
    */
   public void updateVision() {
-    updateVision(15, 4, 14, 5, 16, 3);
+    updateVision(0);
   }
 
   public void updateVision(Apriltag focusedTag) {
@@ -201,7 +199,7 @@ public FilterablePoseManager getPoseManager() {
 
   public void addMockVisionMeasurement() {
     poseManager.registerVisionMeasurement(
-        new VisionMeasurement(getEstimatedPose(), Apriltag.of(1).getTagInfo(), 0, Logger.getTimestamp() / 1e6));
+        new VisionMeasurement(getEstimatedPose(), Apriltag.of(1).getTagInfo(), 0, Logger.getTimestamp() / 1e6),1);
   }
     public VisionTruster getVisionTruster() {
         return poseManager.getVisionTruster();

src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseManager.java

@@ -12,6 +12,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;
@@ -23,7 +24,7 @@
 public class PoseManager {
     private final TimeInterpolatableBuffer<Pose2d> estimatedPoseBuffer;
     //private final SwerveDrivePoseEstimator poseEstimator;
-    protected final Queue<VisionMeasurement> visionMeasurementQueue = new LinkedList<>();
+    protected final LinkedHashMap<Integer, Queue<VisionMeasurement>> visionMeasurementQueueMap = new LinkedHashMap<>();
     protected final SwerveSubsystem drivebase;
     protected final VisionTruster visionTruster;
 
@@ -62,23 +63,25 @@ public void addOdomMeasurement(Pose2d pose, long timestamp) {
         estimatedPoseBuffer.addSample(timestamp, pose);
     }
 
-    public void registerVisionMeasurement(VisionMeasurement measurement) {
+    public void registerVisionMeasurement(VisionMeasurement measurement, int tagId) {
         if (measurement == null) {
             return;
         }
-        while (visionMeasurementQueue.size() >= 3) {
-            visionMeasurementQueue.poll();
+        while (visionMeasurementQueueMap.computeIfAbsent(tagId,k -> new LinkedList<>()).size() >= 3) {
+            visionMeasurementQueueMap.get(tagId).poll();
         }
-        visionMeasurementQueue.add(measurement);
+        visionMeasurementQueueMap.get(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<VisionMeasurement> visionMeasurementQueue : visionMeasurementQueueMap.values()) {
+            VisionMeasurement m = visionMeasurementQueue.poll();
+            while (m != null) {
+                setVisionSTD(visionTruster.calculateTrust(m,drivebase.getCameraPose()));
+                addVisionMeasurement(m);
+                m = visionMeasurementQueue.poll();
+            }
         }
     }