Added the odometry pose correction using the results of graph optimization