Ensuring that pose estimation is doing its job properly
Cleaned up output a bit, tracker now doing data association for object map maker
Cleaned code up a bit