Work on tf

- applied tf transform to get the image coordinates from IMU coordinates
- moved slam old code in c_vision