Run multitag on coprocessor (#816)

2026-06-20 00:51:41 +00:00 · 2023-10-17 10:20:00 -04:00
parent ededc4f130
commit 47bd077bbb
72 changed files with 1708 additions and 1801 deletions
--- a/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java
+++ b/photon-targeting/src/main/java/org/photonvision/targeting/MultiTargetPNPResults.java
@@ -0,0 +1,93 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ */
+
+package org.photonvision.targeting;
+
+import java.util.ArrayList;
+import java.util.List;
+import org.photonvision.common.dataflow.structures.Packet;
+
+public class MultiTargetPNPResults {
+    // Seeing 32 apriltags at once seems like a sane limit
+    private static final int MAX_IDS = 32;
+    // pnpresult + MAX_IDS possible targets (arbitrary upper limit that should never be hit, ideally)
+    public static final int PACK_SIZE_BYTES = PNPResults.PACK_SIZE_BYTES + (Short.BYTES * MAX_IDS);
+
+    public PNPResults estimatedPose = new PNPResults();
+    public List<Integer> fiducialIDsUsed = List.of();
+
+    public MultiTargetPNPResults() {}
+
+    public MultiTargetPNPResults(PNPResults results, List<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++) {
+            int targetId = (int) packet.decodeShort();
+            if (targetId > -1) ids.add(targetId);
+        }
+        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((short) fiducialIDsUsed.get(i).byteValue());
+            } else {
+                packet.encode((short) -1);
+            }
+        }
+    }
+
+    @Override
+    public int hashCode() {
+        final int prime = 31;
+        int result = 1;
+        result = prime * result + ((estimatedPose == null) ? 0 : estimatedPose.hashCode());
+        result = prime * result + ((fiducialIDsUsed == null) ? 0 : fiducialIDsUsed.hashCode());
+        return result;
+    }
+
+    @Override
+    public boolean equals(Object obj) {
+        if (this == obj) return true;
+        if (obj == null) return false;
+        if (getClass() != obj.getClass()) return false;
+        MultiTargetPNPResults other = (MultiTargetPNPResults) obj;
+        if (estimatedPose == null) {
+            if (other.estimatedPose != null) return false;
+        } else if (!estimatedPose.equals(other.estimatedPose)) return false;
+        if (fiducialIDsUsed == null) {
+            if (other.fiducialIDsUsed != null) return false;
+        } else if (!fiducialIDsUsed.equals(other.fiducialIDsUsed)) return false;
+        return true;
+    }
+
+    @Override
+    public String toString() {
+        return "MultiTargetPNPResults [estimatedPose="
+                + estimatedPose
+                + ", fiducialIDsUsed="
+                + fiducialIDsUsed
+                + "]";
+    }
+}