[FEATURE] Reimplement Vision with 6328 Guidance

[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.

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

Type of change

Please delete options that are not relevant.

• New feature (non-breaking change which adds functionality)

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]

Please add videos of the vision testing in simulation and comparisons for preferably the same data.