Second pass on obstacle detection. Works under different assumptions. Pending some changes from Brandon.