Modified scan matcher, adding graph_slam mapper