Vision Logic (fixed version)

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

@@ -73,6 +73,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;
@@ -123,7 +124,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

@@ -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,20 +39,20 @@ 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, 0);
-                        Vector<N3> 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, tag.number());
+                        Vector<N3> 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);
                         Pose2d readingPos = new Pose2d(readingX, readingY, Rotation2d.fromDegrees(readingYaw));
                         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));
                         }
                     }
                 }

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> {
 
@@ -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);
     }
 }

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
@@ -169,6 +169,9 @@ 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;
+    public static final double VISION_STD_DEV_CONST = 1.0/148.0; // TODO: Change later
 
     public static final double MAX_HUB_DISTANCE = 5.18;
     public static final double MIN_HUB_DISTANCE = 1.42;

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;
@@ -318,6 +319,9 @@ public void resetOdometry(Pose2d initialHolonomicPose) {
     public Pose2d getPose() {
         return swerveDrive.getPose();
     }
+    public Pose3d getCameraPose() {
+        return new Pose3d(getPose()).transformBy(Constants.ROBOT_TO_CAMERA);
+    }
     public double getError() {
         return getPose().getTranslation().getDistance((getSimulationPose().get().getTranslation()));
     }

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

@@ -1,11 +1,13 @@
 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;
 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;
@@ -26,6 +28,7 @@
  */
 public class FilterablePoseManager extends PoseManager {
   private final VisionFilter filter;
+  private List<Pose3d> acceptedTagPoses = new ArrayList<>();
 
   public FilterablePoseManager(
       PoseDeviation PoseDeviation,
@@ -57,28 +60,40 @@ public FilterablePoseManager(
   public void processQueue() {
     List<Pose2d> validMeasurementsPose = new ArrayList<>();
     List<Pose2d> invalidMeasurementsPose = new ArrayList<>();
-
-    LinkedHashMap<VisionMeasurement, FilterResult> filteredData =
+    if (Constants.currentMode == Constants.Mode.SIM) {
+      acceptedTagPoses = new ArrayList<>();
+    }
+    for (Map.Entry<Integer, Queue<VisionMeasurement>> queueEntry : visionMeasurementQueueMap.entrySet()) {
+      Queue<VisionMeasurement> visionMeasurementQueue = queueEntry.getValue();
+      LinkedHashMap<VisionMeasurement, FilterResult> filteredData =
               filter.filter(visionMeasurementQueue);
-    visionMeasurementQueue.clear();
-    for (Map.Entry<VisionMeasurement, FilterResult> filterEntry : filteredData.entrySet()) {
-      VisionMeasurement v = filterEntry.getKey();
-      FilterResult r = filterEntry.getValue();
-      switch (r) {
-        case ACCEPTED -> {
-          setVisionSTD(getVisionSTD(v));
-          validMeasurementsPose.add(v.measurement());
-          addVisionMeasurement(v);
+      visionMeasurementQueue.clear();
 
-        }
-        case NOT_PROCESSED -> visionMeasurementQueue.add(v);
-        case REJECTED -> {
-          invalidMeasurementsPose.add(v.measurement());
+      for (Map.Entry<VisionMeasurement, FilterResult> filterEntry : filteredData.entrySet()) {
+        VisionMeasurement v = filterEntry.getKey();
+        FilterResult r = filterEntry.getValue();
+        switch (r) {
+          case ACCEPTED -> {
+            setVisionSTD(getVisionSTD(v));
+            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);
+          case REJECTED -> {
+            invalidMeasurementsPose.add(v.measurement());
+          }
         }
       }
     }
     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() {

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<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(
@@ -145,14 +145,15 @@ private VisionMeasurement getVisionMeasurement(double[] pos, int index) {
     //double timestamp = 0; // latency is not right we are assuming zero
     double timestamp = apriltagSystem.getIO().getInputs().timestamp[index];
     double stdDevFromCamera = 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());
     } else {
         visionPose = new Pose2d(pos[0], pos[1], poseManager.getRotation());
     }
     double distanceFromTag = apriltagSystem.getIO().getInputs().distanceToTag[index];
-    return new VisionMeasurement(visionPose, distanceFromTag, stdDevFromCamera, timestamp/1000);
+    return new VisionMeasurement(visionPose, distanceFromTag, stdDevFromCamera,timestamp/1000, tagId);
   }
 
   /**
@@ -206,7 +207,7 @@ public FilterablePoseManager getPoseManager() {
 
   public void addMockVisionMeasurement() {
     poseManager.registerVisionMeasurement(
-        new VisionMeasurement(getEstimatedPose(), 0, 0, Logger.getTimestamp() / 1e6));
+        new VisionMeasurement(getEstimatedPose(), 0, 0, Logger.getTimestamp() / 1e6, 0));
   }
     public VisionTruster getVisionTruster() {
         return poseManager.getVisionTruster();

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

@@ -3,6 +3,7 @@
 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.interpolation.TimeInterpolatableBuffer;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
@@ -15,6 +16,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;
@@ -26,7 +28,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<>();
     private final SwerveSubsystem drivebase;
     protected final VisionTruster visionTruster;
 
@@ -69,19 +71,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(getVisionSTD(m));
-            addVisionMeasurement(m);
-            m = visionMeasurementQueue.poll();
+        for (Queue<VisionMeasurement> visionMeasurementQueue : visionMeasurementQueueMap.values()) {
+            VisionMeasurement m = visionMeasurementQueue.poll();
+            while (m != null) {
+                setVisionSTD(getVisionSTD(m));
+                addVisionMeasurement(m);
+                m = visionMeasurementQueue.poll();
+            }
         }
     }
 
@@ -94,7 +98,8 @@ protected Vector<N3> getVisionSTD(VisionMeasurement measurement) {
         if (Constants.USE_CAMERA_APRILTAG_STD_DEV) {
             return VecBuilder.fill(StdDev, StdDev, StdDev);
         }
-        return visionTruster.calculateTrust(measurement);
+        Pose3d cameraPos = drivebase.getCameraPose();
+        return visionTruster.calculateTrust(measurement, cameraPos);
     }
 
     protected void addVisionMeasurement(VisionMeasurement measurement) {

src/main/java/frc/robot/subsystems/swervedrive/vision/truster/BasicVisionFilter.java

@@ -51,10 +51,8 @@ public LinkedHashMap<VisionMeasurement, FilterResult> filter(
       Filter poses
       -------------------------------------------------------
       */
-      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 +61,22 @@ public LinkedHashMap<VisionMeasurement, FilterResult> filter(
     return resultMap;
   }
 
-  private boolean filterVision(Pose2d m1Pose, Pose2d m2Pose, double m1Time, double m2Time) {
-    Optional<Pose2d> odomPoseAtVis1 = poseBuffer.getSample(m1Time);
-    Optional<Pose2d> odomPoseAtVis2 = poseBuffer.getSample(m2Time);
+  private boolean filterVision(VisionMeasurement m1, VisionMeasurement m2) {
+    Optional<Pose2d> odomPoseAtVis1 = poseBuffer.getSample(m1.timeOfMeasurement());
+    Optional<Pose2d> 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.stdDev() > Constants.VISION_STD_THRESHOLD) {
+      return false;
+    }
     return Math.abs(diff) <= Constants.VISION_CONSISTENCY_THRESHOLD;
   }
 

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<N3> initialSTD) {
   }
 
   @Override
-  public Vector<N3> calculateTrust(VisionMeasurement measurement) {
+  public Vector<N3> calculateTrust(VisionMeasurement measurement, Pose3d cameraPose) {
     return initialSTD;
   }
 }

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<N3> initialSTD, double slopeSTD) {
   }
 
   @Override
-  public Vector<N3> calculateTrust(VisionMeasurement measurement) {
+  public Vector<N3> calculateTrust(VisionMeasurement measurement, Pose3d cameraPose) {
     double std = measurement.distanceFromTag() * slopeSTD;
     return initialSTD.plus(VecBuilder.fill(std, std, std));
   }