Fixed Polygon generation after loop closure