Cleaned scan matcher. Added stuff to graph_merge. TMB