Add Camera Extrinsic Calibration Command

[CJendantix]

Resolves #111

[Copilot]

Pull request overview

This PR adds a camera extrinsic calibration command to the VisionSubsystem and refactors the vision processing from a periodic() method to a command-based architecture. The calibration feature allows operators to automatically determine camera-to-robot transforms by collecting samples of camera-to-target transforms and averaging them.

Key changes:

• Refactored periodic vision processing into a command that is set as the default command
• Added cameraCalibrationCommand() that collects transform samples and computes camera extrinsics
• Moved and refactored preFilter() and postFilter() methods to the end of the class
• Removed TrigSolve-based pose estimation and related logging functionality

[Spybh66]

I think this behavior could be achieved in a util class in the lib rather then in the subsystem as this isn't really specific to the subsystem impl

[CJendantix]

Yes it is, its a command that relies on internal data the rest of the program shouldn't have access to. In my mind there's no good reason to separate this from the robot code when its actual wanted behavior is slightly different for each robot.

[Spybh66]

What would be the behavior that is robot/season dependent?

[CJendantix]

Which cameras are calibrated together, for example. It's far from impossible to put it in lib, I just don't see a good reason to.

[Spybh66]

https://github.com/wave-2826/Timothy2025/blob/main/src/main/java/frc/robot/commands/vision/VisionTuningCommands.java

[Spybh66]

I think that its a lot of clutter for the vision subsystem for non-operations code (i.e. isn't used at comp or on the field), and it really is a utility in scope. Also I disagree with making the periodic into a command, it breaks the design pattern we have for everything else and can be risky depending on the command scheduler to handle it

[jw00541]

I can see the argument for why it would be in robot, but I agree that it makes more sense in lib. Ideally we run this once per season (if at all, assuming individual corrections are insufficient) and never touch it again

[CJendantix]

I think that its a lot of clutter for the vision subsystem for non-operations code (i.e. isn't used at comp or on the field), and it really is a utility in scope. Also I disagree with making the periodic into a command, it breaks the design pattern we have for everything else and can be risky depending on the command scheduler to handle it

Just to be clear, making the main functionality the default command is no different than periodic, they are both scheduled by the CommandScheduler in pretty much the same way. The difference is that it allows me to run other commands under other circumstances. We never need to schedule the default command, as it is automatically scheduled any time any other command that requires VisionSubsystem ends. It also does not break the design pattern we have. Look in every other subsystem. Generally required things like general logging or calling periodic methods is in periodic, while all other actual functionality is inside of commands. A good example of this is Drive; it handles odometry and logging inside of periodic, but you cannot drive without its default command. We cannot run both the main functionality and calibration at the same time, for they share mutable state (AprilTagCamera#getUnreadResults). I recently attempted to move the functionality out of the subsystem, and it only made things more complicated, for there is no good way to collect samples over time outside of a scheduled command, meaning we would still need the main complexity of the command (collecting the samples), and only remove a small amount of processing. Moving this small amount of work to lib would require clearly defining and documenting many of assumptions about ordering, validity, and frames, which are currently guaranteed by the subsystem. That also makes it harder to change the calibration logic later, since the math and the constraints it relies on would be split in two.

[CJendantix]

Is that better than "no?"

[jw00541]

yes, this sounds reasonable

[jw00541]

this should probably be accomplished via a private helper method at some point because we will likely want to look into increasing the sophistication of this process

[jw00541]

do we want to enforce a tag ID filter to prevent a sloppy calibration scenario in which another tag is visible in frame?

[jw00541]

CALIBRATION_TAG_ID

[jw00541]

since we're calibrating from a known transform, do we want outlier rejection built into the sample collection to account for environmental fluctuations (blur, occlusion, etc.)? implemented via ambiguity cutoff. essentially translates to a Z-score rejection in statistics

[jw00541]

hate to say it but if we end up using this multiple times per season and it lives here it may want to have a unit test for it