Fix fallback for the multitag on rio pose strategy (#1755)

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

@@ -286,8 +286,8 @@ public class PhotonPoseEstimator {
      *   <li>No targets were found in the pipeline results.
      * </ul>
      *
-     * Will report a warning if strategy is multi-tag-on-rio, but camera calibration data is not
-     * provided
+     * Will report a warning if strategy is multi-tag-on-rio because camera calibration data is not
+     * provided in this overload.
      *
      * @param cameraResult The latest pipeline result from the camera
      * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
@@ -341,6 +341,20 @@ public class PhotonPoseEstimator {
         return update(cameraResult, cameraMatrix, distCoeffs, this.primaryStrategy);
     }
 
+    /**
+     * Internal convenience method for using a fallback strategy for update(). This should only be
+     * called after timestamp checks have been done by another update() overload.
+     *
+     * @param cameraResult The latest pipeline result from the camera
+     * @param strategy The pose strategy to use
+     * @return an {@link EstimatedRobotPose} with an estimated pose, timestamp, and targets used to
+     *     create the estimate.
+     */
+    private Optional<EstimatedRobotPose> update(
+            PhotonPipelineResult cameraResult, PoseStrategy strategy) {
+        return update(cameraResult, Optional.empty(), Optional.empty(), strategy);
+    }
+
     private Optional<EstimatedRobotPose> update(
             PhotonPipelineResult cameraResult,
             Optional<Matrix<N3, N3>> cameraMatrix,
@@ -357,21 +371,8 @@ public class PhotonPoseEstimator {
                         yield closestToReferencePoseStrategy(cameraResult, referencePose);
                     }
                     case AVERAGE_BEST_TARGETS -> averageBestTargetsStrategy(cameraResult);
-                    case MULTI_TAG_PNP_ON_RIO -> {
-                        if (cameraMatrix.isEmpty()) {
-                            DriverStation.reportWarning(
-                                    "Camera matrix is empty for multi-tag-on-rio",
-                                    Thread.currentThread().getStackTrace());
-                            yield Optional.empty();
-                        } else if (distCoeffs.isEmpty()) {
-                            DriverStation.reportWarning(
-                                    "Camera matrix is empty for multi-tag-on-rio",
-                                    Thread.currentThread().getStackTrace());
-                            yield Optional.empty();
-                        } else {
-                            yield multiTagOnRioStrategy(cameraResult, cameraMatrix, distCoeffs);
-                        }
-                    }
+                    case MULTI_TAG_PNP_ON_RIO ->
+                            multiTagOnRioStrategy(cameraResult, cameraMatrix, distCoeffs);
                     case MULTI_TAG_PNP_ON_COPROCESSOR -> multiTagOnCoprocStrategy(cameraResult);
                 };
 
@@ -383,41 +384,44 @@ public class PhotonPoseEstimator {
     }
 
     private Optional<EstimatedRobotPose> multiTagOnCoprocStrategy(PhotonPipelineResult result) {
-        if (result.getMultiTagResult().isPresent()) {
-            var best_tf = result.getMultiTagResult().get().estimatedPose.best;
-            var best =
-                    new Pose3d()
-                            .plus(best_tf) // field-to-camera
-                            .relativeTo(fieldTags.getOrigin())
-                            .plus(robotToCamera.inverse()); // field-to-robot
-            return Optional.of(
-                    new EstimatedRobotPose(
-                            best,
-                            result.getTimestampSeconds(),
-                            result.getTargets(),
-                            PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR));
-        } else {
-            // We can never fall back on another multitag strategy
-            return update(result, Optional.empty(), Optional.empty(), this.multiTagFallbackStrategy);
+        if (result.getMultiTagResult().isEmpty()) {
+            return update(result, this.multiTagFallbackStrategy);
         }
+
+        var best_tf = result.getMultiTagResult().get().estimatedPose.best;
+        var best =
+                new Pose3d()
+                        .plus(best_tf) // field-to-camera
+                        .relativeTo(fieldTags.getOrigin())
+                        .plus(robotToCamera.inverse()); // field-to-robot
+        return Optional.of(
+                new EstimatedRobotPose(
+                        best,
+                        result.getTimestampSeconds(),
+                        result.getTargets(),
+                        PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR));
     }
 
     private Optional<EstimatedRobotPose> multiTagOnRioStrategy(
             PhotonPipelineResult result,
             Optional<Matrix<N3, N3>> cameraMatrixOpt,
             Optional<Matrix<N8, N1>> distCoeffsOpt) {
-        boolean hasCalibData = cameraMatrixOpt.isPresent() && distCoeffsOpt.isPresent();
-        // cannot run multitagPNP, use fallback strategy
-        if (!hasCalibData || result.getTargets().size() < 2) {
-            return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy);
+        if (cameraMatrixOpt.isEmpty() || distCoeffsOpt.isEmpty()) {
+            DriverStation.reportWarning(
+                    "No camera calibration data provided for multi-tag-on-rio",
+                    Thread.currentThread().getStackTrace());
+            return update(result, this.multiTagFallbackStrategy);
+        }
+
+        if (result.getTargets().size() < 2) {
+            return update(result, this.multiTagFallbackStrategy);
         }
 
         var pnpResult =
                 VisionEstimation.estimateCamPosePNP(
                         cameraMatrixOpt.get(), distCoeffsOpt.get(), result.getTargets(), fieldTags, tagModel);
         // try fallback strategy if solvePNP fails for some reason
-        if (!pnpResult.isPresent())
-            return update(result, cameraMatrixOpt, distCoeffsOpt, this.multiTagFallbackStrategy);
+        if (!pnpResult.isPresent()) return update(result, this.multiTagFallbackStrategy);
         var best =
                 new Pose3d()
                         .plus(pnpResult.get().best) // field-to-camera

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

@@ -350,18 +350,17 @@ frc::Pose3d detail::ToPose3d(const cv::Mat& tvec, const cv::Mat& rvec) {
 
 std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnCoprocStrategy(
     PhotonPipelineResult result) {
-  if (result.MultiTagResult()) {
-    const auto field2camera = result.MultiTagResult()->estimatedPose.best;
-
-    const auto fieldToRobot =
-        frc::Pose3d() + field2camera + m_robotToCamera.Inverse();
-    return photon::EstimatedRobotPose(fieldToRobot, result.GetTimestamp(),
-                                      result.GetTargets(),
-                                      MULTI_TAG_PNP_ON_COPROCESSOR);
+  if (!result.MultiTagResult()) {
+    return Update(result, this->multiTagFallbackStrategy);
   }
 
-  return Update(result, std::nullopt, std::nullopt,
-                this->multiTagFallbackStrategy);
+  const auto field2camera = result.MultiTagResult()->estimatedPose.best;
+
+  const auto fieldToRobot =
+      frc::Pose3d() + field2camera + m_robotToCamera.Inverse();
+  return photon::EstimatedRobotPose(fieldToRobot, result.GetTimestamp(),
+                                    result.GetTargets(),
+                                    MULTI_TAG_PNP_ON_COPROCESSOR);
 }
 
 std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(
@@ -370,19 +369,17 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(
     std::optional<PhotonCamera::DistortionMatrix> distCoeffs) {
   using namespace frc;
 
-  // Need at least 2 targets
-  if (!result.HasTargets() || result.GetTargets().size() < 2) {
-    return Update(result, std::nullopt, std::nullopt,
-                  this->multiTagFallbackStrategy);
-  }
-
   if (!camMat || !distCoeffs) {
     FRC_ReportError(frc::warn::Warning,
                     "No camera calibration data provided to "
                     "PhotonPoseEstimator::MultiTagOnRioStrategy!",
                     "");
-    return Update(result, std::nullopt, std::nullopt,
-                  this->multiTagFallbackStrategy);
+    return Update(result, this->multiTagFallbackStrategy);
+  }
+
+  // Need at least 2 targets
+  if (!result.HasTargets() || result.GetTargets().size() < 2) {
+    return Update(result, this->multiTagFallbackStrategy);
   }
 
   auto const targets = result.GetTargets();
@@ -408,7 +405,7 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::MultiTagOnRioStrategy(
 
   // We should only do multi-tag if at least 2 tags (* 4 corners/tag)
   if (imagePoints.size() < 8) {
-    return Update(result, camMat, distCoeffs, this->multiTagFallbackStrategy);
+    return Update(result, this->multiTagFallbackStrategy);
   }
 
   // Output mats for results

photon-lib/src/main/native/include/photon/PhotonPoseEstimator.h

@@ -204,6 +204,21 @@ class PhotonPoseEstimator {
 
   inline void InvalidatePoseCache() { poseCacheTimestamp = -1_s; }
 
+  /**
+   * Internal convenience method for using a fallback strategy for update().
+   * This should only be called after timestamp checks have been done by another
+   * update() overload.
+   *
+   * @param cameraResult The latest pipeline result from the camera
+   * @param strategy The pose strategy to use
+   * @return an EstimatedRobotPose with an estimated pose, timestamp, and
+   * targets used to create the estimate.
+   */
+  std::optional<EstimatedRobotPose> Update(const PhotonPipelineResult& result,
+                                           PoseStrategy strategy) {
+    return Update(result, std::nullopt, std::nullopt, strategy);
+  }
+
   std::optional<EstimatedRobotPose> Update(
       const PhotonPipelineResult& result,
       std::optional<PhotonCamera::CameraMatrix> cameraMatrixData,