Realtime graph slam working