add code to add all poses under robotPoses

team401 · Dec 12, 2024 · 64a8a14 · 64a8a14
1 parent 28bd4f3
commit 64a8a14
Show file tree

Hide file tree

Showing 2 changed files with 2 additions and 9 deletions.
diff --git a/vision/src/main/java/coppercore/vision/CoreVisionConstants.java b/vision/src/main/java/coppercore/vision/CoreVisionConstants.java
@@ -19,14 +19,6 @@ public final class CoreVisionConstants {
     public static final Matrix<N3, N1> rejectionStdDev =
             VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
     public static final double singleTagDistanceCutoff = 4.0;
-    public static final double distanceFactor = 30.0;
+    public static final double distanceFactor = 1.0;
     public static final double maxZCutoff = 1.0;
-
-    // Standard deviation multipliers for each camera
-    // (Adjust to trust some cameras more than others)
-    public static double[] cameraStdDevFactors =
-            new double[] {
-                1.0, // Camera 0
-                1.0 // Camera 1
-            };
 }
diff --git a/vision/src/main/java/coppercore/vision/VisionLocalizer.java b/vision/src/main/java/coppercore/vision/VisionLocalizer.java
@@ -74,6 +74,7 @@ public void periodic() {
             List<Pose3d> robotPosesRejected = new LinkedList<>();
 
             for (VisionIO.PoseObservation observation : inputs[cameraIndex].poseObservations) {
+                robotPoses.add(observation.pose());
                 if (shouldRejectPose(observation)) {
                     robotPosesRejected.add(observation.pose());
                     continue;