From bda804e08c5b739348436d799684ca9326f55342 Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Fri, 13 Feb 2026 19:25:50 -0800 Subject: [PATCH 1/6] Add Builder.withPoseFilter() --- .../vision/MultiPhotonPoseEstimator.java | 33 +++++++++++++++++++ 1 file changed, 33 insertions(+) diff --git a/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java b/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java index 01378fb1..1a80f197 100644 --- a/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java +++ b/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java @@ -34,6 +34,7 @@ import java.util.Map; import java.util.Objects; import java.util.Optional; +import java.util.function.Predicate; import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; @@ -63,6 +64,7 @@ */ public class MultiPhotonPoseEstimator implements AutoCloseable { private final List> cameraWrappers; + private final Predicate isPoseValid; private PhotonPoseEstimator.PoseStrategy poseEstimatorStrategy; /** A builder for {@code MultiPhotonPoseEstimator}. */ @@ -71,6 +73,7 @@ public static final class Builder { private final AprilTagFieldLayout aprilTagFieldLayout; private final NetworkTableInstance ntInstance; private final PhotonPoseEstimator.PoseStrategy poseEstimatorStrategy; + private Predicate isPoseValid = pose -> true; Builder( NetworkTableInstance ntInstance, @@ -98,6 +101,19 @@ public Builder addCamera(C camera) { return this; } + /** + * Sets the filter to use deciding which estimates to consider. + * + * @param isPoseValid A predicate that returns {@code true} if the pose should be considered + * valid. + * @return Builder instance. + * @since 2.1.0 + */ + public Builder withPoseFilter(Predicate isPoseValid) { + this.isPoseValid = isPoseValid; + return this; + } + /** Builds a configured MultiPhotonPoseEstimator. */ public MultiPhotonPoseEstimator build() { return new MultiPhotonPoseEstimator<>(this); @@ -233,6 +249,7 @@ public void close() { /** Creates an instance using values from a {@code Builder}. */ private MultiPhotonPoseEstimator(Builder builder) { poseEstimatorStrategy = builder.poseEstimatorStrategy; + isPoseValid = poseIsInField(builder.aprilTagFieldLayout).and(builder.isPoseValid); cameraWrappers = builder.cameras.values().stream() .map(camera -> createCameraWrapper(builder, camera)) @@ -377,6 +394,7 @@ public void processAllUnreadResults(PoseEstimateConsumer poseEstimateConsumer cameraWrapper.photonCamera.getAllUnreadResults().stream() .map(cameraWrapper.estimator::update) // PhotonPipelineResult -> EstimatedRobotPose .flatMap(Optional::stream) // Convert Stream> -> Stream

+ .filter(isPoseValid) .toList(); poses.forEach(pose -> poseEstimateConsumer.addEstimatedRobotPose(pose, cameraWrapper.camera)); @@ -389,4 +407,19 @@ public void close() { cameraWrappers.forEach(PhotonCameraWrapper::close); cameraWrappers.clear(); } + + /** Creates a predicate that determines if a pose is inside the field. */ + private static Predicate poseIsInField( + AprilTagFieldLayout aprilTagFieldLayout) { + return pose -> { + Pose3d estimate = pose.estimatedPose; + double x = estimate.getX(); + double y = estimate.getY(); + + return x >= 0.0 + && x <= aprilTagFieldLayout.getFieldLength() + && y >= 0.0 + && y <= aprilTagFieldLayout.getFieldWidth(); + }; + } } From 815f5a8a743bf167292df5d854d858edc6a5d0c7 Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Thu, 12 Feb 2026 19:21:14 -0800 Subject: [PATCH 2/6] Add overload of processAllUnreadResults() to support logging rejected poses --- .../vision/MultiPhotonPoseEstimator.java | 41 +++++++++++++++---- 1 file changed, 32 insertions(+), 9 deletions(-) diff --git a/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java b/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java index 1a80f197..a813da59 100644 --- a/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java +++ b/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java @@ -34,7 +34,9 @@ import java.util.Map; import java.util.Objects; import java.util.Optional; +import java.util.function.Consumer; import java.util.function.Predicate; +import java.util.stream.Collectors; import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; @@ -381,24 +383,45 @@ public void resetHeadingData(double timestampSeconds, Rotation3d heading) { } /** - * Sends all unread robot-pose estimations from all cameras to the provided consumer. + * Sends all validated unread robot-pose estimations from all cameras to the provided consumer. * - *

This method is supposed to be called from a routine updating drive-train pose with pose - * estimates from the photon vision cameras. + *

This method should be called from a routine updating drive-train pose with pose estimates + * from the photon vision cameras. * - * @param poseEstimateConsumer Functional interface for consuming computed pose estimates. + * @param poseEstimateConsumer Consumer for validated pose estimates. */ public void processAllUnreadResults(PoseEstimateConsumer poseEstimateConsumer) { + processAllUnreadResults(poseEstimateConsumer, pose -> {}); + } + + /** + * Sends all unread robot-pose estimations from all cameras to the provided consumers. + * + *

This method should be called from a routine updating drive-train pose with pose estimates + * from the photon vision cameras. + * + * @param poseEstimateConsumer Consumer for validated pose estimates. + * @param rejectedPoseConsumer Consumer for rejected pose estimates. + * @since 2.1.0 + */ + public void processAllUnreadResults( + PoseEstimateConsumer poseEstimateConsumer, + Consumer rejectedPoseConsumer) { for (PhotonCameraWrapper cameraWrapper : cameraWrappers) { - List poses = + Map> poses = cameraWrapper.photonCamera.getAllUnreadResults().stream() .map(cameraWrapper.estimator::update) // PhotonPipelineResult -> EstimatedRobotPose .flatMap(Optional::stream) // Convert Stream> -> Stream

- .filter(isPoseValid) - .toList(); + .collect(Collectors.partitioningBy(isPoseValid)); + + List validatedPoses = poses.get(Boolean.TRUE); + for (EstimatedRobotPose pose : validatedPoses) { + poseEstimateConsumer.addEstimatedRobotPose(pose, cameraWrapper.camera); + } + cameraWrapper.robotPosePublisher.publish(validatedPoses); - poses.forEach(pose -> poseEstimateConsumer.addEstimatedRobotPose(pose, cameraWrapper.camera)); - cameraWrapper.robotPosePublisher.publish(poses); + List rejectedPoses = poses.get(Boolean.FALSE); + rejectedPoses.forEach(rejectedPoseConsumer); } } From e75908e31e9b8c0c90f6274e98784ab53bb71f7a Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Sat, 14 Feb 2026 13:11:38 -0800 Subject: [PATCH 3/6] Add withMultiTagFallbackStrategy() --- .../vision/MultiPhotonPoseEstimator.java | 56 +++++++++++++++++-- 1 file changed, 52 insertions(+), 4 deletions(-) diff --git a/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java b/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java index a813da59..c4cf397b 100644 --- a/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java +++ b/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java @@ -67,7 +67,9 @@ public class MultiPhotonPoseEstimator implements AutoCloseable { private final List> cameraWrappers; private final Predicate isPoseValid; + private final PhotonPoseEstimator.PoseStrategy fallbackStrategy; private PhotonPoseEstimator.PoseStrategy poseEstimatorStrategy; + private boolean needHeadingData; /** A builder for {@code MultiPhotonPoseEstimator}. */ public static final class Builder { @@ -75,6 +77,8 @@ public static final class Builder { private final AprilTagFieldLayout aprilTagFieldLayout; private final NetworkTableInstance ntInstance; private final PhotonPoseEstimator.PoseStrategy poseEstimatorStrategy; + private PhotonPoseEstimator.PoseStrategy fallbackStrategy = + PhotonPoseEstimator.PoseStrategy.LOWEST_AMBIGUITY; private Predicate isPoseValid = pose -> true; Builder( @@ -116,6 +120,26 @@ public Builder withPoseFilter(Predicate isPoseValid) { return this; } + /** + * Set the Position Estimation Strategy used in multi-tag mode when only one tag can be seen. + * Must NOT be {@link PhotonPoseEstimator.PoseStrategy#MULTI_TAG_PNP_ON_COPROCESSOR} or {@link + * PhotonPoseEstimator.PoseStrategy#MULTI_TAG_PNP_ON_RIO}. + * + *

If this is not called, {@link PhotonPoseEstimator.PoseStrategy#LOWEST_AMBIGUITY} will be + * used. + * + * @param strategy the strategy to set + * @return Builder instance. + * @since 2.1.0 + */ + public Builder withMultiTagFallbackStrategy(PhotonPoseEstimator.PoseStrategy strategy) { + if (isMultiTagStrategy(strategy)) { + throw new IllegalArgumentException("Fallback strategy cannot be " + strategy); + } + fallbackStrategy = strategy; + return this; + } + /** Builds a configured MultiPhotonPoseEstimator. */ public MultiPhotonPoseEstimator build() { return new MultiPhotonPoseEstimator<>(this); @@ -251,11 +275,13 @@ public void close() { /** Creates an instance using values from a {@code Builder}. */ private MultiPhotonPoseEstimator(Builder builder) { poseEstimatorStrategy = builder.poseEstimatorStrategy; + fallbackStrategy = builder.fallbackStrategy; isPoseValid = poseIsInField(builder.aprilTagFieldLayout).and(builder.isPoseValid); cameraWrappers = builder.cameras.values().stream() .map(camera -> createCameraWrapper(builder, camera)) .collect(toCollection(ArrayList::new)); + needHeadingData = calculateNeedHeadingData(); } /** @@ -270,6 +296,7 @@ private static PhotonCameraWrapper createCameraWrapper( PhotonPoseEstimator estimator = new PhotonPoseEstimator( builder.aprilTagFieldLayout, builder.poseEstimatorStrategy, camera.robotToCamera()); + estimator.setMultiTagFallbackStrategy(builder.fallbackStrategy); // Create NetworkTables publishers for 1) the position of the camera relative to the robot and // 2) the estimated position provided by the camera. @@ -302,6 +329,7 @@ public void setPrimaryStrategy(PhotonPoseEstimator.PoseStrategy poseStrategy) { if (!poseStrategy.equals(poseEstimatorStrategy)) { cameraWrappers.forEach(wrapper -> wrapper.estimator.setPrimaryStrategy(poseStrategy)); poseEstimatorStrategy = poseStrategy; + needHeadingData = calculateNeedHeadingData(); } } @@ -311,10 +339,7 @@ public void setPrimaryStrategy(PhotonPoseEstimator.PoseStrategy poseStrategy) { * @return {@code true} if the pose strategy is documented to require addHeadingData(). */ public boolean poseStrategyRequiresHeadingData() { - return switch (poseEstimatorStrategy) { - case PNP_DISTANCE_TRIG_SOLVE, CONSTRAINED_SOLVEPNP -> true; - default -> false; - }; + return needHeadingData; } /** @@ -431,6 +456,29 @@ public void close() { cameraWrappers.clear(); } + private static boolean poseStrategyRequiresHeadingData( + PhotonPoseEstimator.PoseStrategy strategy) { + return switch (strategy) { + case PNP_DISTANCE_TRIG_SOLVE, CONSTRAINED_SOLVEPNP -> true; + default -> false; + }; + } + + private static boolean isMultiTagStrategy(PhotonPoseEstimator.PoseStrategy strategy) { + return switch (strategy) { + case MULTI_TAG_PNP_ON_COPROCESSOR, MULTI_TAG_PNP_ON_RIO -> true; + default -> false; + }; + } + + private boolean calculateNeedHeadingData() { + if (poseStrategyRequiresHeadingData(poseEstimatorStrategy)) { + return true; + } + return isMultiTagStrategy(poseEstimatorStrategy) + && poseStrategyRequiresHeadingData(fallbackStrategy); + } + /** Creates a predicate that determines if a pose is inside the field. */ private static Predicate poseIsInField( AprilTagFieldLayout aprilTagFieldLayout) { From 3990754e859bb6a02d3a55e7f4b1e379dadac3ce Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Mon, 9 Mar 2026 15:47:52 -0700 Subject: [PATCH 4/6] Add overload for addCamerasToSimulator() that takes in a BiConsumer --- .../vision/MultiPhotonPoseEstimator.java | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java b/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java index c4cf397b..e1682ba5 100644 --- a/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java +++ b/vision/src/main/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimator.java @@ -34,6 +34,7 @@ import java.util.Map; import java.util.Objects; import java.util.Optional; +import java.util.function.BiConsumer; import java.util.function.Consumer; import java.util.function.Predicate; import java.util.stream.Collectors; @@ -192,6 +193,21 @@ public static Builder builder( * @param simVisionSystem The simulated visual system. */ public void addCamerasToSimulator(VisionSystemSim simVisionSystem) { + addCamerasToSimulator(simVisionSystem, (camera, simCamera) -> {}); + } + + /** + * Adds all cameras to a simulated vision system, updating camera properties + * + *

Note that the robot code is responsible for calling {@link VisionSystemSim#update(Pose2d)} + * or {@link VisionSystemSim#update(Pose3d)} in {@code simulationPeriodic()}. + * + * @param simVisionSystem The simulated visual system. + * @param simCameraUpdater Callback that is called for each new simulated camera. + * @since 2.1.0 + */ + public void addCamerasToSimulator( + VisionSystemSim simVisionSystem, BiConsumer simCameraUpdater) { // Validate all inputs and create SimCameraProperties for each camera. Map cameraNameToSimProperties = cameraWrappers.stream() @@ -203,6 +219,7 @@ public void addCamerasToSimulator(VisionSystemSim simVisionSystem) { wrapper -> { SimCameraProperties cameraProps = cameraNameToSimProperties.get(wrapper.camera.name()); PhotonCameraSim simCamera = new PhotonCameraSim(wrapper.photonCamera, cameraProps); + simCameraUpdater.accept(wrapper.camera, simCamera); simVisionSystem.addCamera(simCamera, wrapper.estimator.getRobotToCameraTransform()); }); } From 01a4f8e4e38a4fc5bc9f9e91ab0a80eee5d67814 Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Mon, 9 Mar 2026 16:28:00 -0700 Subject: [PATCH 5/6] Upgrade photonlib to v2026.1.1-rc-2 --- vision/vendordeps/photonlib.json | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/vision/vendordeps/photonlib.json b/vision/vendordeps/photonlib.json index a1bc5a57..0a948bd3 100644 --- a/vision/vendordeps/photonlib.json +++ b/vision/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2026.1.1", + "version": "v2026.1.1-rc-2", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2026", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2026.1.1", + "version": "v2026.1.1-rc-2", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -28,7 +28,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2026.1.1", + "version": "v2026.1.1-rc-2", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -43,7 +43,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2026.1.1", + "version": "v2026.1.1-rc-2", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -60,12 +60,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2026.1.1" + "version": "v2026.1.1-rc-2" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2026.1.1" + "version": "v2026.1.1-rc-2" } ] } \ No newline at end of file From 024489d1b9e9e454aab8c356d6324a8e59011485 Mon Sep 17 00:00:00 2001 From: Kevin Cooney Date: Mon, 9 Mar 2026 14:07:39 -0700 Subject: [PATCH 6/6] Add tests for processAllUnreadResults() --- .../vision/MultiPhotonPoseEstimatorTest.java | 190 ++++++++++++++++-- .../lib2813/vision/ReefscapeAprilTag.java | 68 +++++++ 2 files changed, 237 insertions(+), 21 deletions(-) create mode 100644 vision/src/test/java/com/team2813/lib2813/vision/ReefscapeAprilTag.java diff --git a/vision/src/test/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimatorTest.java b/vision/src/test/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimatorTest.java index 447ad23b..5399a9c9 100644 --- a/vision/src/test/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimatorTest.java +++ b/vision/src/test/java/com/team2813/lib2813/vision/MultiPhotonPoseEstimatorTest.java @@ -16,54 +16,202 @@ package com.team2813.lib2813.vision; import static com.google.common.truth.Truth.assertThat; +import static com.team2813.lib2813.testing.truth.Pose3dSubject.assertThat; +import static edu.wpi.first.units.Units.Meters; +import com.team2813.lib2813.testing.junit.jupiter.InitWPILib; import com.team2813.lib2813.testing.junit.jupiter.ProvideUniqueNetworkTableInstance; -import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Quaternion; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.units.measure.Distance; +import java.util.ArrayList; import java.util.List; +import java.util.function.Consumer; +import java.util.stream.Stream; import org.junit.jupiter.params.ParameterizedTest; +import org.junit.jupiter.params.provider.Arguments; import org.junit.jupiter.params.provider.EnumSource; +import org.junit.jupiter.params.provider.MethodSource; +import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VisionSystemSim; +import org.photonvision.targeting.PhotonTrackedTarget; /** Tests for {@link MultiPhotonPoseEstimator}. */ @ProvideUniqueNetworkTableInstance +@InitWPILib class MultiPhotonPoseEstimatorTest { - private static final double FIELD_LENGTH = 17.548; - private static final double FIELD_WIDTH = 8.052; - private static final int REEFSCAPE_APRIL_TAG_ID = 7; - private static final Pose3d REEFSCAPE_APRIL_TAG_POSE = - new Pose3d( - new Translation3d(13.890498, 4.0259, 0.308102), - new Rotation3d(new Quaternion(1.0, 0.0, 0.0, 0.0))); + // Place the camera in the center of the robot, ~17.1cm up, facing forward and up. private static final Transform3d FRONT_CAMERA_TRANSFORM = - new Transform3d( - 0.1688157406, - 0.2939800826, - 0.1708140348, - new Rotation3d(0, -0.1745329252, -0.5235987756)); + new Transform3d(0, 0, 0.1708140348, new Rotation3d(0, -0.1745329252, 0)); - private static final Camera FRONT_CAMERA = new Camera("front", FRONT_CAMERA_TRANSFORM); + private static final Camera FRONT_CAMERA = + new Camera("front", FRONT_CAMERA_TRANSFORM, SimCameraProperties::PERFECT_90DEG); @ParameterizedTest @EnumSource(value = PoseStrategy.class) void getPrimaryStrategy(PoseStrategy poseStrategy, NetworkTableInstance ntInstance) { try (var estimator = - MultiPhotonPoseEstimator.builder(ntInstance, createFieldLayout(), poseStrategy) + MultiPhotonPoseEstimator.builder( + ntInstance, ReefscapeAprilTag.createFieldLayout(), poseStrategy) .addCamera(FRONT_CAMERA) .build()) { assertThat(estimator.getPrimaryStrategy()).isEqualTo(poseStrategy); } } - private static AprilTagFieldLayout createFieldLayout() { - List aprilTags = - List.of(new AprilTag(REEFSCAPE_APRIL_TAG_ID, REEFSCAPE_APRIL_TAG_POSE)); - return new AprilTagFieldLayout(aprilTags, FIELD_LENGTH, FIELD_WIDTH); + private record PoseTestData(Pose3d robotPose, ReefscapeAprilTag aprilTag) {} + + private static Stream posesInField() { + return Stream.of( + facingAprilTag(ReefscapeAprilTag.RED_REEF_CENTER, Meters.of(1)), + facingAprilTag(ReefscapeAprilTag.BLUE_REEF_CENTER, Meters.of(1)), + facingAprilTag(ReefscapeAprilTag.RED_PROCESSOR, Meters.of(0.5)), + facingAprilTag(ReefscapeAprilTag.BLUE_PROCESSOR, Meters.of(0.5))); + } + + @ParameterizedTest(name = "{0}") + @MethodSource("posesInField") + void processAllUnreadResults_estimatedPoseInField( + String testName, PoseTestData testData, NetworkTableInstance ntInstance) { + AprilTagFieldLayout fieldLayout = ReefscapeAprilTag.createFieldLayout(testData.aprilTag); + VisionSystemSim visionSystemSim = new VisionSystemSim("test"); + visionSystemSim.addAprilTags(fieldLayout); + + double z = testData.aprilTag.toAprilTag().pose.getZ(); + Camera camera = + new Camera( + "front", + new Transform3d(0, 0, z, FRONT_CAMERA_TRANSFORM.getRotation()), + SimCameraProperties::PERFECT_90DEG); + + try (var estimator = + MultiPhotonPoseEstimator.builder(ntInstance, fieldLayout, PoseStrategy.LOWEST_AMBIGUITY) + .addCamera(camera) + .build()) { + estimator.addCamerasToSimulator( + visionSystemSim, + (c, simCamera) -> { + simCamera.enableRawStream(false); + simCamera.enableProcessedStream(false); + }); + visionSystemSim.update(testData.robotPose); + + var estimateCollector = new EstimateCollector(); + var rejectedPoseCollector = new RejectedPoseCollector(); + + // Call the method under test + estimator.processAllUnreadResults(estimateCollector, rejectedPoseCollector); + + assertThat(estimateCollector.estimates).hasSize(1); + assertThat(rejectedPoseCollector.rejectedPoses).isEmpty(); + assertThat(estimateCollector.estimates.get(0).estimatedPose) + .isWithin(0.01) + .of(testData.robotPose); + } + } + + private static Stream posesOutOfField() { + return Stream.of( + facingAprilTag(ReefscapeAprilTag.RED_REEF_CENTER, Meters.of(4)), + facingAprilTag(ReefscapeAprilTag.BLUE_REEF_CENTER, Meters.of(4)), + facingAprilTag(ReefscapeAprilTag.RED_PROCESSOR, Meters.of(8.1)), + facingAprilTag(ReefscapeAprilTag.BLUE_PROCESSOR, Meters.of(8.1))); + } + + @ParameterizedTest(name = "{0}") + @MethodSource("posesOutOfField") + void processAllUnreadResults_estimatedPoseOutsideField( + String testName, PoseTestData testData, NetworkTableInstance ntInstance) { + AprilTagFieldLayout fieldLayout = ReefscapeAprilTag.createFieldLayout(testData.aprilTag); + VisionSystemSim visionSystemSim = new VisionSystemSim("test"); + visionSystemSim.addAprilTags(fieldLayout); + + double z = testData.aprilTag.toAprilTag().pose.getZ(); + Camera camera = + new Camera( + "front", + new Transform3d(0, 0, z, FRONT_CAMERA_TRANSFORM.getRotation()), + SimCameraProperties::PERFECT_90DEG); + + try (var estimator = + MultiPhotonPoseEstimator.builder(ntInstance, fieldLayout, PoseStrategy.LOWEST_AMBIGUITY) + .addCamera(camera) + .build()) { + estimator.addCamerasToSimulator( + visionSystemSim, + (c, simCamera) -> { + simCamera.enableRawStream(false); + simCamera.enableProcessedStream(false); + }); + visionSystemSim.update(testData.robotPose); + + var estimateCollector = new EstimateCollector(); + var rejectedPoseCollector = new RejectedPoseCollector(); + + // Call the method under test + estimator.processAllUnreadResults(estimateCollector, rejectedPoseCollector); + + assertThat(estimateCollector.estimates).isEmpty(); + assertThat(rejectedPoseCollector.rejectedPoses).hasSize(1); + assertThat(targetsUsed(rejectedPoseCollector.rejectedPoses.get(0))) + .containsExactly(testData.aprilTag.id()); + } + } + + private static List targetsUsed(EstimatedRobotPose estimatedPose) { + return estimatedPose.targetsUsed.stream().map(PhotonTrackedTarget::getFiducialId).toList(); + } + + /** Creates test data with a position that is the given distance away from the given AprilTag. */ + private static Arguments facingAprilTag(ReefscapeAprilTag tag, Distance distanceFromTag) { + Pose2d closestTagPose = tag.toAprilTag().pose.toPose2d(); + Rotation2d tagRotation = closestTagPose.getRotation(); + double distance = distanceFromTag.in(Meters); + Translation2d translation = + new Translation2d(distance * tagRotation.getCos(), distance * tagRotation.getSin()); + + Pose2d robotPose = + new Pose2d( + closestTagPose.getTranslation().plus(translation), + tagRotation.rotateBy(Rotation2d.k180deg)); + + var testName = String.format("%.1fmFrom%s", distance, toCamelCase(tag.name())); + return Arguments.of(testName, new PoseTestData(new Pose3d(robotPose), tag)); + } + + private static String toCamelCase(String s) { + StringBuilder camelCaseString = new StringBuilder(); + for (String part : s.split("_")) { + camelCaseString.append(part.substring(0, 1).toUpperCase()); + camelCaseString.append(part.substring(1).toLowerCase()); + } + return camelCaseString.toString(); + } + + private static class EstimateCollector implements PoseEstimateConsumer { + final List estimates = new ArrayList<>(); + + @Override + public void addEstimatedRobotPose(EstimatedRobotPose estimatedPose, Camera camera) { + assertThat(camera.name()).isEqualTo(FRONT_CAMERA.name()); + estimates.add(estimatedPose); + } + } + + private static class RejectedPoseCollector implements Consumer { + final List rejectedPoses = new ArrayList<>(); + + @Override + public void accept(EstimatedRobotPose estimatedRobotPose) { + rejectedPoses.add(estimatedRobotPose); + } } } diff --git a/vision/src/test/java/com/team2813/lib2813/vision/ReefscapeAprilTag.java b/vision/src/test/java/com/team2813/lib2813/vision/ReefscapeAprilTag.java new file mode 100644 index 00000000..6c9169b0 --- /dev/null +++ b/vision/src/test/java/com/team2813/lib2813/vision/ReefscapeAprilTag.java @@ -0,0 +1,68 @@ +/* +Copyright 2026 Prospect Robotics SWENext Club + +Licensed under the Apache License, Version 2.0 (the "License"); +you may not use this file except in compliance with the License. +You may obtain a copy of the License at + +http://www.apache.org/licenses/LICENSE-2.0 + +Unless required by applicable law or agreed to in writing, software +distributed under the License is distributed on an "AS IS" BASIS, +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +See the License for the specific language governing permissions and +limitations under the License. +*/ +package com.team2813.lib2813.vision; + +import edu.wpi.first.apriltag.AprilTag; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.geometry.Pose3d; +import java.util.List; +import java.util.Optional; + +/** Contains AprilTags for the Reefscape Welded field. */ +enum ReefscapeAprilTag { + RED_REEF_CENTER(7), + BLUE_REEF_CENTER(18), + RED_PROCESSOR(3), + BLUE_PROCESSOR(16); + + /** Creates a (mutable) AprilTag for this enum value. */ + AprilTag toAprilTag() { + Optional tagPose = memoizedFieldLayout().getTagPose(tagId); + return new AprilTag(tagId, tagPose.orElseThrow()); + } + + public int id() { + return tagId; + } + + /** Creates a field layout for Reefscape Welded with the origin set to zero. */ + static AprilTagFieldLayout createFieldLayout() { + return AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded); + } + + /** Creates a field layout with the dimensions of Reefscape Welded and the given tag. */ + static AprilTagFieldLayout createFieldLayout(ReefscapeAprilTag tag) { + var fieldLayout = memoizedFieldLayout(); + return new AprilTagFieldLayout( + List.of(tag.toAprilTag()), fieldLayout.getFieldLength(), fieldLayout.getFieldWidth()); + } + + private static AprilTagFieldLayout possiblyNullFieldLayout; + + private static AprilTagFieldLayout memoizedFieldLayout() { + if (possiblyNullFieldLayout == null) { + possiblyNullFieldLayout = createFieldLayout(); + } + return possiblyNullFieldLayout; + } + + ReefscapeAprilTag(int tagId) { + this.tagId = tagId; + } + + private final int tagId; +}