Plane segmentation working

Added viz for point cloud. Clustering is slow