[photon-lib] Simulate multitag result (#973)

PhotonVision · Oct 26, 2023 · d61225e · d61225e
1 parent 5b2be11
commit d61225e
Show file tree

Hide file tree

Showing 3 changed files with 71 additions and 26 deletions.
diff --git a/.gitignore b/.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
diff --git a/photon-lib/src/main/java/org/photonvision/PhotonCamera.java b/photon-lib/src/main/java/org/photonvision/PhotonCamera.java
@@ -24,11 +24,9 @@
 
 package org.photonvision;
 
-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;
 import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.numbers.*;
@@ -121,6 +119,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;
 
@@ -331,27 +332,45 @@ public boolean isConnected() {
         return (now - prevHeartbeatChangeTime) < HEARBEAT_DEBOUNCE_SEC;
     }
 
-    /**
-     * 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.
-     * 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) {
-        try {
-            var layout_json = new ObjectMapper().writeValueAsString(layout);
-            atflPublisher.set(layout_json);
-            return true;
-        } catch (JsonProcessingException e) {
-            MathSharedStore.reportError("Error setting ATFL in " + this.getName(), e.getStackTrace());
-        }
-        return false;
-    }
+    // TODO: Implement ATFL subscribing in backend
+    // /**
+    //  * 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) {
+    //     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());
+    //     }
+    //     return false;
+    // }
 
     public Optional<Matrix<N3, N3>> getCameraMatrix() {
         var cameraMatrix = cameraIntrinsicsSubscriber.get();

diff --git a/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java b/photon-lib/src/main/java/org/photonvision/simulation/PhotonCameraSim.java
@@ -24,6 +24,8 @@
 
 package org.photonvision.simulation;
 
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.cscore.CvSource;
 import edu.wpi.first.cscore.VideoMode.PixelFormat;
@@ -36,6 +38,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 +55,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;
@@ -77,6 +82,8 @@ public class PhotonCameraSim implements AutoCloseable {
     private double minTargetAreaPercent;
     private PhotonTargetSortMode sortMode = PhotonTargetSortMode.Largest;
 
+    private AprilTagFieldLayout tagLayout = AprilTagFields.kDefaultField.loadAprilTagLayoutField();
+
     // video stream simulation
     private final CvSource videoSimRaw;
     private final Mat videoSimFrameRaw = new Mat();
@@ -514,11 +521,27 @@ public PhotonPipelineResult process(
             videoSimProcessed.putFrame(videoSimFrameProcessed);
         } else videoSimProcessed.setConnectionStrategy(ConnectionStrategy.kForceClose);
 
-        // put this simulated data to NT
+        // calculate multitag results
+        var multitagResults = new MultiTargetPNPResults();
+        // TODO: Implement ATFL subscribing in backend
+        // 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);
     }
 
     /**