From 2a59220dcb1124e1ac25d19a0ae5cf09eafc3fd1 Mon Sep 17 00:00:00 2001 From: Jack Lingle Date: Mon, 25 Nov 2024 19:50:51 -0500 Subject: [PATCH 1/5] start adding distance based deviations (failing to build due to static issues) --- .../main/java/coppercore/vision/Camera.java | 15 +++++++++-- .../coppercore/vision/CameraIOPhoton.java | 25 +++++++++++-------- .../vision/CoreVisionConstants.java | 7 ++++++ 3 files changed, 35 insertions(+), 12 deletions(-) diff --git a/vision/src/main/java/coppercore/vision/Camera.java b/vision/src/main/java/coppercore/vision/Camera.java index 08bd7aa..1f07857 100644 --- 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,19 @@ public CameraMeasurement getLatestMeasurement() { } public Matrix getLatestVariance() { + Matrix 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; + } + stdDev = stdDev.times(1 + (Math.pow(avgDistanceFromTarget, 2) / 30)); + } + return stdDev; } } diff --git a/vision/src/main/java/coppercore/vision/CameraIOPhoton.java b/vision/src/main/java/coppercore/vision/CameraIOPhoton.java index da313b8..27f7678 100644 --- a/vision/src/main/java/coppercore/vision/CameraIOPhoton.java +++ b/vision/src/main/java/coppercore/vision/CameraIOPhoton.java @@ -4,7 +4,6 @@ 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 +32,7 @@ public CameraIOPhoton( poseEstimator = new PhotonPoseEstimator( layout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, robotToCamera); + poseEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); } public static CameraIOPhoton fromRealCameraParams( @@ -91,19 +91,19 @@ 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; } - if (calculateAverageTagDistance(photonPose) - > CoreVisionConstants.maxAcceptedDistanceMeters) { + double avgDistanceFromTarget = calculateAverageTagDistance(photonPose); + + if (avgDistanceFromTarget > CoreVisionConstants.maxAcceptedDistanceMeters) { + return false; + } + + if (avgDistanceFromTarget > 4.0 && photonPose.targetsUsed.size() < 2) { return false; } @@ -113,10 +113,15 @@ private static boolean filterPhotonPose(EstimatedRobotPose photonPose) { private static double calculateAverageTagDistance(EstimatedRobotPose pose) { double distance = 0.0; for (PhotonTrackedTarget target : pose.targetsUsed) { + var tagPose = poseEstimator.getFieldTags().getTagPose(target.getFiducialId()); + if (tagPose.isEmpty()) { + continue; + } distance += - target.getBestCameraToTarget() + tagPose.get() + .toPose2d() .getTranslation() - .getDistance(new Translation3d()); + .getDistance(pose.get().estimatedPose.toPose2d().getTranslation()); } distance /= pose.targetsUsed.size(); diff --git a/vision/src/main/java/coppercore/vision/CoreVisionConstants.java b/vision/src/main/java/coppercore/vision/CoreVisionConstants.java index 432f992..b230365 100644 --- 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,6 @@ 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 singleTagStdDev = VecBuilder.fill(1, 1, 1); + public static final Matrix multiTagStdDev = VecBuilder.fill(0.5, 0.5, 0.5); } From 3a96a034e1330bdeeddf8a44e31334a623d91cc7 Mon Sep 17 00:00:00 2001 From: Jack Lingle Date: Tue, 26 Nov 2024 11:37:24 -0500 Subject: [PATCH 2/5] add method for 2025 commented out and update variances --- .../main/java/coppercore/vision/Camera.java | 4 +++ .../coppercore/vision/CameraIOPhoton.java | 27 ++++++------------- .../vision/CoreVisionConstants.java | 1 + 3 files changed, 13 insertions(+), 19 deletions(-) diff --git a/vision/src/main/java/coppercore/vision/Camera.java b/vision/src/main/java/coppercore/vision/Camera.java index 1f07857..7bc943b 100644 --- a/vision/src/main/java/coppercore/vision/Camera.java +++ b/vision/src/main/java/coppercore/vision/Camera.java @@ -48,7 +48,11 @@ public Matrix getLatestVariance() { } else { if (numTags > 1) { stdDev = CoreVisionConstants.multiTagStdDev; + } else if (numTags == 1 && avgDistanceFromTarget > 4) { + return CoreVisionConstants.rejectionStdDev; } + + // distance based variance stdDev = stdDev.times(1 + (Math.pow(avgDistanceFromTarget, 2) / 30)); } return stdDev; diff --git a/vision/src/main/java/coppercore/vision/CameraIOPhoton.java b/vision/src/main/java/coppercore/vision/CameraIOPhoton.java index 27f7678..8237930 100644 --- a/vision/src/main/java/coppercore/vision/CameraIOPhoton.java +++ b/vision/src/main/java/coppercore/vision/CameraIOPhoton.java @@ -72,8 +72,6 @@ public void updateInputs(CameraIOInputs inputs) { latestTimestampSeconds = result.getTimestampSeconds(); Optional photonPose = poseEstimator.update(result); - photonPose.filter(CameraIOPhoton::filterPhotonPose); - photonPose.ifPresentOrElse( (pose) -> { inputs.latestFieldToRobot = pose.estimatedPose; @@ -90,25 +88,16 @@ public void updateInputs(CameraIOInputs inputs) { }); } - private static boolean filterPhotonPose(EstimatedRobotPose photonPose) { - Pose3d pose = photonPose.estimatedPose; - // check that the pose isn't insane - if (pose.getZ() > 1 || pose.getZ() < -0.1) { - return false; - } - - double avgDistanceFromTarget = calculateAverageTagDistance(photonPose); + // NOTE: Can be used in 2025 code just not ready yet + // private Optional getEstimatedPose () { + // Optional visionEstimate = Optional.empty(); - if (avgDistanceFromTarget > CoreVisionConstants.maxAcceptedDistanceMeters) { - return false; - } + // for (var change : camera.getAllUnreadResults()) { + // visionEstimate = poseEstimator.update(change); + // } - if (avgDistanceFromTarget > 4.0 && photonPose.targetsUsed.size() < 2) { - return false; - } - - return true; - } + // return visionEstimate; + // } private static double calculateAverageTagDistance(EstimatedRobotPose pose) { double distance = 0.0; diff --git a/vision/src/main/java/coppercore/vision/CoreVisionConstants.java b/vision/src/main/java/coppercore/vision/CoreVisionConstants.java index b230365..612f8e1 100644 --- a/vision/src/main/java/coppercore/vision/CoreVisionConstants.java +++ b/vision/src/main/java/coppercore/vision/CoreVisionConstants.java @@ -16,4 +16,5 @@ public final class CoreVisionConstants { public static final double maxAcceptedDistanceMeters = 10.0; public static final Matrix singleTagStdDev = VecBuilder.fill(1, 1, 1); public static final Matrix multiTagStdDev = VecBuilder.fill(0.5, 0.5, 0.5); + public static final Matrix rejectionStdDev = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); } From 539f3bc376fcdf043ebb9a0f103ea06b1b9b7cc4 Mon Sep 17 00:00:00 2001 From: Jack Lingle Date: Tue, 26 Nov 2024 11:39:01 -0500 Subject: [PATCH 3/5] fix errors related to static attribute in average tag distance --- .../main/java/coppercore/vision/CameraIOPhoton.java | 5 ++--- .../java/coppercore/vision/CoreVisionConstants.java | 3 ++- .../src/main/java/frc/robot/BuildConstants.java | 12 ++++++------ 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/vision/src/main/java/coppercore/vision/CameraIOPhoton.java b/vision/src/main/java/coppercore/vision/CameraIOPhoton.java index 8237930..32ac513 100644 --- a/vision/src/main/java/coppercore/vision/CameraIOPhoton.java +++ b/vision/src/main/java/coppercore/vision/CameraIOPhoton.java @@ -1,7 +1,6 @@ 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 java.util.Optional; @@ -99,7 +98,7 @@ public void updateInputs(CameraIOInputs inputs) { // return visionEstimate; // } - private static double calculateAverageTagDistance(EstimatedRobotPose pose) { + private double calculateAverageTagDistance(EstimatedRobotPose pose) { double distance = 0.0; for (PhotonTrackedTarget target : pose.targetsUsed) { var tagPose = poseEstimator.getFieldTags().getTagPose(target.getFiducialId()); @@ -110,7 +109,7 @@ private static double calculateAverageTagDistance(EstimatedRobotPose pose) { tagPose.get() .toPose2d() .getTranslation() - .getDistance(pose.get().estimatedPose.toPose2d().getTranslation()); + .getDistance(pose.estimatedPose.toPose2d().getTranslation()); } distance /= pose.targetsUsed.size(); diff --git a/vision/src/main/java/coppercore/vision/CoreVisionConstants.java b/vision/src/main/java/coppercore/vision/CoreVisionConstants.java index 612f8e1..d8a3e6e 100644 --- a/vision/src/main/java/coppercore/vision/CoreVisionConstants.java +++ b/vision/src/main/java/coppercore/vision/CoreVisionConstants.java @@ -16,5 +16,6 @@ public final class CoreVisionConstants { public static final double maxAcceptedDistanceMeters = 10.0; public static final Matrix singleTagStdDev = VecBuilder.fill(1, 1, 1); public static final Matrix multiTagStdDev = VecBuilder.fill(0.5, 0.5, 0.5); - public static final Matrix rejectionStdDev = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); + public static final Matrix rejectionStdDev = + VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); } diff --git a/wpilib_interface/src/main/java/frc/robot/BuildConstants.java b/wpilib_interface/src/main/java/frc/robot/BuildConstants.java index d0d87a3..ba482ab 100644 --- a/wpilib_interface/src/main/java/frc/robot/BuildConstants.java +++ b/wpilib_interface/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = "com.github.team401"; public static final String MAVEN_NAME = "wpilib_interface"; public static final String VERSION = "0.0"; - public static final int GIT_REVISION = 38; - public static final String GIT_SHA = "05a9f4bc4c7a1a520f8134a8735bca511f8ef504"; - public static final String GIT_DATE = "2024-11-18 15:53:10 EST"; - public static final String GIT_BRANCH = "drive-command"; - public static final String BUILD_DATE = "2024-11-18 18:48:10 EST"; - public static final long BUILD_UNIX_TIME = 1731973690360L; + public static final int GIT_REVISION = 36; + public static final String GIT_SHA = "3a96a034e1330bdeeddf8a44e31334a623d91cc7"; + public static final String GIT_DATE = "2024-11-26 11:37:24 EST"; + public static final String GIT_BRANCH = "vision-distance"; + public static final String BUILD_DATE = "2024-11-26 11:38:39 EST"; + public static final long BUILD_UNIX_TIME = 1732639119513L; public static final int DIRTY = 1; private BuildConstants() {} From 84ed9947f3aeea05a8edc9ef817d2e215ab91346 Mon Sep 17 00:00:00 2001 From: Jack Lingle Date: Thu, 28 Nov 2024 11:29:16 -0500 Subject: [PATCH 4/5] unnest conditionals and add constants --- vision/src/main/java/coppercore/vision/Camera.java | 13 ++++++------- .../java/coppercore/vision/CoreVisionConstants.java | 2 ++ 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/vision/src/main/java/coppercore/vision/Camera.java b/vision/src/main/java/coppercore/vision/Camera.java index 7bc943b..97a8d2d 100644 --- a/vision/src/main/java/coppercore/vision/Camera.java +++ b/vision/src/main/java/coppercore/vision/Camera.java @@ -45,16 +45,15 @@ public Matrix getLatestVariance() { if (numTags == 0) { return stdDev; - } else { - if (numTags > 1) { + } else if (numTags > 1) { stdDev = CoreVisionConstants.multiTagStdDev; - } else if (numTags == 1 && avgDistanceFromTarget > 4) { + } else if (numTags == 1 && avgDistanceFromTarget > CoreVisionConstants.singleTagDistanceCutoff) { return CoreVisionConstants.rejectionStdDev; - } - - // distance based variance - stdDev = stdDev.times(1 + (Math.pow(avgDistanceFromTarget, 2) / 30)); } + + // distance based variance + stdDev = stdDev.times(1 + (Math.pow(avgDistanceFromTarget, 2) / CoreVisionConstants.distanceFactor)); + return stdDev; } } diff --git a/vision/src/main/java/coppercore/vision/CoreVisionConstants.java b/vision/src/main/java/coppercore/vision/CoreVisionConstants.java index d8a3e6e..47f8390 100644 --- a/vision/src/main/java/coppercore/vision/CoreVisionConstants.java +++ b/vision/src/main/java/coppercore/vision/CoreVisionConstants.java @@ -18,4 +18,6 @@ public final class CoreVisionConstants { public static final Matrix multiTagStdDev = VecBuilder.fill(0.5, 0.5, 0.5); public static final Matrix 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; } From 0a19bcddd6395beb8dd4328728b2ad8eefd3ffcb Mon Sep 17 00:00:00 2001 From: Jack Lingle Date: Thu, 28 Nov 2024 11:33:47 -0500 Subject: [PATCH 5/5] switch to counting tags --- vision/src/main/java/coppercore/vision/Camera.java | 13 +++++++++---- .../main/java/coppercore/vision/CameraIOPhoton.java | 10 ++++++---- .../src/main/java/frc/robot/BuildConstants.java | 10 +++++----- 3 files changed, 20 insertions(+), 13 deletions(-) diff --git a/vision/src/main/java/coppercore/vision/Camera.java b/vision/src/main/java/coppercore/vision/Camera.java index 97a8d2d..48c6597 100644 --- a/vision/src/main/java/coppercore/vision/Camera.java +++ b/vision/src/main/java/coppercore/vision/Camera.java @@ -46,13 +46,18 @@ public Matrix getLatestVariance() { if (numTags == 0) { return stdDev; } else if (numTags > 1) { - stdDev = CoreVisionConstants.multiTagStdDev; - } else if (numTags == 1 && avgDistanceFromTarget > CoreVisionConstants.singleTagDistanceCutoff) { - return CoreVisionConstants.rejectionStdDev; + 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)); + 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 index 32ac513..a76c79f 100644 --- a/vision/src/main/java/coppercore/vision/CameraIOPhoton.java +++ b/vision/src/main/java/coppercore/vision/CameraIOPhoton.java @@ -73,11 +73,10 @@ public void updateInputs(CameraIOInputs inputs) { 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; @@ -98,13 +97,15 @@ public void updateInputs(CameraIOInputs inputs) { // return visionEstimate; // } - private 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 += tagPose.get() .toPose2d() @@ -113,7 +114,8 @@ private double calculateAverageTagDistance(EstimatedRobotPose pose) { } distance /= pose.targetsUsed.size(); - return distance; + inputs.nTags = numTags; + inputs.averageTagDistanceM = distance; } private static Rotation2d calculateAverageTagYaw(EstimatedRobotPose pose) { diff --git a/wpilib_interface/src/main/java/frc/robot/BuildConstants.java b/wpilib_interface/src/main/java/frc/robot/BuildConstants.java index ba482ab..88b33f3 100644 --- a/wpilib_interface/src/main/java/frc/robot/BuildConstants.java +++ b/wpilib_interface/src/main/java/frc/robot/BuildConstants.java @@ -5,12 +5,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = "com.github.team401"; public static final String MAVEN_NAME = "wpilib_interface"; public static final String VERSION = "0.0"; - public static final int GIT_REVISION = 36; - public static final String GIT_SHA = "3a96a034e1330bdeeddf8a44e31334a623d91cc7"; - public static final String GIT_DATE = "2024-11-26 11:37:24 EST"; + public static final int GIT_REVISION = 38; + public static final String GIT_SHA = "84ed9947f3aeea05a8edc9ef817d2e215ab91346"; + public static final String GIT_DATE = "2024-11-28 11:29:16 EST"; public static final String GIT_BRANCH = "vision-distance"; - public static final String BUILD_DATE = "2024-11-26 11:38:39 EST"; - public static final long BUILD_UNIX_TIME = 1732639119513L; + public static final String BUILD_DATE = "2024-11-28 11:33:37 EST"; + public static final long BUILD_UNIX_TIME = 1732811617044L; public static final int DIRTY = 1; private BuildConstants() {}