From 64a8a145a4bd4227ad2d5d97a5b596814aec2e10 Mon Sep 17 00:00:00 2001
From: Jack Lingle <captainjack0818@gmail.com>
Date: Thu, 12 Dec 2024 08:49:03 -0500
Subject: [PATCH] add code to add all poses under robotPoses

---
 .../java/coppercore/vision/CoreVisionConstants.java    | 10 +---------
 .../main/java/coppercore/vision/VisionLocalizer.java   |  1 +
 2 files changed, 2 insertions(+), 9 deletions(-)

diff --git a/vision/src/main/java/coppercore/vision/CoreVisionConstants.java b/vision/src/main/java/coppercore/vision/CoreVisionConstants.java
index 02d4ceb..2ac2404 100644
--- 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
index a8e5d3d..48a7391 100644
--- 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;