Detect straight edges from organized pointcloud