Add IDs used to multi-tag result

photon-core/src/main/java/org/photonvision/common/dataflow/websocket/UIDataPublisher.java

@@ -54,11 +54,13 @@ public void accept(CVPipelineResult result) {
         dataMap.put("targets", uiTargets);
 
         // Only send Multitag Results if they are present, similar to 3d pose
-        if (result.multiTagResult.isPresent) {
+        if (result.multiTagResult.estimatedPose.isPresent) {
             var multitagData = new HashMap<String, Object>();
             multitagData.put(
-                    "bestTransform", SerializationUtils.transformToHashMap(result.multiTagResult.best));
-            multitagData.put("bestReprojectionError", result.multiTagResult.bestReprojErr);
+                    "bestTransform",
+                    SerializationUtils.transformToHashMap(result.multiTagResult.estimatedPose.best));
+            multitagData.put("bestReprojectionError", result.multiTagResult.estimatedPose.bestReprojErr);
+            multitagData.put("fiducialIDsUsed", result.multiTagResult.fiducialIDsUsed);
             dataMap.put("multitagResult", multitagData);
         }
 

photon-core/src/main/java/org/photonvision/vision/pipe/impl/MultiTargetPNPPipe.java

@@ -26,22 +26,22 @@
 import org.photonvision.common.logging.LogGroup;
 import org.photonvision.common.logging.Logger;
 import org.photonvision.estimation.VisionEstimation;
-import org.photonvision.targeting.PNPResults;
+import org.photonvision.targeting.MultiTargetPNPResults;
 import org.photonvision.targeting.TargetCorner;
 import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
 import org.photonvision.vision.pipe.CVPipe;
-import org.photonvision.vision.target.TargetModel;
 import org.photonvision.vision.target.TrackedTarget;
 
 /** Estimate the camera pose given multiple Apriltag observations */
 public class MultiTargetPNPPipe
-        extends CVPipe<List<TrackedTarget>, PNPResults, MultiTargetPNPPipe.MultiTargetPNPPipeParams> {
+        extends CVPipe<
+                List<TrackedTarget>, MultiTargetPNPResults, MultiTargetPNPPipe.MultiTargetPNPPipeParams> {
     private static final Logger logger = new Logger(MultiTargetPNPPipe.class, LogGroup.VisionModule);
 
     private boolean hasWarned = false;
 
     @Override
-    protected PNPResults process(List<TrackedTarget> targetList) {
+    protected MultiTargetPNPResults process(List<TrackedTarget> targetList) {
         if (params.cameraCoefficients == null
                 || params.cameraCoefficients.getCameraIntrinsicsMat() == null
                 || params.cameraCoefficients.getDistCoeffsMat() == null) {
@@ -50,21 +50,22 @@ protected PNPResults process(List<TrackedTarget> targetList) {
                         "Cannot perform solvePNP an uncalibrated camera! Please calibrate this resolution...");
                 hasWarned = true;
             }
-            return new PNPResults();
+            return new MultiTargetPNPResults();
         }
 
         return calculateTargetPose(targetList);
     }
 
-    private PNPResults calculateTargetPose(List<TrackedTarget> targetList) {
+    private MultiTargetPNPResults calculateTargetPose(List<TrackedTarget> targetList) {
         if (params.cameraCoefficients == null
                 || params.cameraCoefficients.getCameraIntrinsicsMat() == null
                 || params.cameraCoefficients.getDistCoeffsMat() == null) {
-            return new PNPResults();
+            return new MultiTargetPNPResults();
         }
 
         var visCorners = new ArrayList<TargetCorner>();
         var knownVisTags = new ArrayList<AprilTag>();
+        var tagIDsUsed = new ArrayList<Integer>();
         for (var target : targetList) {
             for (var corner : target.getTargetCorners()) {
                 visCorners.add(new TargetCorner(corner.x, corner.y));
@@ -73,15 +74,17 @@ private PNPResults calculateTargetPose(List<TrackedTarget> targetList) {
 
             // actual layout poses of visible tags -- not exposed, so have to recreate
             knownVisTags.add(new AprilTag(target.getFiducialId(), tagPose));
+            tagIDsUsed.add(target.getFiducialId());
         }
-        var pnpResults =
+        var ret = new MultiTargetPNPResults();
+        ret.estimatedPose =
                 VisionEstimation.estimateCamPosePNP(
                         params.cameraCoefficients.getCameraIntrinsicsMat(),
                         params.cameraCoefficients.getDistCoeffsMat(),
                         visCorners,
                         knownVisTags);
-
-        return pnpResults;
+        ret.fiducialIDsUsed = tagIDsUsed;
+        return ret;
     }
 
     Mat rotationMatrix = new Mat();
@@ -92,15 +95,11 @@ private PNPResults calculateTargetPose(List<TrackedTarget> targetList) {
 
     public static class MultiTargetPNPPipeParams {
         private final CameraCalibrationCoefficients cameraCoefficients;
-        private final TargetModel targetModel;
         private final AprilTagFieldLayout atfl;
 
         public MultiTargetPNPPipeParams(
-                CameraCalibrationCoefficients cameraCoefficients,
-                TargetModel targetModel,
-                AprilTagFieldLayout atfl) {
+                CameraCalibrationCoefficients cameraCoefficients, AprilTagFieldLayout atfl) {
             this.cameraCoefficients = cameraCoefficients;
-            this.targetModel = targetModel;
             this.atfl = atfl;
         }
     }

photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipeline.java

@@ -27,7 +27,7 @@
 import java.util.List;
 import org.photonvision.common.configuration.ConfigManager;
 import org.photonvision.common.util.math.MathUtils;
-import org.photonvision.targeting.PNPResults;
+import org.photonvision.targeting.MultiTargetPNPResults;
 import org.photonvision.vision.frame.Frame;
 import org.photonvision.vision.frame.FrameThresholdType;
 import org.photonvision.vision.pipe.CVPipe.CVPipeResult;
@@ -109,8 +109,7 @@ protected void setPipeParamsImpl() {
                 // TODO global state ew
                 var atfl = ConfigManager.getInstance().getConfig().getApriltagFieldLayout();
                 multiTagPNPPipe.setParams(
-                        new MultiTargetPNPPipeParams(
-                                frameStaticProperties.cameraCalibration, settings.targetModel, atfl));
+                        new MultiTargetPNPPipeParams(frameStaticProperties.cameraCalibration, atfl));
             }
         }
     }
@@ -127,7 +126,7 @@ protected CVPipelineResult process(Frame frame, AprilTagPipelineSettings setting
         if (frame.type != FrameThresholdType.GREYSCALE) {
             // TODO so all cameras should give us GREYSCALE -- how should we handle if not?
             // Right now, we just return nothing
-            return new CVPipelineResult(0, 0, List.of(), new PNPResults(), frame);
+            return new CVPipelineResult(0, 0, List.of(), new MultiTargetPNPResults(), frame);
         }
 
         CVPipeResult<List<AprilTagDetection>> tagDetectionPipeResult;
@@ -167,7 +166,7 @@ protected CVPipelineResult process(Frame frame, AprilTagPipelineSettings setting
             targetList.add(target);
         }
 
-        PNPResults multiTagResult = new PNPResults();
+        MultiTargetPNPResults multiTagResult = new MultiTargetPNPResults();
         if (settings.solvePNPEnabled && settings.doMultiTarget) {
             var multiTagOutput = multiTagPNPPipe.run(targetList);
             sumPipeNanosElapsed += multiTagOutput.nanosElapsed;

photon-core/src/main/java/org/photonvision/vision/pipeline/ArucoPipeline.java

@@ -38,7 +38,7 @@
 import java.util.ArrayList;
 import java.util.List;
 import org.opencv.core.Mat;
-import org.photonvision.targeting.PNPResults;
+import org.photonvision.targeting.MultiTargetPNPResults;
 import org.photonvision.vision.aruco.ArucoDetectionResult;
 import org.photonvision.vision.aruco.ArucoDetectorParams;
 import org.photonvision.vision.frame.Frame;
@@ -97,7 +97,8 @@ protected CVPipelineResult process(Frame frame, ArucoPipelineSettings settings)
         CVPipeResult<List<ArucoDetectionResult>> tagDetectionPipeResult;
 
         if (rawInputMat.empty()) {
-            return new CVPipelineResult(sumPipeNanosElapsed, 0, List.of(), new PNPResults(), frame);
+            return new CVPipelineResult(
+                    sumPipeNanosElapsed, 0, List.of(), new MultiTargetPNPResults(), frame);
         }
 
         tagDetectionPipeResult = arucoDetectionPipe.run(rawInputMat);
@@ -124,6 +125,7 @@ protected CVPipelineResult process(Frame frame, ArucoPipelineSettings settings)
         var fpsResult = calculateFPSPipe.run(null);
         var fps = fpsResult.output;
 
-        return new CVPipelineResult(sumPipeNanosElapsed, fps, targetList, new PNPResults(), frame);
+        return new CVPipelineResult(
+                sumPipeNanosElapsed, fps, targetList, new MultiTargetPNPResults(), frame);
     }
 }

photon-core/src/main/java/org/photonvision/vision/pipeline/Calibrate3dPipeline.java

@@ -34,7 +34,7 @@
 import org.photonvision.common.logging.Logger;
 import org.photonvision.common.util.SerializationUtils;
 import org.photonvision.common.util.file.FileUtils;
-import org.photonvision.targeting.PNPResults;
+import org.photonvision.targeting.MultiTargetPNPResults;
 import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
 import org.photonvision.vision.frame.Frame;
 import org.photonvision.vision.frame.FrameThresholdType;
@@ -111,7 +111,7 @@ protected CVPipelineResult process(Frame frame, Calibration3dPipelineSettings se
         Mat inputColorMat = frame.colorImage.getMat();
 
         if (this.calibrating || inputColorMat.empty()) {
-            return new CVPipelineResult(0, 0, null, new PNPResults(), frame);
+            return new CVPipelineResult(0, 0, null, new MultiTargetPNPResults(), frame);
         }
 
         long sumPipeNanosElapsed = 0L;
@@ -147,7 +147,7 @@ protected CVPipelineResult process(Frame frame, Calibration3dPipelineSettings se
                 sumPipeNanosElapsed,
                 fps, // Unused but here in case
                 Collections.emptyList(),
-                new PNPResults(),
+                new MultiTargetPNPResults(),
                 new Frame(
                         new CVMat(), outputColorCVMat, FrameThresholdType.NONE, frame.frameStaticProperties));
     }

photon-core/src/main/java/org/photonvision/vision/pipeline/ColoredShapePipeline.java

@@ -22,7 +22,7 @@
 import java.util.stream.Collectors;
 import org.apache.commons.lang3.tuple.Pair;
 import org.opencv.core.Point;
-import org.photonvision.targeting.PNPResults;
+import org.photonvision.targeting.MultiTargetPNPResults;
 import org.photonvision.vision.frame.Frame;
 import org.photonvision.vision.frame.FrameThresholdType;
 import org.photonvision.vision.opencv.*;
@@ -227,6 +227,7 @@ protected CVPipelineResult process(Frame frame, ColoredShapePipelineSettings set
         var fpsResult = calculateFPSPipe.run(null);
         var fps = fpsResult.output;
 
-        return new CVPipelineResult(sumPipeNanosElapsed, fps, targetList, new PNPResults(), frame);
+        return new CVPipelineResult(
+                sumPipeNanosElapsed, fps, targetList, new MultiTargetPNPResults(), frame);
     }
 }

photon-core/src/main/java/org/photonvision/vision/pipeline/OutputStreamPipeline.java

@@ -19,7 +19,6 @@
 
 import java.util.List;
 import org.apache.commons.lang3.tuple.Pair;
-import org.photonvision.targeting.PNPResults;
 import org.photonvision.vision.frame.Frame;
 import org.photonvision.vision.frame.FrameStaticProperties;
 import org.photonvision.vision.opencv.DualOffsetValues;
@@ -225,7 +224,6 @@ public CVPipelineResult process(
                 sumPipeNanosElapsed,
                 fps, // Unused but here just in case
                 targetsToDraw,
-                new PNPResults(),
                 inputAndOutputFrame);
     }
 }

photon-core/src/main/java/org/photonvision/vision/pipeline/ReflectivePipeline.java

@@ -18,7 +18,6 @@
 package org.photonvision.vision.pipeline;
 
 import java.util.List;
-import org.photonvision.targeting.PNPResults;
 import org.photonvision.vision.frame.Frame;
 import org.photonvision.vision.frame.FrameThresholdType;
 import org.photonvision.vision.opencv.Contour;
@@ -194,6 +193,6 @@ public CVPipelineResult process(Frame frame, ReflectivePipelineSettings settings
 
         PipelineProfiler.printReflectiveProfile(pipeProfileNanos);
 
-        return new CVPipelineResult(sumPipeNanosElapsed, fps, targetList, new PNPResults(), frame);
+        return new CVPipelineResult(sumPipeNanosElapsed, fps, targetList, frame);
     }
 }

photon-core/src/main/java/org/photonvision/vision/pipeline/result/CVPipelineResult.java

@@ -20,7 +20,7 @@
 import java.util.Collections;
 import java.util.List;
 import org.photonvision.common.util.math.MathUtils;
-import org.photonvision.targeting.PNPResults;
+import org.photonvision.targeting.MultiTargetPNPResults;
 import org.photonvision.vision.frame.Frame;
 import org.photonvision.vision.opencv.Releasable;
 import org.photonvision.vision.target.TrackedTarget;
@@ -31,13 +31,18 @@ public class CVPipelineResult implements Releasable {
     public final double fps;
     public final List<TrackedTarget> targets;
     public final Frame inputAndOutputFrame;
-    public PNPResults multiTagResult;
+    public MultiTargetPNPResults multiTagResult;
+
+    public CVPipelineResult(
+            double processingNanos, double fps, List<TrackedTarget> targets, Frame inputFrame) {
+        this(processingNanos, fps, targets, new MultiTargetPNPResults(), inputFrame);
+    }
 
     public CVPipelineResult(
             double processingNanos,
             double fps,
             List<TrackedTarget> targets,
-            PNPResults multiTagResults,
+            MultiTargetPNPResults multiTagResults,
             Frame inputFrame) {
         this.processingNanos = processingNanos;
         this.fps = fps;
@@ -48,7 +53,10 @@ public CVPipelineResult(
     }
 
     public CVPipelineResult(
-            double processingNanos, double fps, List<TrackedTarget> targets, PNPResults multiTagResults) {
+            double processingNanos,
+            double fps,
+            List<TrackedTarget> targets,
+            MultiTargetPNPResults multiTagResults) {
         this(processingNanos, fps, targets, multiTagResults, null);
     }
 

photon-core/src/main/java/org/photonvision/vision/pipeline/result/DriverModePipelineResult.java

@@ -18,11 +18,10 @@
 package org.photonvision.vision.pipeline.result;
 
 import java.util.List;
-import org.photonvision.targeting.PNPResults;
 import org.photonvision.vision.frame.Frame;
 
 public class DriverModePipelineResult extends CVPipelineResult {
     public DriverModePipelineResult(double latencyNanos, double fps, Frame outputFrame) {
-        super(latencyNanos, fps, List.of(), new PNPResults(), outputFrame);
+        super(latencyNanos, fps, List.of(), outputFrame);
     }
 }

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

@@ -35,7 +35,7 @@
 import org.photonvision.PhotonTargetSortMode;
 import org.photonvision.common.dataflow.structures.Packet;
 import org.photonvision.common.networktables.NTTopicSet;
-import org.photonvision.targeting.PNPResults;
+import org.photonvision.targeting.MultiTargetPNPResults;
 import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
 
@@ -142,7 +142,7 @@ public void submitProcessedFrame(
         }
 
         PhotonPipelineResult newResult =
-                new PhotonPipelineResult(latencyMillis, targetList, new PNPResults());
+                new PhotonPipelineResult(latencyMillis, targetList, new MultiTargetPNPResults());
         var newPacket = new Packet(newResult.getPacketSize());
         newResult.populatePacket(newPacket);
         ts.rawBytesEntry.set(newPacket.getData());

photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java

@@ -0,0 +1,41 @@
+package org.photonvision.targeting;
+
+import java.util.ArrayList;
+import java.util.List;
+import org.photonvision.common.dataflow.structures.Packet;
+
+public class MultiTargetPNPResults {
+    // pnpresult + 32 possible targets (arbitrary upper limit that should never be hit, ideally)
+    private static final int MAX_IDS = 32;
+    public static final int PACK_SIZE_BYTES = PNPResults.PACK_SIZE_BYTES + Byte.BYTES * MAX_IDS;
+
+    public PNPResults estimatedPose = new PNPResults();
+    public List<Integer> fiducialIDsUsed = List.of();
+
+    public MultiTargetPNPResults() {}
+
+    public MultiTargetPNPResults(PNPResults results, ArrayList<Integer> ids) {
+        estimatedPose = results;
+        fiducialIDsUsed = ids;
+    }
+
+    public static MultiTargetPNPResults createFromPacket(Packet packet) {
+        var results = PNPResults.createFromPacket(packet);
+        var ids = new ArrayList<Integer>(MAX_IDS);
+        for (int i = 0; i < MAX_IDS; i++) {
+            ids.add((int) packet.decodeByte());
+        }
+        return new MultiTargetPNPResults(results, ids);
+    }
+
+    public void populatePacket(Packet packet) {
+        estimatedPose.populatePacket(packet);
+        for (int i = 0; i < MAX_IDS; i++) {
+            if (i < fiducialIDsUsed.size()) {
+                packet.encode(fiducialIDsUsed.get(i).byteValue());
+            } else {
+                packet.encode(Byte.MIN_VALUE);
+            }
+        }
+    }
+}