Skip to content
Draft
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion build.gradle
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@ plugins {
id "edu.wpi.first.wpilib.repositories.WPILibRepositoriesPlugin" version "2020.2"
id "edu.wpi.first.GradleRIO" version "2025.3.2"
id 'edu.wpi.first.WpilibTools' version '1.3.0'
id 'com.google.protobuf' version '0.9.3' apply false
id 'com.google.protobuf' version '0.9.5' apply false
id 'edu.wpi.first.GradleJni' version '1.1.0'
id "org.ysb33r.doxygen" version "1.0.4" apply false
id 'com.gradleup.shadow' version '8.3.4' apply false
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -6,4 +6,5 @@ image-rotation
time-sync
camera-matching
e2e-latency
tag-limiting
```
Original file line number Diff line number Diff line change
@@ -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?
Comment on lines +19 to +20
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

My current plan for this is allow setting values on the coprocessor and always let the robot code override. The coprocessor can publish a value that says "I am going to be overriden" if it doesn't then a warning will show up in Photon Alerts saying your overriding coprocessor rejected tag IDs

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Wrap long lines pls. Sounds like a generally good plan? Not sold on the alert, might be better to have setting only in user code. Or the user has to publish a list to a NT topic and then it reads tags to exclude from there.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

From previous discussions we want to be able to control this with the coprocessor as a standalone unit meaning only setting in user code or through an NT topic isn't an option

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.
Original file line number Diff line number Diff line change
Expand Up @@ -160,7 +160,8 @@ public void accept(CVPipelineResult result) {
now + offset,
NetworkTablesManager.getInstance().getTimeSinceLastPong(),
TrackedTarget.simpleFromTrackedTargets(acceptedResult.targets),
acceptedResult.multiTagResult);
acceptedResult.multiTagResult,
acceptedResult.rejectedTags);

// random guess at size of the array
ts.resultPublisher.set(simplified, 1024);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -140,10 +140,21 @@ 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<>();

// Filter out detections based on pipeline settings
for (AprilTagDetection detection : detections) {
if (settings.rejectTagIds.contains(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;
Expand Down Expand Up @@ -236,7 +247,13 @@ 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,
frame,
rejectedTags);
}

@Override
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
package org.photonvision.vision.pipeline;

import com.fasterxml.jackson.annotation.JsonTypeName;
import java.util.List;
import org.photonvision.vision.apriltag.AprilTagFamily;
import org.photonvision.vision.target.TargetModel;

Expand All @@ -34,8 +35,7 @@ public class AprilTagPipelineSettings extends AdvancedPipelineSettings {
public int decisionMargin = 35;
public boolean doMultiTarget = false;
public boolean doSingleTargetAlways = false;

// 3d settings
public List<Integer> rejectedTagIds = List.of();

public AprilTagPipelineSettings() {
super();
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,7 @@ public class CVPipelineResult implements Releasable {
public final List<TrackedTarget> targets;
public final Frame inputAndOutputFrame;
public Optional<MultiTargetPNPResult> multiTagResult;
public final Optional<List<AprilTagDetection>> rejectedTags;
public final List<String> objectDetectionClassNames;

public CVPipelineResult(
Expand Down Expand Up @@ -72,13 +73,15 @@ public CVPipelineResult(
List<TrackedTarget> targets,
Optional<MultiTargetPNPResult> multiTagResult,
Frame inputFrame,
List<String> classNames) {
List<String> classNames,
List<AprilTagDetection> rejectedTags) {
this.sequenceID = sequenceID;
this.processingNanos = processingNanos;
this.fps = fps;
this.targets = targets != null ? targets : Collections.emptyList();
this.multiTagResult = multiTagResult;
this.objectDetectionClassNames = classNames;
this.rejectedTags = rejectedTags;

this.inputAndOutputFrame = inputFrame;
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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 = "29e82c187da35e0337c86f4c14ee5ac7"
MESSAGE_FORMAT = "PhotonPipelineMetadata:ac0a45f686457856fb30af77699ea356 metadata;PhotonTrackedTarget:cc6dbb5c5c1e0fa808108019b20863f1 targets[?];optional MultiTargetPNPResult:541096947e9f3ca2d3f425ff7b04aa7b multitagResult;optional PhotonTrackedTarget:cc6dbb5c5c1e0fa808108019b20863f1 rejectedTags[?];"

@staticmethod
def pack(value: "PhotonPipelineResult") -> "Packet":
Expand All @@ -56,6 +56,9 @@ def pack(value: "PhotonPipelineResult") -> "Packet":

# multitagResult is optional! it better not be a VLA too
ret.encodeOptional(value.multitagResult, MultiTargetPNPResult.photonStruct)

# rejectedTags is optional! it better not be a VLA too
ret.encodeOptional(value.rejectedTags, PhotonTrackedTarget.photonStruct)
return ret

@staticmethod
Expand All @@ -71,6 +74,9 @@ def unpack(packet: "Packet") -> "PhotonPipelineResult":
# multitagResult is optional! it better not be a VLA too
ret.multitagResult = packet.decodeOptional(MultiTargetPNPResult.photonStruct)

# rejectedTags is optional! it better not be a VLA too
ret.rejectedTags = packet.decodeOptional(PhotonTrackedTarget.photonStruct)

return ret


Expand Down
8 changes: 0 additions & 8 deletions photon-serde/generate_messages.py
100644 → 100755
Original file line number Diff line number Diff line change
Expand Up @@ -218,14 +218,6 @@ def get_struct_schema_str(message: MessageType, message_db: List[MessageType]):
ret = ""

for field in message["fields"]:
if (
"optional" in field
and field["optional"] == True
and "vla" in field
and field["vla"] == True
):
raise Exception(f"Field {field} must be optional OR vla!")

typestr = get_fully_defined_field_name(field, message_db)

array_modifier = ""
Expand Down
4 changes: 4 additions & 0 deletions photon-serde/messages.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -92,3 +92,7 @@
- name: multitagResult
type: MultiTargetPNPResult
optional: True
- name: rejectedTags
type: PhotonTrackedTarget
optional: True
vla: True
10 changes: 8 additions & 2 deletions photon-serde/templates/Message.java.jinja
Original file line number Diff line number Diff line change
Expand Up @@ -63,8 +63,11 @@ public class {{ name }}Serde implements PacketSerde<{{name}}> {
{%- for field in fields -%}
{%- if field.type | is_shimmed %}
{{ get_message_by_name(field.type).java_encode_shim }}(packet, value.{{ field.name }});
{%- elif field.optional == True and field.vla == True %}
// {{ field.name }} is optional vla!
packet.encodeOptionalVla(value.{{ field.name }});
{%- elif field.optional == True %}
// {{ field.name }} is optional! it better not be a VLA too
// {{ field.name }} is optional!
packet.encodeOptional(value.{{ field.name }});
{%- elif field.vla == True and field.type | is_intrinsic %}
// {{ field.name }} is a intrinsic VLA!
Expand All @@ -90,8 +93,11 @@ public class {{ name }}Serde implements PacketSerde<{{name}}> {
{% for field in fields -%}
{%- if field.type | is_shimmed %}
ret.{{ field.name }} = {{ get_message_by_name(field.type).java_decode_shim }}(packet);
{%- elif field.optional == True and field.vla == True %}
// {{ field.name }} is optional vla!
ret.{{ field.name }} = packet.decodeOptionalVla({{ field.type }}.photonStruct);
{%- elif field.optional == True %}
// {{ field.name }} is optional! it better not be a VLA too
// {{ field.name }} is optional!
ret.{{ field.name }} = packet.decodeOptional({{ field.type }}.photonStruct);
{%- elif field.vla == True and not field.type | is_intrinsic %}
// {{ field.name }} is a custom VLA!
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -43,9 +43,9 @@
public class PhotonPipelineResultSerde implements PacketSerde<PhotonPipelineResult> {

@Override
public final String getInterfaceUUID() { return "4b2ff16a964b5e2bf04be0c1454d91c4"; }
public final String getInterfaceUUID() { return "29e82c187da35e0337c86f4c14ee5ac7"; }
@Override
public final String getSchema() { return "PhotonPipelineMetadata:ac0a45f686457856fb30af77699ea356 metadata;PhotonTrackedTarget:cc6dbb5c5c1e0fa808108019b20863f1 targets[?];optional MultiTargetPNPResult:541096947e9f3ca2d3f425ff7b04aa7b multitagResult;"; }
public final String getSchema() { return "PhotonPipelineMetadata:ac0a45f686457856fb30af77699ea356 metadata;PhotonTrackedTarget:cc6dbb5c5c1e0fa808108019b20863f1 targets[?];optional MultiTargetPNPResult:541096947e9f3ca2d3f425ff7b04aa7b multitagResult;optional PhotonTrackedTarget:cc6dbb5c5c1e0fa808108019b20863f1 rejectedTags[?];"; }
@Override
public final String getTypeName() { return "PhotonPipelineResult"; }

Expand All @@ -63,8 +63,11 @@ public void pack(Packet packet, PhotonPipelineResult value) {
// targets is a custom VLA!
packet.encodeList(value.targets);

// multitagResult is optional! it better not be a VLA too
// multitagResult is optional!
packet.encodeOptional(value.multitagResult);

// rejectedTags is optional vla!
packet.encodeOptionalVla(value.rejectedTags);
}

@Override
Expand All @@ -77,16 +80,19 @@ public PhotonPipelineResult unpack(Packet packet) {
// targets is a custom VLA!
ret.targets = packet.decodeList(PhotonTrackedTarget.photonStruct);

// multitagResult is optional! it better not be a VLA too
// multitagResult is optional!
ret.multitagResult = packet.decodeOptional(MultiTargetPNPResult.photonStruct);

// rejectedTags is optional vla!
ret.rejectedTags = packet.decodeOptionalVla(PhotonTrackedTarget.photonStruct);

return ret;
}

@Override
public PacketSerde<?>[] getNestedPhotonMessages() {
return new PacketSerde<?>[] {
MultiTargetPNPResult.photonStruct,PhotonPipelineMetadata.photonStruct,PhotonTrackedTarget.photonStruct
PhotonPipelineMetadata.photonStruct,PhotonTrackedTarget.photonStruct,MultiTargetPNPResult.photonStruct
};
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -34,13 +34,15 @@ void StructType::Pack(Packet& packet, const PhotonPipelineResult& value) {
packet.Pack<photon::PhotonPipelineMetadata>(value.metadata);
packet.Pack<std::vector<photon::PhotonTrackedTarget>>(value.targets);
packet.Pack<std::optional<photon::MultiTargetPNPResult>>(value.multitagResult);
packet.Pack<std::optional<photon::PhotonTrackedTarget>>(value.rejectedTags);
}

PhotonPipelineResult StructType::Unpack(Packet& packet) {
return PhotonPipelineResult{ PhotonPipelineResult_PhotonStruct{
.metadata = packet.Unpack<photon::PhotonPipelineMetadata>(),
.targets = packet.Unpack<std::vector<photon::PhotonTrackedTarget>>(),
.multitagResult = packet.Unpack<std::optional<photon::MultiTargetPNPResult>>(),
.rejectedTags = packet.Unpack<std::optional<photon::PhotonTrackedTarget>>(),
}};
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -46,11 +46,11 @@ namespace photon {
template <>
struct WPILIB_DLLEXPORT SerdeType<PhotonPipelineResult> {
static constexpr std::string_view GetSchemaHash() {
return "4b2ff16a964b5e2bf04be0c1454d91c4";
return "29e82c187da35e0337c86f4c14ee5ac7";
}

static constexpr std::string_view GetSchema() {
return "PhotonPipelineMetadata:ac0a45f686457856fb30af77699ea356 metadata;PhotonTrackedTarget:cc6dbb5c5c1e0fa808108019b20863f1 targets[?];optional MultiTargetPNPResult:541096947e9f3ca2d3f425ff7b04aa7b multitagResult;";
return "PhotonPipelineMetadata:ac0a45f686457856fb30af77699ea356 metadata;PhotonTrackedTarget:cc6dbb5c5c1e0fa808108019b20863f1 targets[?];optional MultiTargetPNPResult:541096947e9f3ca2d3f425ff7b04aa7b multitagResult;optional PhotonTrackedTarget:cc6dbb5c5c1e0fa808108019b20863f1 rejectedTags[?];";
}

static photon::PhotonPipelineResult Unpack(photon::Packet& packet);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,7 @@ struct PhotonPipelineResult_PhotonStruct {
photon::PhotonPipelineMetadata metadata;
std::vector<photon::PhotonTrackedTarget> targets;
std::optional<photon::MultiTargetPNPResult> multitagResult;
std::optional<photon::PhotonTrackedTarget> rejectedTags;

friend bool operator==(PhotonPipelineResult_PhotonStruct const&, PhotonPipelineResult_PhotonStruct const&) = default;
};
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -282,6 +282,13 @@ public <T extends PhotonStructSerializable<T>> void encodeOptional(Optional<T> d
}
}

public <T extends PhotonStructSerializable<T>> void encodeOptionalVla(Optional<List<T>> data) {
encode(data.isPresent());
if (data.isPresent()) {
encodeList(data.get());
}
}

/**
* Returns a decoded byte from the packet.
*
Expand Down Expand Up @@ -427,6 +434,15 @@ public <T extends PhotonStructSerializable<T>> Optional<T> decodeOptional(Packet
return Optional.empty();
}

public <T extends PhotonStructSerializable<T>> Optional<List<T>> decodeOptionalVla(
PacketSerde<T> serde) {
var present = decodeBoolean();
if (present) {
return Optional.of(decodeList(serde));
}
return Optional.empty();
}

public List<Short> decodeShortList() {
byte length = decodeByte();

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -40,9 +40,12 @@ public class PhotonPipelineResult
// Multi-tag result
public Optional<MultiTargetPNPResult> multitagResult;

// Rejected tags
public Optional<List<PhotonTrackedTarget>> rejectedTags;

/** Constructs an empty pipeline result. */
public PhotonPipelineResult() {
this(new PhotonPipelineMetadata(), List.of(), Optional.empty());
this(new PhotonPipelineMetadata(), List.of(), Optional.empty(), Optional.empty());
}

/**
Expand All @@ -65,6 +68,7 @@ public PhotonPipelineResult(
new PhotonPipelineMetadata(
captureTimestampMicros, publishTimestampMicros, sequenceID, timeSinceLastPong),
targets,
Optional.empty(),
Optional.empty());
}

Expand All @@ -85,21 +89,25 @@ public PhotonPipelineResult(
long publishTimestamp,
long timeSinceLastPong,
List<PhotonTrackedTarget> targets,
Optional<MultiTargetPNPResult> result) {
Optional<MultiTargetPNPResult> result,
Optional<List<PhotonTrackedTarget>> rejectedTags) {
this(
new PhotonPipelineMetadata(
captureTimestamp, publishTimestamp, sequenceID, timeSinceLastPong),
targets,
result);
result,
rejectedTags);
}

public PhotonPipelineResult(
PhotonPipelineMetadata metadata,
List<PhotonTrackedTarget> targets,
Optional<MultiTargetPNPResult> result) {
Optional<MultiTargetPNPResult> result,
Optional<List<PhotonTrackedTarget>> rejectedTags) {
this.metadata = metadata;
this.targets.addAll(targets);
this.multitagResult = result;
this.rejectedTags = rejectedTags;
}

/**
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -52,6 +52,9 @@ public PhotonPipelineResult unpack(ProtobufPhotonPipelineResult msg) {
PhotonTrackedTarget.proto.unpack(msg.getTargets()),
msg.hasMultiTargetResult()
? Optional.of(MultiTargetPNPResult.proto.unpack(msg.getMultiTargetResult()))
: Optional.empty(),
msg.hasRejectedTags()
? Optional.of(PhotonTrackedTarget.proto.unpack(msg.getRejectedTags()))
: Optional.empty());
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,14 @@ public PhotonTrackedTarget unpack(ProtobufPhotonTrackedTarget msg) {
}

public List<PhotonTrackedTarget> unpack(RepeatedMessage<ProtobufPhotonTrackedTarget> msg) {
ArrayList<PhotonTrackedTarget> targets = new ArrayList<>(msg.getTargets().length());
for (ProtobufPhotonTrackedTarget target : msg.getTargets()) {
targets.add(unpack(target));
}
return targets;
}

public List<PhotonTrackedTarget> unpack(ProtobufPhotonTrackedTargetList msg) {
ArrayList<PhotonTrackedTarget> targets = new ArrayList<>(msg.length());
for (ProtobufPhotonTrackedTarget target : msg) {
targets.add(unpack(target));
Expand Down
Loading
Loading