Fix transfromed coordinates reference points

* Also add height and width to obstacles