diff --git a/active_grasp/nbv.py b/active_grasp/nbv.py
index bd9fd3f..f9b0d24 100644
--- a/active_grasp/nbv.py
+++ b/active_grasp/nbv.py
@@ -28,7 +28,7 @@ class NextBestView(BasePolicy):
         utilities = gains / np.sum(gains) - costs / np.sum(costs)
 
         # Visualize
-        self.visualizer.views(self.base_frame, self.intrinsic, views, utilities)
+        self.vis.views(self.base_frame, self.intrinsic, views, utilities)
 
         # Determine next-best-view
         i = np.argmax(utilities)
@@ -86,7 +86,7 @@ class NextBestView(BasePolicy):
                 direction = np.r_[(u - cx) / fx, (v - cy) / fy, 1.0]
                 direction = view.rotation.apply(direction / np.linalg.norm(direction))
 
-                # self.visualizer.rays(self.task_frame, origin, [direction])
+                # self.vis.rays(self.task_frame, origin, [direction])
                 # rospy.sleep(0.01)
 
                 t, tsdf_prev = t_min, -1.0
@@ -94,7 +94,7 @@ class NextBestView(BasePolicy):
                     p = origin + t * direction
                     t += t_step
 
-                    # self.visualizer.point(self.task_frame, p)
+                    # self.vis.point(self.task_frame, p)
                     # rospy.sleep(0.01)
 
                     index = get_voxel_at(p)
diff --git a/active_grasp/policy.py b/active_grasp/policy.py
index ec1cfc4..b0b16e7 100644
--- a/active_grasp/policy.py
+++ b/active_grasp/policy.py
@@ -36,7 +36,7 @@ class BasePolicy(Policy):
         self.score_fn = lambda g: g.pose.translation[2]  # TODO
 
     def init_visualizer(self):
-        self.visualizer = Visualizer()
+        self.vis = Visualizer()
 
     def activate(self, bbox):
         self.bbox = bbox
@@ -60,8 +60,8 @@ class BasePolicy(Policy):
         self.done = False
         self.best_grasp = None
 
-        self.visualizer.clear()
-        self.visualizer.bbox(self.base_frame, bbox)
+        self.vis.clear()
+        self.vis.bbox(self.base_frame, bbox)
 
     def integrate_img(self, img, extrinsic):
         self.viewpoints.append(extrinsic.inv())
@@ -76,11 +76,11 @@ class BasePolicy(Policy):
             self.width_hist[t, ...] = out.width
 
             mean_qual = self.compute_mean_quality()
-            self.visualizer.quality(self.task_frame, voxel_size, mean_qual)
+            self.vis.quality(self.task_frame, voxel_size, mean_qual)
 
-        self.visualizer.scene_cloud(self.task_frame, self.tsdf.get_scene_cloud())
-        self.visualizer.map_cloud(self.task_frame, voxel_size, tsdf_grid)
-        self.visualizer.path(self.base_frame, self.viewpoints)
+        self.vis.scene_cloud(self.task_frame, self.tsdf.get_scene_cloud())
+        self.vis.map_cloud(self.task_frame, self.tsdf.get_map_cloud())
+        self.vis.path(self.base_frame, self.viewpoints)
 
     def compute_best_grasp(self):
         if self.filter_grasps:
@@ -97,8 +97,8 @@ class BasePolicy(Policy):
         grasps = self.transform_and_reject(grasps)
         grasps = sort_grasps(grasps, self.score_fn)
 
-        self.visualizer.quality(self.task_frame, self.tsdf.voxel_size, qual)
-        self.visualizer.grasps(self.base_frame, grasps)
+        self.vis.quality(self.task_frame, self.tsdf.voxel_size, qual)
+        self.vis.grasps(self.base_frame, grasps)
 
         return grasps[0] if len(grasps) > 0 else None
 
diff --git a/active_grasp/visualization.py b/active_grasp/visualization.py
index 5504cbc..098d4b5 100644
--- a/active_grasp/visualization.py
+++ b/active_grasp/visualization.py
@@ -63,9 +63,10 @@ class Visualizer:
         )
         self.draw([marker])
 
-    def map_cloud(self, frame, voxel_size, tsdf):
-        points, values = grid_to_map_cloud(voxel_size, tsdf, threshold=0.0)
-        msg = to_cloud_msg(frame, points, intensities=values)
+    def map_cloud(self, frame, cloud):
+        points = np.asarray(cloud.points)
+        distances = np.expand_dims(np.asarray(cloud.colors)[:, 0], 1)
+        msg = to_cloud_msg(frame, points, distances=distances)
         self.map_cloud_pub.publish(msg)
 
     def path(self, frame, poses):