Limit apriltag IDs (WIP)

docs/source/docs/contributing/design-descriptions/index.md

@@ -6,4 +6,5 @@ image-rotation
 time-sync
 camera-matching
 e2e-latency
+tag-limiting
 ```

docs/source/docs/contributing/design-descriptions/tag-limiting.md

@@ -0,0 +1,21 @@
+# AprilTag Rejection
+
+With the introduction of AprilTag's and full field localisation becoming more common rejecting tags that aren't relevant to the robot has become more important for maintaining a useful pose estimation. Including tags that aren't relevant to your objective can harm localisation, causing results to become more noisy and eventually skewed over the course of an event. In order to prevent this we can reject tags that aren't related to our current objective. There are a couple of things to consider when it comes to rejecting tags:
+- why can't this be done in user code
+- accessing rejected tag info
+- synchronisation between coprocessor and robot code
+
+## Why not do this in user code?
+
+The first thing that may come to mind when talking about tag rejection is, why not just do this in user code? After all we provide the tag IDs to the user. The main issue with doing this in user code is it means that you have to reject entire multitag results, when in reality you still want a multitag estimate just without the rejected tag. One way to work around this would be running all pose estimations in user code but this is a significant performance to robot code, particularly on the roboRIO.
+
+## Accessing rejected tag info
+
+Accessing rejected tag info is still important, as tags can serve other information not just localisation. For example in the 2025-2026 FTC Game, Decode, there was a tag utilised to determine different patterns of scoring elements. Mechanisms like this make it desirable to still have access to detection info even when we reject a tag for localisation purposes.
+
+## Synchronisation between coprocessor and robot code
+
+In order to be able to use Photon standalone without any robot code we must be able to change the tags to reject on the coprocessor. This unfortunately doesn't scale well with more complex robot code usecases, for example rejecting tags based on current scoring location or alliance, making it beneficial to be able to set the tags to reject from robot code as well. This raises a couple of questions:
+- How do we keep the coprocessor and robot code in sync?
+- What do we do if the robot code and coprocessor have conflicting values?
+The core of our approach to this will be always treating the robot code as a source of truth. If the robot code is publishing values for this then you won't be able to modify the values in the UI. Publishing from the UI will still be enabled when the robot code isn't publishing, but if you've set a value in the user code it will always override the value set on the coprocessor. If the value is ever overriden an Alert will be published. Furthermore if the robot code stops publishing it will return to the value set on the coprocessor.

photon-client/src/components/dashboard/tabs/AprilTagTab.vue

@@ -1,6 +1,7 @@
 <script setup lang="ts">
 import { PipelineType } from "@/types/PipelineTypes";
 import PvSelect from "@/components/common/pv-select.vue";
+import PvInput from "@/components/common/pv-input.vue";
 import PvSlider from "@/components/common/pv-slider.vue";
 import PvSwitch from "@/components/common/pv-switch.vue";
 import { computed } from "vue";
@@ -48,6 +49,23 @@ const interactiveCols = computed(() =>
       :step="0.1"
       @update:modelValue="(value) => useCameraSettingsStore().changeCurrentPipelineSetting({ blur: value }, false)"
     />
+    <pv-input
+      v-model="currentPipelineSettings.rejectTagIds"
+      label="Reject Tag IDs"
+      tooltip="Tag IDs to reject for multitag estimation"
+      @update:modelValue="
+        (value) =>
+          useCameraSettingsStore().changeCurrentPipelineSetting(
+            {
+              rejectTagIds: value
+                .split(',')
+                .filter((s) => s.trim() !== '')
+                .map(Number)
+            },
+            false
+          )
+      "
+    />
     <pv-slider
       v-model="currentPipelineSettings.threads"
       :slider-cols="interactiveCols"

photon-client/src/types/PipelineTypes.ts

@@ -228,6 +228,7 @@ export interface AprilTagPipelineSettings extends PipelineSettings {
   tagFamily: AprilTagFamily;
   doMultiTarget: boolean;
   doSingleTargetAlways: boolean;
+  rejectTagIds: number[];
 }
 export type ConfigurableAprilTagPipelineSettings = Partial<
   Omit<AprilTagPipelineSettings, "pipelineType" | "hammingDist" | "debug">
@@ -251,7 +252,8 @@ export const DefaultAprilTagPipelineSettings: AprilTagPipelineSettings = {
   threads: 4,
   tagFamily: AprilTagFamily.Family36h11,
   doMultiTarget: false,
-  doSingleTargetAlways: false
+  doSingleTargetAlways: false,
+  rejectTagIds: []
 };
 
 export interface ArucoPipelineSettings extends PipelineSettings {

photon-core/src/main/java/org/photonvision/common/dataflow/networktables/NTDataPublisher.java

@@ -186,6 +186,7 @@ public void accept(CVPipelineResult result) {
                         now + offset,
                         NetworkTablesManager.getInstance().getTimeSinceLastPong(),
                         TrackedTarget.simpleFromTrackedTargets(acceptedResult.targets),
+                        TrackedTarget.simpleFromTrackedTargets(acceptedResult.rejectedTags),
                         acceptedResult.multiTagResult);
 
         // random guess at size of the array

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

@@ -26,12 +26,16 @@
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.networktables.BooleanSubscriber;
+import edu.wpi.first.networktables.IntegerArrayPublisher;
+import edu.wpi.first.networktables.IntegerArraySubscriber;
 import java.util.ArrayList;
 import java.util.List;
 import java.util.Optional;
 import org.photonvision.common.configuration.ConfigManager;
 import org.photonvision.common.dataflow.structures.Packet;
 import org.photonvision.common.logging.LogGroup;
+import org.photonvision.common.logging.LogLevel;
 import org.photonvision.common.logging.Logger;
 import org.photonvision.common.util.math.MathUtils;
 import org.photonvision.estimation.TargetModel;
@@ -62,6 +66,10 @@ public class AprilTagPipeline extends CVPipeline<CVPipelineResult, AprilTagPipel
 
     private static final FrameThresholdType PROCESSING_TYPE = FrameThresholdType.GREYSCALE;
 
+    IntegerArrayPublisher rejectTagIdPublisher;
+    IntegerArraySubscriber rejectTagIdSubscriber;
+    BooleanSubscriber overrideCoprocRejectTag;
+
     public AprilTagPipeline() {
         super(PROCESSING_TYPE);
         settings = new AprilTagPipelineSettings();
@@ -145,10 +153,24 @@ protected CVPipelineResult process(Frame frame, AprilTagPipelineSettings setting
 
         List<AprilTagDetection> detections = tagDetectionPipeResult.output;
         List<AprilTagDetection> usedDetections = new ArrayList<>();
+        List<TrackedTarget> rejectedTags = new ArrayList<>();
         List<TrackedTarget> targetList = new ArrayList<>();
 
+        logger.log(settings.rejectTagIds.toString(), LogLevel.DEBUG);
+
         // Filter out detections based on pipeline settings
         for (AprilTagDetection detection : detections) {
+            if (settings.rejectTagIds.contains(Integer.valueOf(detection.getId()))) {
+                TrackedTarget target =
+                        new TrackedTarget(
+                                detection,
+                                null,
+                                new TargetCalculationParameters(
+                                        false, null, null, null, null, frameStaticProperties));
+                rejectedTags.add(target);
+                continue;
+            }
+
             // TODO this should be in a pipe, not in the top level here (Matt)
             if (detection.getDecisionMargin() < settings.decisionMargin) continue;
             if (detection.getHamming() > settings.hammingDist) continue;
@@ -247,13 +269,22 @@ protected CVPipelineResult process(Frame frame, AprilTagPipelineSettings setting
         var fps = fpsResult.output;
 
         return new CVPipelineResult(
-                frame.sequenceID, sumPipeNanosElapsed, fps, targetList, multiTagResult, frame);
+                frame.sequenceID,
+                sumPipeNanosElapsed,
+                fps,
+                targetList,
+                multiTagResult,
+                rejectedTags,
+                frame);
     }
 
     @Override
     public void release() {
         aprilTagDetectionPipe.release();
         singleTagPoseEstimatorPipe.release();
+        rejectTagIdPublisher.close();
+        rejectTagIdSubscriber.close();
+        overrideCoprocRejectTag.close();
         super.release();
     }
 }

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

@@ -18,6 +18,8 @@
 package org.photonvision.vision.pipeline;
 
 import com.fasterxml.jackson.annotation.JsonTypeName;
+import java.util.ArrayList;
+import java.util.List;
 import org.photonvision.vision.apriltag.AprilTagFamily;
 import org.photonvision.vision.target.TargetModel;
 
@@ -34,8 +36,7 @@ public class AprilTagPipelineSettings extends AdvancedPipelineSettings {
     public int decisionMargin = 35;
     public boolean doMultiTarget = false;
     public boolean doSingleTargetAlways = false;
-
-    // 3d settings
+    public List<Integer> rejectTagIds = new ArrayList<>();
 
     public AprilTagPipelineSettings() {
         super();
@@ -63,6 +64,9 @@ public int hashCode() {
         result = prime * result + decisionMargin;
         result = prime * result + (doMultiTarget ? 1231 : 1237);
         result = prime * result + (doSingleTargetAlways ? 1231 : 1237);
+        for (var id : rejectTagIds) {
+            result = prime * result + id;
+        }
         return result;
     }
 
@@ -83,6 +87,7 @@ public boolean equals(Object obj) {
         if (decisionMargin != other.decisionMargin) return false;
         if (doMultiTarget != other.doMultiTarget) return false;
         if (doSingleTargetAlways != other.doSingleTargetAlways) return false;
+        if (rejectTagIds != other.rejectTagIds) return false;
         return true;
     }
 }

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

@@ -234,8 +234,9 @@ protected CVPipelineResult process(Frame frame, ArucoPipelineSettings settings)
         var fpsResult = calculateFPSPipe.run(null);
         var fps = fpsResult.output;
 
+        // TODO add here
         return new CVPipelineResult(
-                frame.sequenceID, sumPipeNanosElapsed, fps, targetList, multiTagResult, frame);
+                frame.sequenceID, sumPipeNanosElapsed, fps, targetList, multiTagResult, List.of(), frame);
     }
 
     private void drawThresholdFrame(Mat greyMat, Mat outputMat, int windowSize, double constant) {

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

@@ -34,6 +34,7 @@ public class CVPipelineResult implements Releasable {
     public final List<TrackedTarget> targets;
     public final Frame inputAndOutputFrame;
     public Optional<MultiTargetPNPResult> multiTagResult;
+    public final List<TrackedTarget> rejectedTags;
     public final List<String> objectDetectionClassNames;
 
     public CVPipelineResult(
@@ -42,7 +43,7 @@ public CVPipelineResult(
             double fps,
             List<TrackedTarget> targets,
             Frame inputFrame) {
-        this(sequenceID, processingNanos, fps, targets, Optional.empty(), inputFrame, List.of());
+        this(sequenceID, processingNanos, fps, targets, inputFrame, List.of());
     }
 
     public CVPipelineResult(
@@ -52,7 +53,15 @@ public CVPipelineResult(
             List<TrackedTarget> targets,
             Frame inputFrame,
             List<String> classNames) {
-        this(sequenceID, processingNanos, fps, targets, Optional.empty(), inputFrame, classNames);
+        this(
+                sequenceID,
+                processingNanos,
+                fps,
+                targets,
+                Optional.empty(),
+                List.of(),
+                inputFrame,
+                classNames);
     }
 
     public CVPipelineResult(
@@ -61,8 +70,17 @@ public CVPipelineResult(
             double fps,
             List<TrackedTarget> targets,
             Optional<MultiTargetPNPResult> multiTagResult,
+            List<TrackedTarget> rejectedTags,
             Frame inputFrame) {
-        this(sequenceID, processingNanos, fps, targets, multiTagResult, inputFrame, List.of());
+        this(
+                sequenceID,
+                processingNanos,
+                fps,
+                targets,
+                multiTagResult,
+                rejectedTags,
+                inputFrame,
+                List.of());
     }
 
     public CVPipelineResult(
@@ -71,6 +89,7 @@ public CVPipelineResult(
             double fps,
             List<TrackedTarget> targets,
             Optional<MultiTargetPNPResult> multiTagResult,
+            List<TrackedTarget> rejectedTags,
             Frame inputFrame,
             List<String> classNames) {
         this.sequenceID = sequenceID;
@@ -79,6 +98,7 @@ public CVPipelineResult(
         this.targets = targets != null ? targets : Collections.emptyList();
         this.multiTagResult = multiTagResult;
         this.objectDetectionClassNames = classNames;
+        this.rejectedTags = rejectedTags;
 
         this.inputAndOutputFrame = inputFrame;
     }
@@ -89,7 +109,7 @@ public CVPipelineResult(
             double fps,
             List<TrackedTarget> targets,
             Optional<MultiTargetPNPResult> multiTagResult) {
-        this(sequenceID, processingNanos, fps, targets, multiTagResult, null, List.of());
+        this(sequenceID, processingNanos, fps, targets, multiTagResult, List.of(), null);
     }
 
     public boolean hasTargets() {

photon-lib/py/photonlibpy/generated/PhotonPipelineResultSerde.py

@@ -41,8 +41,8 @@
 
 class PhotonPipelineResultSerde:
     # Message definition md5sum. See photon_packet.adoc for details
-    MESSAGE_VERSION = "4b2ff16a964b5e2bf04be0c1454d91c4"
-    MESSAGE_FORMAT = "PhotonPipelineMetadata:ac0a45f686457856fb30af77699ea356 metadata;PhotonTrackedTarget:cc6dbb5c5c1e0fa808108019b20863f1 targets[?];optional MultiTargetPNPResult:541096947e9f3ca2d3f425ff7b04aa7b multitagResult;"
+    MESSAGE_VERSION = "829142ea1cc21c437194a69e22a6aac4"
+    MESSAGE_FORMAT = "PhotonPipelineMetadata:ac0a45f686457856fb30af77699ea356 metadata;PhotonTrackedTarget:cc6dbb5c5c1e0fa808108019b20863f1 targets[?];PhotonTrackedTarget:cc6dbb5c5c1e0fa808108019b20863f1 rejectedTags[?];optional MultiTargetPNPResult:541096947e9f3ca2d3f425ff7b04aa7b multitagResult;"
 
     @staticmethod
     def pack(value: "PhotonPipelineResult") -> "Packet":
@@ -54,6 +54,9 @@ def pack(value: "PhotonPipelineResult") -> "Packet":
         # targets is a custom VLA!
         ret.encodeList(value.targets, PhotonTrackedTarget.photonStruct)
 
+        # rejectedTags is a custom VLA!
+        ret.encodeList(value.rejectedTags, PhotonTrackedTarget.photonStruct)
+
         # multitagResult is optional! it better not be a VLA too
         ret.encodeOptional(value.multitagResult, MultiTargetPNPResult.photonStruct)
         return ret
@@ -68,6 +71,9 @@ def unpack(packet: "Packet") -> "PhotonPipelineResult":
         # targets is a custom VLA!
         ret.targets = packet.decodeList(PhotonTrackedTarget.photonStruct)
 
+        # rejectedTags is a custom VLA!
+        ret.rejectedTags = packet.decodeList(PhotonTrackedTarget.photonStruct)
+
         # multitagResult is optional! it better not be a VLA too
         ret.multitagResult = packet.decodeOptional(MultiTargetPNPResult.photonStruct)