[photon-lib] Sim multitag result

.gitignore

@@ -157,5 +157,8 @@ photonlib-java-examples/*/vendordeps/*
 photonlib-cpp-examples/*/vendordeps/*
 
 */networktables.json
+*/networktables.json.bck
+photonlib-cpp-examples/*/networktables.json.bck
+photonlib-java-examples/*/networktables.json.bck
 *.sqlite
 photon-server/src/main/resources/web/index.html

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

@@ -27,6 +27,7 @@
 import com.fasterxml.jackson.core.JsonProcessingException;
 import com.fasterxml.jackson.databind.ObjectMapper;
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.Matrix;
@@ -121,6 +122,9 @@ public static void setVersionCheckEnabled(boolean enabled) {
 
     Packet packet = new Packet(1);
 
+    private static AprilTagFieldLayout lastSetTagLayout =
+            AprilTagFields.kDefaultField.loadAprilTagLayoutField();
+
     // Existing is enough to make this multisubscriber do its thing
     private final MultiSubscriber m_topicNameSubscriber;
 
@@ -332,20 +336,32 @@ public boolean isConnected() {
     }
 
     /**
-     * Set the Apriltag Field Layout used by all PhotonVision coprocessors that are (or might later)
-     * connect to this robot. The topic is marked as persistant, so even if you only call this once
-     * ever, it will be saved on the RoboRIO and pushed out to all NT clients when code reboots.
+     * Get the last set {@link AprilTagFieldLayout}. The tag layout is shared between all PhotonVision
+     * coprocessors on the same NT instance-- this method returns the most recent layout set. If no
+     * layout has been set by the user, this is equal to {@link AprilTagFields#kDefaultField}.
+     *
+     * @return The last set layout
+     */
+    public AprilTagFieldLayout getAprilTagFieldLayout() {
+        return lastSetTagLayout;
+    }
+
+    /**
+     * Set the {@link AprilTagFieldLayout} used by all PhotonVision coprocessors that are (or might
+     * later) connect to this robot. The topic is marked as persistent, so even if you only call this
+     * once ever, it will be saved on the RoboRIO and pushed out to all NT clients when code reboots.
      * PhotonVision will also store a copy of this layout locally on the coprocessor, but subscribes
      * to this topic and the local copy will be updated when this function is called.
      *
      * @param layout The layout to use for *all* PhotonVision cameras
      * @return Success of serializing the JSON. This does *not* mean that all PhotonVision clients
      *     have updated their internal layouts.
      */
-    public boolean setApriltagFieldLayout(AprilTagFieldLayout layout) {
+    public boolean setAprilTagFieldLayout(AprilTagFieldLayout layout) {
         try {
             var layout_json = new ObjectMapper().writeValueAsString(layout);
             atflPublisher.set(layout_json);
+            lastSetTagLayout = layout;
             return true;
         } catch (JsonProcessingException e) {
             MathSharedStore.reportError("Error setting ATFL in " + this.getName(), e.getStackTrace());

photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java

@@ -37,6 +37,7 @@
 import java.util.ArrayList;
 import java.util.List;
 import java.util.Optional;
+import java.util.stream.Collectors;
 import org.opencv.core.Core;
 import org.opencv.core.CvType;
 import org.opencv.core.Mat;
@@ -52,6 +53,8 @@
 import org.photonvision.estimation.OpenCVHelp;
 import org.photonvision.estimation.RotTrlTransform3d;
 import org.photonvision.estimation.TargetModel;
+import org.photonvision.estimation.VisionEstimation;
+import org.photonvision.targeting.MultiTargetPNPResults;
 import org.photonvision.targeting.PNPResults;
 import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
@@ -524,11 +527,26 @@ public PhotonPipelineResult process(
             videoSimProcessed.putFrame(videoSimFrameProcessed);
         } else videoSimProcessed.setConnectionStrategy(ConnectionStrategy.kForceClose);
 
-        // put this simulated data to NT
+        // calculate multitag results
+        var multitagResults = new MultiTargetPNPResults();
+        var tagLayout = cam.getAprilTagFieldLayout();
+        var visibleLayoutTags = VisionEstimation.getVisibleLayoutTags(detectableTgts, tagLayout);
+        if (visibleLayoutTags.size() > 1) {
+            List<Integer> usedIDs =
+                    visibleLayoutTags.stream().map(t -> t.ID).sorted().collect(Collectors.toList());
+            var pnpResults =
+                    VisionEstimation.estimateCamPosePNP(
+                            prop.getIntrinsics(), prop.getDistCoeffs(), detectableTgts, tagLayout);
+            multitagResults = new MultiTargetPNPResults(pnpResults, usedIDs);
+        }
+
+        // sort target order
         if (sortMode != null) {
             detectableTgts.sort(sortMode.getComparator());
         }
-        return new PhotonPipelineResult(latencyMillis, detectableTgts);
+
+        // put this simulated data to NT
+        return new PhotonPipelineResult(latencyMillis, detectableTgts, multitagResults);
     }
 
     /**