[FEATURE]: Add 6328 single-tag pose estimation

[vncero]

Description

Please include a summary of the changes and the related issue. Please also include relevant motivation and context. List any dependencies that are required for this change.

Adds support for single tag estimates when close to reef via PhotonVision's PoseStrategy.PNP_DISTANCE_TRIG_SOLVE. Note that each Camera instance internally switches between single- and multi-tag estimation depending on how many tags are seen, which propagates upward into Vision and thus the SwerveDrive's pose estimator. Further adds an EstimateType field to VisionEstimate for logging which type of estimate was produced (full logging for vision beyond PhotonVision's existing logging will be written soon).

Type of change

Please delete options that are not relevant.

• New feature (non-breaking change which adds functionality)
• Breaking change (fix or feature that would cause existing functionality to not work as expected)

Checklist:

• My code follows the style guidelines of this project
• Someone have performed a self-review of my code
• I have commented my code, particularly in hard-to-understand areas
• My changes generate no new warnings
• I have added tests that prove my fix is effective or that my feature works
• New and existing unit tests pass locally with my changes
• Any dependent changes have been merged and published in downstream modules

[BananasAmIRite]

see changes

[BananasAmIRite]

Also, add the vision logging changes here too.

[vncero]

Will add logging changes in another PR due to concerns with slowing down this relatively high priority feature.

[BananasAmIRite]

look @ comments

also are you sure you don't wanna meet w/ 6328?

[BananasAmIRite]

any way to return both? Right now, if we don't see 2+ tags in auto, our main pose estimate won't update its angle, which means our angles would be a bit wrong, resulting in big errors for the single tag estimate. Adding multitag to the main pose estimate no matter what, with ambiguity filters, might resolve that.

tldr: add multitag estimate to main pose estimate always, but not single tag; add single tag or multitag estimate to reef pose estimate?

better yet: for alignment, add a method to see if we see a specific reef tag with our reef cameras. If we do see it, we use the reef pose estimate; otherwise, we use the main pose estimate until we do see it. This helps eliminate any blind spots through the use of a third non-reef camera for the main pose estimate.

I'm just guessing here, idk if it'll work. We might benefit hugely from a meeting with 6328 with our vision routine.

[Raptura]

all looks good outside of what jason already requested of you

[BananasAmIRite]

New vision pipeline:

• multiple tag -> update pose est with those tags (trans + rotate)

  • Low rotation std devs

• Single tag + >0.4 ambig -> reject

• Single tag + <0.4 ambig -> disambiguate (use a separate trig solve photon pose estimator for this), high rotation std devs

• use rotation component of the main pose estimate for trig solve angles