Added robust pose graph optimization step.