modify PoseObservation logging, error logging struct

src/main/java/frc/robot/subsystems/vision/Vision.java

@@ -29,8 +29,6 @@ public void periodic() {
       for (var observation : observations) {
         if (observation.ambiguity() > AMBIGUITY_CUTOFF || observation.ambiguity() == -1) continue;
       }
-
-
     }
   }
 }

src/main/java/frc/robot/subsystems/vision/VisionIO.java

@@ -1,25 +1,22 @@
 package frc.robot.subsystems.vision;
 
 import edu.wpi.first.math.geometry.Pose3d;
-import java.util.LinkedList;
-import java.util.List;
 import org.littletonrobotics.junction.AutoLog;
 
 public interface VisionIO {
   @AutoLog
-  class VisionIOInputs {
+  public class VisionIOInputs {
     public boolean connected = false;
-    public List<PoseObservation> observations = new LinkedList<PoseObservation>();
+    public PoseObservation[] observations = new PoseObservation[0];
   }
 
-  default void updateInputs(VisionIOInputs inputs) {}
-
   // from EstimatedRobotPose
-  public record PoseObservation(
+  public static record PoseObservation(
       double timestamp,
       Pose3d estimatedPose,
       double ambiguity,
       int tagCount,
-      double averageDistance,
-      int[] tagIDs) {}
+      double averageDistance) {}
+
+  default void updateInputs(VisionIOInputs inputs) {}
 }

src/main/java/frc/robot/subsystems/vision/VisionIOPhotonvision.java

@@ -2,7 +2,6 @@
 
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.math.geometry.Transform3d;
-import java.util.ArrayList;
 import java.util.List;
 import java.util.Optional;
 import org.photonvision.EstimatedRobotPose;
@@ -28,9 +27,10 @@ public VisionIOPhotonvision(AprilTagFieldLayout fieldLayout, int index) {
   public void updateInputs(VisionIOInputs inputs) {
     inputs.connected = camera.isConnected();
     List<PhotonPipelineResult> results = camera.getAllUnreadResults();
-    List<PoseObservation> observations = new ArrayList<PoseObservation>();
+    PoseObservation[] observations = new PoseObservation[results.size()];
 
-    for (PhotonPipelineResult frame : results) {
+    for (int frameIndex = 0; frameIndex < results.size(); ++frameIndex) {
+      var frame = results.get(frameIndex);
       if (!frame.hasTargets()) return;
 
       Optional<EstimatedRobotPose> optEstimation = estimator.update(frame);
@@ -56,9 +56,8 @@ public void updateInputs(VisionIOInputs inputs) {
               estimation.estimatedPose,
               frame.getMultiTagResult().get().estimatedPose.ambiguity,
               tagIDs.length,
-              totalDistance / tagIDs.length, // FIXME check if valid
-              tagIDs);
-      observations.add(observation);
+              totalDistance / tagIDs.length);
+      observations[frameIndex] = observation;
     }
 
     inputs.observations = observations;