updated the pose graph and added initialization