Vision

First you are going to need to set up the cameras, field layout, etc.

To do that, you need a PhotonCamera and a PhotonPoseEstimator.

The April tag layout is currently the welded type in Peachtree. The name of the camera MUST BE THE EXACT SAME name as the one in PhotonClient (photonvision.local:5800). The robotToCamera is the 3d transform from the origin of the robot to the lens of the camera.

You are then going to call camera.getAllUnreadResults() ONLY ONCE PER LOOP and iterate through each result, estimating the robot pose from each result.

estimateCoprocMultiTagPose(result) uses multiple tags to get a more accurate pose, there are more estimation methods, but this is the most accurate and recommended. If that doesn't work (estimate.isEmpty()) you need a fallback, which right now we use the closest to reference pose method (chooses the estimate closest to the robot's current pose) another popular fallback is lowest ambiguity.

Next, you need to decide whether to reject or accept the estimated pose. In this example, maxAmbiguity is 0.3 maxSingleTagDistanceMeters is 3.8, and maxZError is 0.75.

Now that you have all the poses you want to accept, you need to determine how accurate/trustworthy each one is. We do this using average distance squared divided by the number of tags seen. There is also a constant factor that each camera has that can increase/decrease standard deviations.

The higher your standard deviation, the less it will affect the pose when you fuse it with the pose.

To fuse the vision pose with the robot pose, use addVisionMeasurement() from the CTRE generated swerve drive.