Clarify when estimate methods return empty (#2323)

## Description Clarifies when estimate methods return an empty optional. This also fixes some parity issues with Constrained SolvePnP not checking the optional returned by the heading buffer. Resolves #2322. ## Meta Merge checklist: - [x] Pull Request title is [short, imperative summary](https://cbea.ms/git-commit/) of proposed changes - [x] The description documents the _what_ and _why_ - [ ] If this PR changes behavior or adds a feature, user documentation is updated - [ ] If this PR touches photon-serde, all messages have been regenerated and hashes have not changed unexpectedly - [ ] If this PR touches configuration, this is backwards compatible with settings back to v2025.3.2 - [ ] If this PR touches pipeline settings or anything related to data exchange, the frontend typing is updated - [ ] If this PR addresses a bug, a regression test for it is added
2026-06-29 02:21:41 +00:00 · 2026-01-21 18:13:47 -05:00
parent 12f74423d9
commit e73420d62a
4 changed files with 46 additions and 22 deletions
--- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
+++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
@@ -686,7 +686,7 @@ public class PhotonPoseEstimator {
     *
     * @param cameraResult A pipeline result from the camera.
     * @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
-     *     create the estimate.
+     *     create the estimate, or an empty optional if there's no targets or heading data.
     */
    public Optional<EstimatedRobotPose> estimatePnpDistanceTrigSolvePose(
            PhotonPipelineResult cameraResult) {
@@ -758,7 +758,8 @@ public class PhotonPoseEstimator {
     * @param headingScaleFactor If headingFree is false, this weights the cost of changing our robot
     *     heading estimate against the tag corner reprojection error cont.
     * @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
-     *     create the estimate.
+     *     create the estimate, or an empty optional if there's no targets or heading data, or if the
+     *     solver fails to solve the problem.
     */
    public Optional<EstimatedRobotPose> estimateConstrainedSolvepnpPose(
            PhotonPipelineResult cameraResult,
@@ -770,6 +771,18 @@ public class PhotonPoseEstimator {
        if (!shouldEstimate(cameraResult)) {
            return Optional.empty();
        }
+        // Need heading if heading fixed
+        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()));
+            }
+        }
        var pnpResult =
                VisionEstimation.estimateRobotPoseConstrainedSolvepnp(
                        cameraMatrix,
@@ -799,7 +812,8 @@ public class PhotonPoseEstimator {
     *
     * @param cameraResult A pipeline result from the camera.
     * @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
-     *     create the estimate.
+     *     create the estimate, or an empty optional if there's no targets, no multi-tag results, or
+     *     multi-tag is disabled in the web UI.
     */
    public Optional<EstimatedRobotPose> estimateCoprocMultiTagPose(
            PhotonPipelineResult cameraResult) {
@@ -829,7 +843,8 @@ public class PhotonPoseEstimator {
     * @param cameraMatrix Camera intrinsics from camera calibration data
     * @param distCoeffs Distortion coefficients from camera calibration data.
     * @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
-     *     create the estimate.
+     *     create the estimate, or an empty optional if there's less than 2 targets visible or
+     *     SolvePnP fails.
     */
    public Optional<EstimatedRobotPose> estimateRioMultiTagPose(
            PhotonPipelineResult cameraResult, Matrix<N3, N3> cameraMatrix, Matrix<N8, N1> distCoeffs) {
@@ -861,7 +876,7 @@ public class PhotonPoseEstimator {
     *
     * @param cameraResult A pipeline result from the camera.
     * @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
-     *     create the estimate.
+     *     create the estimate, or an empty optional if there's no targets.
     */
    public Optional<EstimatedRobotPose> estimateLowestAmbiguityPose(
            PhotonPipelineResult cameraResult) {
@@ -911,7 +926,7 @@ public class PhotonPoseEstimator {
     *
     * @param cameraResult A pipeline result from the camera.
     * @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
-     *     create the estimate.
+     *     create the estimate, or an empty optional if there's no targets.
     */
    public Optional<EstimatedRobotPose> estimateClosestToCameraHeightPose(
            PhotonPipelineResult cameraResult) {
@@ -989,7 +1004,7 @@ public class PhotonPoseEstimator {
     * @param cameraResult A pipeline result from the camera.
     * @param referencePose reference pose to check vector magnitude difference against.
     * @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
-     *     create the estimate.
+     *     create the estimate, or an empty optional if there's no targets.
     */
    public Optional<EstimatedRobotPose> estimateClosestToReferencePose(
            PhotonPipelineResult cameraResult, Pose3d referencePose) {
@@ -1062,7 +1077,7 @@ public class PhotonPoseEstimator {
     *
     * @param cameraResult A pipeline result from the camera.
     * @return An {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
-     *     create the estimate.
+     *     create the estimate, or an empty optional if there's no targets.
     */
    public Optional<EstimatedRobotPose> estimateAverageBestTargetsPose(
            PhotonPipelineResult cameraResult) {
--- a/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp
+++ b/photon-lib/src/main/native/cpp/photon/PhotonPoseEstimator.cpp
@@ -634,13 +634,18 @@ PhotonPoseEstimator::EstimateConstrainedSolvepnpPose(
  if (!ShouldEstimate(cameraResult)) {
    return std::nullopt;
  }
+  // Need heading if heading fixed
  if (!headingFree) {
-    seedPose = frc::Pose3d{
-        seedPose.Translation(),
-        frc::Rotation3d{
-            headingBuffer.Sample(cameraResult.GetTimestamp()).value()}};
+    if (!headingBuffer.Sample(cameraResult.GetTimestamp())) {
+      return std::nullopt;
+    } else {
+      // If heading fixed, force rotation component
+      seedPose = frc::Pose3d{
+          seedPose.Translation(),
+          frc::Rotation3d{
+              headingBuffer.Sample(cameraResult.GetTimestamp()).value()}};
+    }
  }
-
  std::vector<photon::PhotonTrackedTarget> targets{
      cameraResult.GetTargets().begin(), cameraResult.GetTargets().end()};

--- a/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h
+++ b/photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h
@@ -294,7 +294,7 @@ class PhotonPoseEstimator {
   *
   * @param cameraResult A pipeline result from the camera.
   * @return An EstimatedRobotPose with an estimated pose, timestamp, and
-   * targets used to create the estimate.
+   * targets used to create the estimate, or std::nullopt if there's no targets.
   */
  std::optional<EstimatedRobotPose> EstimateLowestAmbiguityPose(
      PhotonPipelineResult cameraResult);
@@ -306,7 +306,7 @@ class PhotonPoseEstimator {
   *
   * @param cameraResult A pipeline result from the camera.
   * @return An EstimatedRobotPose with an estimated pose, timestamp and
-   * targets used to create the estimate.
+   * targets used to create the estimate, or std::nullopt if there's no targets.
   */
  std::optional<EstimatedRobotPose> EstimateClosestToCameraHeightPose(
      PhotonPipelineResult cameraResult);
@@ -319,7 +319,7 @@ class PhotonPoseEstimator {
   * @param referencePose reference pose to check vector magnitude difference
   * against.
   * @return An EstimatedRobotPose with an estimated pose, timestamp, and
-   * targets used to create the estimate.
+   * targets used to create the estimate, or std::nullopt if there's no targets.
   */
  std::optional<EstimatedRobotPose> EstimateClosestToReferencePose(
      PhotonPipelineResult cameraResult, frc::Pose3d referencePose);
@@ -331,7 +331,8 @@ class PhotonPoseEstimator {
   *
   * @param cameraResult A pipeline result from the camera.
   * @return An EstimatedRobotPose with an estimated pose, timestamp, and
-   * targets used to create the estimate.
+   * targets used to create the estimate or std::nullopt if there's no targets,
+   * no multi-tag results, or multi-tag is disabled in the web UI.
   */
  std::optional<EstimatedRobotPose> EstimateCoprocMultiTagPose(
      PhotonPipelineResult cameraResult);
@@ -345,7 +346,8 @@ class PhotonPoseEstimator {
   * @param cameraMatrix Camera intrinsics from camera calibration data.
   * @param distCoeffs Distortion coefficients from camera calibration data.
   * @return An EstimatedRobotPose with an estimated pose, timestamp, and
-   * targets used to create the estimate.
+   * targets used to create the estimate, or std::nullopt if there's less than 2
+   * targets visible or SolvePnP fails.
   */
  std::optional<EstimatedRobotPose> EstimateRioMultiTagPose(
      PhotonPipelineResult cameraResult, PhotonCamera::CameraMatrix camMat,
@@ -363,7 +365,8 @@ class PhotonPoseEstimator {
   *
   * @param cameraResult A pipeline result from the camera.
   * @return An EstimatedRobotPose with an estimated pose, timestamp, and
-   * targets used to create the estimate.
+   * targets used to create the estimate, or std::nullopt if there's no targets
+   * or heading data.
   */
  std::optional<EstimatedRobotPose> EstimatePnpDistanceTrigSolvePose(
      PhotonPipelineResult cameraResult);
@@ -372,7 +375,7 @@ class PhotonPoseEstimator {
   * Return the average of the best target poses using ambiguity as weight.
   * @param cameraResult A pipeline result from the camera.
   * @return An EstimatedRobotPose with an estimated pose, timestamp, and
-   * targets used to create the estimate.
+   * targets used to create the estimate, or std::nullopt if there's no targets.
   */
  std::optional<EstimatedRobotPose> EstimateAverageBestTargetsPose(
      PhotonPipelineResult cameraResult);
@@ -401,7 +404,8 @@ class PhotonPoseEstimator {
   * changing our robot heading estimate against the tag corner reprojection
   * error cost.
   * @return An EstimatedRobotPose with an estimated pose, timestamp, and
-   * targets used to create the estimate.
+   * targets used to create the estimate, or std::nullopt if there's no targets
+   * or heading data, or if the solver fails to solve the problem.
   */
  std::optional<EstimatedRobotPose> EstimateConstrainedSolvepnpPose(
      photon::PhotonPipelineResult cameraResult,