Vision Circle Fittting

[varun7654]

implement circle fitting for the vision pose estimates.
Most of this has been stolen from: https://www.chiefdelphi.com/t/frc-6328-mechanical-advantage-2022-build-thread/398645/21

[ravzilla]

If I understand this correctly the solver works by computing a residual that is equal to sum of (point-center guess)^2 then iterates on that solution by taking a step in the 4 cardinal directions and choosing an optimal point based on the residual of all 5 (center included).
right now the optimizer will take a path like this:

if this is working fine, i see no reason to change it, BUT the correct way to do this is by taking advantage of the fact that our residual function is differentiable. this will allow us to take a step directly towards the optimal solution like this:

The way we would do this would be by moving the center by stepsize * -1 * gradient of the residual each iteration
this would give us a 2D vector to move the center, just like the old code we would decrease our stepsize by some amount (https://en.wikipedia.org/wiki/Gradient_descent#Choosing_the_step_size_and_descent_direction)

If we need to improve the circle fitting we can do this, but if its working fine I see no reason to change it.

We could also do some optimizations by playing with the rate at which the step size decreases, dividing by half may be too much but that is just a guess.

tldr: if its working, we dont need to do this. If we want it to be faster or "more correct" we should do this

[ravzilla]

also if java has a convex optimization library we should just use that, would make this a lot easier, im sure it exists, just not sure what we can put on the rio
http://haifengl.github.io/api/java/smile/math/BFGS.html
BFGS is a nice algorithm but idk what works on the robot