From 2ab1708ad701b4cc4c1137aff8cde55664024ca5 Mon Sep 17 00:00:00 2001 From: Joe Lockwood Date: Sat, 4 Jul 2026 10:39:49 -0400 Subject: [PATCH 1/2] fix(photon-lib): do not crash constrained solvepnp when heading-free with empty buffer estimateConstrainedSolvepnpPose only checked for a heading sample when headingFree was false, but then unconditionally sampled the heading buffer again and unwrapped it when building the solver call. A user running CONSTRAINED_SOLVEPNP with headingFree=true who never called addHeadingData therefore got a NoSuchElementException (Java) or std::bad_optional_access (C++) instead of a pose estimate, even though the heading-free solver never reads that heading: the generated casadi cost functions for the heading-free problems never dereference the gyro_theta argument (no arg[6] access in any constrained_solvepnp_*_tags_free.c), only the heading-fixed variants do. Sample the buffer once per invocation, keep returning empty when the heading is fixed and no sample exists, and fall back to a zero Rotation2d placeholder in heading-free mode. This also removes the redundant double/triple buffer sampling per estimate. Add a Java regression test that runs the heading-free strategy with no heading data and asserts it does not throw. --- .../org/photonvision/PhotonPoseEstimator.java | 20 ++++---- .../native/cpp/photon/PhotonPoseEstimator.cpp | 25 +++++----- .../photonvision/PhotonPoseEstimatorTest.java | 48 +++++++++++++++++++ 3 files changed, 71 insertions(+), 22 deletions(-) diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index 537b18ff44..0d81994aa1 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -366,17 +366,17 @@ public Optional estimateConstrainedSolvepnpPose( if (!shouldEstimate(cameraResult)) { return Optional.empty(); } + var headingSampleOpt = headingBuffer.getSample(cameraResult.getTimestampSeconds()); // Need heading if heading fixed + if (!headingFree && headingSampleOpt.isEmpty()) { + return Optional.empty(); + } + // The solver's heading-free cost function never reads the provided heading, so any + // placeholder value works when the buffer is 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 +388,7 @@ public Optional estimateConstrainedSolvepnpPose( 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 diff --git a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp index fb10bbe3a7..e8c5e7b380 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp @@ -483,17 +483,20 @@ PhotonPoseEstimator::EstimateConstrainedSolvepnpPose( if (!ShouldEstimate(cameraResult)) { return std::nullopt; } + std::optional headingSampleOpt = + headingBuffer.Sample(cameraResult.GetTimestamp()); // Need heading if heading fixed + if (!headingFree && !headingSampleOpt) { + return std::nullopt; + } + // The solver's heading-free cost function never reads the provided heading, + // so any placeholder value works when the buffer is empty + 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 targets{ cameraResult.GetTargets().begin(), cameraResult.GetTargets().end()}; @@ -501,9 +504,7 @@ PhotonPoseEstimator::EstimateConstrainedSolvepnpPose( std::optional 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) { diff --git a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java b/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java index b698fc697a..58b2af7c1f 100644 --- a/photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java +++ b/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 @@ public void testConstrainedPnpOneTag() { 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 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"); From 3f6045776f5b18d58210509191f43f1ee50f730a Mon Sep 17 00:00:00 2001 From: Joe Lockwood Date: Tue, 7 Jul 2026 16:24:46 -0400 Subject: [PATCH 2/2] docs(photon-lib): drop unhelpful heading-free seed comment Address review feedback on #2529: remove the comment describing why an empty heading buffer is safe in heading-free mode. It described this specific fix rather than documenting the code; the guard above and the .orElse/.value_or placeholder are self-explanatory. Removed on both Java and C++. --- .../src/main/java/org/photonvision/PhotonPoseEstimator.java | 2 -- photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp | 2 -- 2 files changed, 4 deletions(-) diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java index 0d81994aa1..266374ef5c 100644 --- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java @@ -371,8 +371,6 @@ public Optional estimateConstrainedSolvepnpPose( if (!headingFree && headingSampleOpt.isEmpty()) { return Optional.empty(); } - // The solver's heading-free cost function never reads the provided heading, so any - // placeholder value works when the buffer is empty var headingSample = headingSampleOpt.orElse(Rotation2d.kZero); if (!headingFree) { // If heading fixed, force rotation component diff --git a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp index e8c5e7b380..cbd91c4373 100644 --- a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp +++ b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp @@ -489,8 +489,6 @@ PhotonPoseEstimator::EstimateConstrainedSolvepnpPose( if (!headingFree && !headingSampleOpt) { return std::nullopt; } - // The solver's heading-free cost function never reads the provided heading, - // so any placeholder value works when the buffer is empty wpi::math::Rotation2d headingSample = headingSampleOpt.value_or(wpi::math::Rotation2d{}); if (!headingFree) {