Lots of progress on vision, mostly just need filtering and laser scan conversion now