Vision distance (#51)

* start adding distance based deviations (failing to build due to static issues) * add method for 2025 commented out and update variances * fix errors related to static attribute in average tag distance * unnest conditionals and add constants * switch to counting tags * start adding distance based deviations (failing to build due to static issues) * add method for 2025 commented out and update variances * fix errors related to static attribute in average tag distance * unnest conditionals and add constants * switch to counting tags * add buildconstants for now
team401 · Dec 4, 2024 · b23b2d3 · b23b2d3
1 parent 994c331
commit b23b2d3
Show file tree

Hide file tree

Showing 4 changed files with 54 additions and 30 deletions.
diff --git a/vision/src/main/java/coppercore/vision/Camera.java b/vision/src/main/java/coppercore/vision/Camera.java
@@ -2,7 +2,6 @@
 
 import coppercore.vision.VisionLocalizer.CameraMeasurement;
 import edu.wpi.first.math.Matrix;
-import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
 import org.littletonrobotics.junction.Logger;
@@ -39,7 +38,27 @@ public CameraMeasurement getLatestMeasurement() {
     }
 
     public Matrix<N3, N1> getLatestVariance() {
+        Matrix<N3, N1> stdDev = CoreVisionConstants.singleTagStdDev;
         // TODO: Actually calculate variances!
-        return VecBuilder.fill(0.0, 0.0, 0.0);
+        double avgDistanceFromTarget = inputs.averageTagDistanceM;
+        int numTags = inputs.nTags;
+
+        if (numTags == 0) {
+            return stdDev;
+        } else if (numTags > 1) {
+            stdDev = CoreVisionConstants.multiTagStdDev;
+        } else if (numTags == 1
+                && avgDistanceFromTarget > CoreVisionConstants.singleTagDistanceCutoff) {
+            return CoreVisionConstants.rejectionStdDev;
+        }
+
+        // distance based variance
+        stdDev =
+                stdDev.times(
+                        1
+                                + (Math.pow(avgDistanceFromTarget, 2)
+                                        / CoreVisionConstants.distanceFactor));
+
+        return stdDev;
     }
 }
diff --git a/vision/src/main/java/coppercore/vision/CameraIOPhoton.java b/vision/src/main/java/coppercore/vision/CameraIOPhoton.java
@@ -1,10 +1,8 @@
 package coppercore.vision;
 
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
-import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Transform3d;
-import edu.wpi.first.math.geometry.Translation3d;
 import java.util.Optional;
 import org.photonvision.EstimatedRobotPose;
 import org.photonvision.PhotonCamera;
@@ -33,6 +31,7 @@ public CameraIOPhoton(
         poseEstimator =
                 new PhotonPoseEstimator(
                         layout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, robotToCamera);
+        poseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);
     }
 
     public static CameraIOPhoton fromRealCameraParams(
@@ -72,15 +71,12 @@ public void updateInputs(CameraIOInputs inputs) {
         latestTimestampSeconds = result.getTimestampSeconds();
         Optional<EstimatedRobotPose> photonPose = poseEstimator.update(result);
 
-        photonPose.filter(CameraIOPhoton::filterPhotonPose);
-
         photonPose.ifPresentOrElse(
                 (pose) -> {
+                    calculateAverageTagDistance(pose, inputs);
                     inputs.latestFieldToRobot = pose.estimatedPose;
-                    inputs.nTags = pose.targetsUsed.size();
 
                     inputs.latestTimestampSeconds = this.latestTimestampSeconds;
-                    inputs.averageTagDistanceM = calculateAverageTagDistance(pose);
                     inputs.averageTagYaw = calculateAverageTagYaw(pose);
 
                     inputs.wasAccepted = true;
@@ -90,37 +86,36 @@ public void updateInputs(CameraIOInputs inputs) {
                 });
     }
 
-    private static boolean filterPhotonPose(EstimatedRobotPose photonPose) {
-        // TODO: Actual pose rejection
-        if (photonPose.targetsUsed.size() < 2) {
-            return false;
-        }
-
-        Pose3d pose = photonPose.estimatedPose;
-        // check that the pose isn't insane
-        if (pose.getZ() > 1 || pose.getZ() < -0.1) {
-            return false;
-        }
+    // NOTE: Can be used in 2025 code just not ready yet
+    // private Optional<EstimatedRobotPose> getEstimatedPose () {
+    //     Optional<EstimatedRobotPose> visionEstimate = Optional.empty();
 
-        if (calculateAverageTagDistance(photonPose)
-                > CoreVisionConstants.maxAcceptedDistanceMeters) {
-            return false;
-        }
+    //     for (var change : camera.getAllUnreadResults()) {
+    //         visionEstimate = poseEstimator.update(change);
+    //     }
 
-        return true;
-    }
+    //     return visionEstimate;
+    // }
 
-    private static double calculateAverageTagDistance(EstimatedRobotPose pose) {
+    private void calculateAverageTagDistance(EstimatedRobotPose pose, CameraIOInputs inputs) {
         double distance = 0.0;
+        int numTags = 0;
         for (PhotonTrackedTarget target : pose.targetsUsed) {
+            var tagPose = poseEstimator.getFieldTags().getTagPose(target.getFiducialId());
+            if (tagPose.isEmpty()) {
+                continue;
+            }
+            numTags += 1;
             distance +=
-                    target.getBestCameraToTarget()
+                    tagPose.get()
+                            .toPose2d()
                             .getTranslation()
-                            .getDistance(new Translation3d());
+                            .getDistance(pose.estimatedPose.toPose2d().getTranslation());
         }
         distance /= pose.targetsUsed.size();
 
-        return distance;
+        inputs.nTags = numTags;
+        inputs.averageTagDistanceM = distance;
     }
 
     private static Rotation2d calculateAverageTagYaw(EstimatedRobotPose pose) {

diff --git a/vision/src/main/java/coppercore/vision/CoreVisionConstants.java b/vision/src/main/java/coppercore/vision/CoreVisionConstants.java
@@ -1,5 +1,10 @@
 package coppercore.vision;
 
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+
 /**
  * Vision constants for coppercore's internal vision constants. Not to be confused with
  * VisionConstants, which is likely your robot project's local vision constants that are configured
@@ -9,4 +14,10 @@ public final class CoreVisionConstants {
     // TODO: Tune this value
     // This value mainly exists so that this file stays named CoreVisionConstants.
     public static final double maxAcceptedDistanceMeters = 10.0;
+    public static final Matrix<N3, N1> singleTagStdDev = VecBuilder.fill(1, 1, 1);
+    public static final Matrix<N3, N1> multiTagStdDev = VecBuilder.fill(0.5, 0.5, 0.5);
+    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;
 }
diff --git a/wpilib_interface/src/.gitignore b/wpilib_interface/src/.gitignore
@@ -1 +0,0 @@
-frc/