From a4554d9bd44a25404cbbc9a81f0a8b19203608a2 Mon Sep 17 00:00:00 2001
From: Matt <matthew.morley.ca@gmail.com>
Date: Tue, 14 Feb 2023 19:50:17 -0500
Subject: [PATCH] Fix non-checked optional in mulit-tag (#808)

* Fix non-checked optional in mulit-tag

* Fix fallback return

* Run format
---
 .../java/org/photonvision/PhotonPoseEstimator.java    | 11 +++++++++--
 1 file changed, 9 insertions(+), 2 deletions(-)

diff --git a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
index b358ad73d..2b94f3d26 100644
--- a/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
+++ b/photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java
@@ -331,12 +331,19 @@ public class PhotonPoseEstimator {
 
         if (result.getTargets().size() < 2) {
             // Run fallback strategy instead
-            update(result, this.multiTagFallbackStrategy);
+            return update(result, this.multiTagFallbackStrategy);
         }
 
         for (var target : result.getTargets()) {
             visCorners.addAll(target.getDetectedCorners());
-            Pose3d tagPose = fieldTags.getTagPose(target.getFiducialId()).get();
+
+            var tagPoseOpt = fieldTags.getTagPose(target.getFiducialId());
+            if (tagPoseOpt.isEmpty()) {
+                reportFiducialPoseError(target.getFiducialId());
+                continue;
+            }
+
+            var tagPose = tagPoseOpt.get();
 
             // actual layout poses of visible tags -- not exposed, so have to recreate
             knownVisTags.add(new AprilTag(target.getFiducialId(), tagPose));