added function to optimize a pose graph based on a path and keyframes