new_pose2D initialization values are all zero. I think new_pose2D should be set to the i-th polygon-corners' position before this if-case. Otherwise we refuse the sensor initialization in the corners if the map origin lies within the forbidden area.