fix(photon-lib): do not crash constrained solvepnp when heading-free with empty buffer

photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java

@@ -176,7 +176,7 @@
      *
      * <p>By default, this is {@link TargetModel#kAprilTag36h11}.
      */
     public TargetModel getTagModel() {
         return tagModel;
     }
 
@@ -240,7 +240,7 @@
     }
 
     /**
      * @return The current transform from the center of the robot to the camera mount position
      */
     public Transform3d getRobotToCameraTransform() {
         return robotToCamera;
@@ -366,17 +366,15 @@
         if (!shouldEstimate(cameraResult)) {
             return Optional.empty();
         }
+        var headingSampleOpt = headingBuffer.getSample(cameraResult.getTimestampSeconds());
         // Need heading if heading fixed
+        if (!headingFree && headingSampleOpt.isEmpty()) {
+            return Optional.empty();
+        }
+        var headingSample = headingSampleOpt.orElse(Rotation2d.kZero);
         if (!headingFree) {
-            if (headingBuffer.getSample(cameraResult.getTimestampSeconds()).isEmpty()) {
-                return Optional.empty();
-            } else {
-                // If heading fixed, force rotation component
-                seedPose =
-                        new Pose3d(
-                                seedPose.getTranslation(),
-                                new Rotation3d(headingBuffer.getSample(cameraResult.getTimestampSeconds()).get()));
-            }
+            // If heading fixed, force rotation component
+            seedPose = new Pose3d(seedPose.getTranslation(), new Rotation3d(headingSample));
         }
         var pnpResult =
                 VisionEstimation.estimateRobotPoseConstrainedSolvepnp(
@@ -388,7 +386,7 @@
                         fieldTags,
                         tagModel,
                         headingFree,
-                        headingBuffer.getSample(cameraResult.getTimestampSeconds()).get(),
+                        headingSample,
                         headingScaleFactor);
         if (!pnpResult.isPresent()) return Optional.empty();
         var best = Pose3d.kZero.plus(pnpResult.get().best); // field-to-robot

photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp

@@ -483,27 +483,26 @@ PhotonPoseEstimator::EstimateConstrainedSolvepnpPose(
   if (!ShouldEstimate(cameraResult)) {
     return std::nullopt;
   }
+  std::optional<wpi::math::Rotation2d> headingSampleOpt =
+      headingBuffer.Sample(cameraResult.GetTimestamp());
   // Need heading if heading fixed
+  if (!headingFree && !headingSampleOpt) {
+    return std::nullopt;
+  }
+  wpi::math::Rotation2d headingSample =
+      headingSampleOpt.value_or(wpi::math::Rotation2d{});
   if (!headingFree) {
-    if (!headingBuffer.Sample(cameraResult.GetTimestamp())) {
-      return std::nullopt;
-    } else {
-      // If heading fixed, force rotation component
-      seedPose = wpi::math::Pose3d{
-          seedPose.Translation(),
-          wpi::math::Rotation3d{
-              headingBuffer.Sample(cameraResult.GetTimestamp()).value()}};
-    }
+    // If heading fixed, force rotation component
+    seedPose = wpi::math::Pose3d{seedPose.Translation(),
+                                 wpi::math::Rotation3d{headingSample}};
   }
   std::vector<photon::PhotonTrackedTarget> targets{
       cameraResult.GetTargets().begin(), cameraResult.GetTargets().end()};
 
   std::optional<photon::PnpResult> pnpResult =
       VisionEstimation::EstimateRobotPoseConstrainedSolvePNP(
           cameraMatrix, distCoeffs, targets, m_robotToCamera, seedPose,
-          aprilTags, photon::kAprilTag36h11, headingFree,
-          wpi::math::Rotation2d{
-              headingBuffer.Sample(cameraResult.GetTimestamp()).value()},
+          aprilTags, photon::kAprilTag36h11, headingFree, headingSample,
           headingScaleFactor);
 
   if (!pnpResult) {

photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java

@@ -24,6 +24,7 @@
 
 package org.photonvision;
 
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertNotNull;
@@ -941,6 +942,53 @@
         System.out.println(pose);
     }
 
+    @Test
+    public void testConstrainedPnpHeadingFreeEmptyHeadingBuffer() {
+        var distortion = VecBuilder.fill(0, 0, 0, 0, 0, 0, 0, 0);
+        var cameraMat =
+                MatBuilder.fill(
+                        Nat.N3(),
+                        Nat.N3(),
+                        399.37500000000006,
+                        0,
+                        319.5,
+                        0,
+                        399.16666666666674,
+                        239.5,
+                        0,
+                        0,
+                        1);
+
+        List<TargetCorner> corners8 =
+                List.of(
+                        new TargetCorner(98.09875447066685, 331.0093220119495),
+                        new TargetCorner(122.20226758624413, 335.50083894738486),
+                        new TargetCorner(127.17118732489361, 313.81406314178633),
+                        new TargetCorner(104.28543773760417, 309.6516557438994));
+
+        var result =
+                new PhotonPipelineResult(
+                        new PhotonPipelineMetadata(10000, 2000, 1, 100),
+                        List.of(
+                                new PhotonTrackedTarget(0, 0, 0, 0, 8, 0, 0, null, null, 0, corners8, corners8)),
+                        Optional.empty());
+
+        final double camPitch = Units.degreesToRadians(30.0);
+        final Transform3d kRobotToCam =
+                new Transform3d(new Translation3d(0.5, 0.0, 0.5), new Rotation3d(0, -camPitch, 0));
+
+        PhotonPoseEstimator estimator =
+                new PhotonPoseEstimator(
+                        AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo), kRobotToCam);
+
+        // No addHeadingData calls -- in heading-free mode an empty heading buffer must
+        // not throw (the estimate may still be empty for other reasons)
+        assertDoesNotThrow(
+                () ->
+                        estimator.estimateConstrainedSolvepnpPose(
+                                result, cameraMat, distortion, new Pose3d(), true, 0));
+    }
+
     private static class PhotonCameraInjector extends PhotonCamera {
         public PhotonCameraInjector() {
             super("Test");
@@ -954,7 +1002,7 @@
         }
 
         @Override
         public PhotonPipelineResult getLatestResult() {
             return result;
         }
     }