Fixed bug in adding of local map info when the map is optimised. Added code to...

Fixed bug in adding of local map info when the map is optimised. Added code to store optimised map positions in 3d graph slam, and to listen to these changes in 3D Display node. Display node still doesn't move the point clouds though after an optimisation