add test for empty case

Signed-off-by: Jade Turner <[email protected]>
PhotonVision · Feb 19, 2025 · 18d0b3a · 18d0b3a
1 parent 04ea1db
commit 18d0b3a
Showing 1 changed file with 13 additions and 0 deletions.
diff --git a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java
@@ -808,6 +808,7 @@ void testMultiTagOnRioFallback() {
                 () -> assertEquals(2, pose.getZ(), 1e-9));
     }
 
+    @Test
     public void testConstrainedPnpOneTag() {
         var distortion = VecBuilder.fill(0, 0, 0, 0, 0, 0, 0, 0);
         var cameraMat =
@@ -898,6 +899,18 @@ public void testConstrainedPnpOneTag() {
         System.out.println(pose);
     }
 
+    @Test
+    void testConstrainedPnpEmptyCase() {
+        PhotonPoseEstimator estimator =
+                new PhotonPoseEstimator(
+                        AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo),
+                        PoseStrategy.CONSTRAINED_SOLVEPNP,
+                        Transform3d.kZero);
+        PhotonPipelineResult result = new PhotonPipelineResult();
+        var estimate = estimator.update(result);
+        assertEquals(estimate, Optional.empty());
+    }
+
     private static class PhotonCameraInjector extends PhotonCamera {
         public PhotonCameraInjector() {
             super("Test");