Working real-time GraphSLAM