Changed visualization to use PCL, cleaned up code