Fixed a bunch of bugs in vision code. Still not working in simulation, most likely due to weird camera rotation issues (the set of points that the camera thinks it can see does not make sense based on the image).