Fixed bug in map building

Caused by atan2 on border pi/-pi