scan matcher for GraphSLAM