Implement more accurate pose estimation using factor graphs

[calcmogul]

Is your feature request related to a problem? Please describe. We currently use standard deviations of odometry and vision poses in a linear Kalman filter, but that's a poor fit for the true probability distribution. For example, the measurement noise in terms of distance increases with range to the tag.

Describe the solution you'd like Factor graphs using GTSAM. It can be compiled for the roboRIO like this:

cmake -B build -S . --toolchain=~/wpilib/2023/roborio/toolchain-config.cmake -DGTSAM_ENABLE_BOOST_SERIALIZATION=OFF -DGTSAM_USE_BOOST_FEATURES=OFF

[JoshuaAN]

This is the current architecture for pose estimation

This is sub-optimal because feeding a normal distribution for pixels to solvepnp creates a nonlinear distribution for the output pose, while the kalman filter assumes a normal distribution.

The desired architecture is

The solvepnp math is done implicitly in the factor graphs solver, but the cost function is on pixels instead of output pose; combined with odometry measurements at the same time.

[calcmogul]

This also helps reduce pose ambiguity with AprilTags since the odometry is weighted a lot more than pixels in that case (since pixel variance is small for a wide range of poses).

[calcmogul]

Declan mentioned recently we could try making the measurements the corner pixels with the measurement model in a UKF mapping from pose to corner pixels (so camera model, basically).