Added method for Graph Slam that can be used for experimentation