Vision Subsystem by StewardHail3433 · Pull Request #4 · DevilBotz2876/Rebuilt2026_old

StewardHail3433 · 2026-01-08T23:27:27Z

The Vision Subsystem.
Includes:

Vision Interface
multiple cameras
PhotonVision and sim implementations
validation checks of pose . A measurement is valid
- 2+ april tag visible to the camera at the time
- 1 april tag that with a distance less than the maximum
- the measurement matches april tags with another measurement from a different camera
add measurement to drive pose estimation

danielcastellarin

I'd like see full docstrings for your functions, but for now let's just address the individual comments I made

danielcastellarin · 2026-01-09T01:46:59Z

+      inputs.targetIds = new int[multitagResult.fiducialIDsUsed.size()];
+      for (int j = 0; j < inputs.targetIds.length; j++) {
+        inputs.targetIds[j] = multitagResult.fiducialIDsUsed.get(j).intValue();
+      }


You could do this in one line. Potential learning opportunity

I learned about java streams and was able to do it with this line in the update method:
inputs.targetIds = multitagResult.fiducialIDsUsed.stream().mapToInt(Short::toUnsignedInt).toArray();

danielcastellarin · 2026-01-09T01:56:28Z

+      measurement.robotPose =
+          cameraPose.transformBy(
+              (new Transform2d(
+                      robotToCamera.getTranslation().toTranslation2d(),
+                      robotToCamera.getRotation().toRotation2d()))
+                  .inverse());
+      measurement.targetIds = inputs.targetIds;
+      measurement.timestamp = result.getTimestampSeconds();
+
+      // robot to camera + camera to target = robot to target
+      measurement.robotToBestTargetDistanceInMeters =
+          robotToCamera
+              .plus(result.getBestTarget().getBestCameraToTarget())
+              .getTranslation()
+              .getDistance(Translation3d.kZero);
+
+      inputs.cameraDistanceToTargetMeters =
+          result
+              .getBestTarget()
+              .getBestCameraToTarget()
+              .getTranslation()
+              .getDistance(Translation3d.kZero);
+    }


This feels repetitive from the previous else-if; let's see if we can consolidate them?

danielcastellarin · 2026-01-09T03:44:41Z

+    if (VISION_LOGGING_DEBUG) {
+      for (int cameraIndex = 0; cameraIndex < cameras.size(); cameraIndex++) {
+        for (int i = 0; i < cameras.get(cameraIndex).getVisionPoseMeasurements().length; i++) {
+          VisionPoseMeasurement poseMeasurement =
+              cameras.get(cameraIndex).getVisionPoseMeasurements()[i];
+          boolean isValid = validPoseMeasurements.contains(poseMeasurement);
+          String reason = "";
+
+          if (poseMeasurement.targetIds.length >= 2) {
+            reason = "MultiTag with IDs:" + Arrays.toString(poseMeasurement.targetIds);
+          } else if (poseMeasurement.robotToBestTargetDistanceInMeters != -1
+              && poseMeasurement.robotToBestTargetDistanceInMeters
+                  <= MAXIMUM_SINGLE_TAG_DISTANCE_METERS) {
+            reason =
+                "Single tag with distance of : "
+                    + poseMeasurement.robotToBestTargetDistanceInMeters;
+          } else if (isValid) {
+            reason = "Cross Tag Check,  Same AprilTag as different camera";
+
+          } else {
+            reason =
+                "Failed to have Single Tag with a match and is greater than min valid distance: "
+                    + poseMeasurement.robotToBestTargetDistanceInMeters;
+          }
+
+          Logger.recordOutput(
+              "Vision/"
+                  + cameras.get(cameraIndex).getName()
+                  + "/PoseMeasurements/"
+                  + String.valueOf(i)
+                  + "/reason",
+              reason);
+          Logger.recordOutput(
+              "Vision/"
+                  + cameras.get(cameraIndex).getName()
+                  + "/PoseMeasurements/"
+                  + String.valueOf(i)
+                  + "/isValid",
+              isValid);
+        }
+      }
+    }


Would it make sense to consolidate the logging parts from earlier in this function into here?

danielcastellarin · 2026-01-09T03:47:35Z

+    }
+
+    // add valid pose measurment to consumer
+    for (int i = 0; i < validPoseMeasurements.size(); i++) {


change this to a "for each" loop instead of a "for i". You don't really need the index for anything here

danielcastellarin · 2026-01-09T03:48:43Z

+    double currentTime = Timer.getFPGATimestamp();
+    for (int cameraIndex = 0; cameraIndex < cameraTagPoses.size(); cameraIndex++) {
+      // the time between one publish and the next
+      double oneMeasureDurationSeconds = 1.0 / cameras.get(cameraIndex).getCameraSettings().fps;
+
+      double measureLifespan = oneMeasureDurationSeconds * 5; // found 5 from testing in sim
+
+      Map<Integer, List<VisionPoseMeasurement>> tagMap = cameraTagPoses.get(cameraIndex);
+
+      for (List<VisionPoseMeasurement> posesAtSeenTag : tagMap.values()) {
+        posesAtSeenTag.removeIf(
+            measurement -> currentTime - measurement.timestamp > measureLifespan);
+      }
+    }


Reminder for Dan to look at this because I sleep

danielcastellarin · 2026-01-09T03:49:40Z

+    Logger.processInputs("Vision/" + camera.getName(), inputs);
+    camera.update(inputs);
+
+    if (visionMeasurementConsumer.isPresent()) {


Do we need to do anything in updateCamera if the consumer doesn't exist?

I think it depends on weather we want to use the camera with anything else that is not related to updating pose estimation. My thought was that information could still be gather weather or not we have a consumer.

As it stands, the only other functionality photonvision enables is object detection/tracking. I think we can assume we won't be utilizing that (and if some reason we do need it, we can modify the code at that time)

danielcastellarin · 2026-01-09T03:52:40Z

+      for (int i = 0; i < poseMeasurements.length; i++) {
+        for (int j = 0; j < poseMeasurements[i].targetIds.length; j++) {
+          if (cameraTagPoses.get(cameraIndex).containsKey(poseMeasurements[i].targetIds[j])) {
+            cameraTagPoses
+                .get(cameraIndex)
+                .get(poseMeasurements[i].targetIds[j])
+                .add(poseMeasurements[i]);
+          } else {
+            // make new key if first time tag is being seen
+            List<VisionPoseMeasurement> poseMeasurementList =
+                new ArrayList<VisionPoseMeasurement>();
+            poseMeasurementList.add(poseMeasurements[i]);
+            cameraTagPoses
+                .get(cameraIndex)
+                .put(poseMeasurements[i].targetIds[j], poseMeasurementList);
+          }
+        }


reminder for dan to reread

danielcastellarin · 2026-01-11T00:11:02Z

+    public static class VisionPoseMeasurement {
+      public double timestamp = -1.0;
+      public Pose2d robotPose = new Pose2d();
+      public int[] targetIds = new int[0];
+      public double robotToBestTargetDistanceInMeters = -1.0;
+    }


Looking at this again: why does this class have to be static?

StewardHail3433 added 15 commits December 22, 2025 19:11

visionInterface start

c88abb4

vision interface change

9bffe44

start visionSubsytem

d20b854

matching ids are valid

4dc0458

valid single tag based on distance

992c505

update validation

d113750

typo

93002e3

camera returns latest result

8235685

fix typo

c8ccb54

remove ambiguity as it was not used

090e934

matching tag helper method

de6941f

delete based on lifespan

d2fc22d

update field

1b0a766

add comments and rename

934affd

spotless

1dcc381

StewardHail3433 requested a review from danielcastellarin January 8, 2026 23:29

remove missed yagsl code and vendordep

d5e0074

danielcastellarin requested changes Jan 9, 2026

View reviewed changes

StewardHail3433 added 2 commits January 9, 2026 20:21

CameraPhoton update method additions

09bfc47

fix typo

2ec8e0a

danielcastellarin reviewed Jan 11, 2026

View reviewed changes

Conversation

StewardHail3433 commented Jan 8, 2026

Uh oh!

danielcastellarin left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

StewardHail3433 Jan 9, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

StewardHail3433 Jan 9, 2026 •

edited

Loading