More robust graph-slam