hofee/nbv_sim
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Small change to the visualization

Browse Source
This commit is contained in:
Michel Breyer 2021-08-25 14:57:43 +02:00
parent 327826de90
commit fc8d9a8791
3 changed files with 16 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
active_grasp/nbv.py
View File

@ -28,7 +28,7 @@ class NextBestView(BasePolicy):
utilities = gains / np.sum(gains) - costs / np.sum(costs) utilities = gains / np.sum(gains) - costs / np.sum(costs)
# Visualize # 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 # Determine next-best-view
i = np.argmax(utilities) i = np.argmax(utilities)
@ -86,7 +86,7 @@ class NextBestView(BasePolicy):
direction = np.r_[(u - cx) / fx, (v - cy) / fy, 1.0] direction = np.r_[(u - cx) / fx, (v - cy) / fy, 1.0]
direction = view.rotation.apply(direction / np.linalg.norm(direction)) 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) # rospy.sleep(0.01)
t, tsdf_prev = t_min, -1.0 t, tsdf_prev = t_min, -1.0
@ -94,7 +94,7 @@ class NextBestView(BasePolicy):
p = origin + t * direction p = origin + t * direction
t += t_step t += t_step
# self.visualizer.point(self.task_frame, p) # self.vis.point(self.task_frame, p)
# rospy.sleep(0.01) # rospy.sleep(0.01)
index = get_voxel_at(p) index = get_voxel_at(p)

18
active_grasp/policy.py
View File

@ -36,7 +36,7 @@ class BasePolicy(Policy):
self.score_fn = lambda g: g.pose.translation[2] # TODO self.score_fn = lambda g: g.pose.translation[2] # TODO
def init_visualizer(self): def init_visualizer(self):
self.visualizer = Visualizer() self.vis = Visualizer()
def activate(self, bbox): def activate(self, bbox):
self.bbox = bbox self.bbox = bbox
@ -60,8 +60,8 @@ class BasePolicy(Policy):
self.done = False self.done = False
self.best_grasp = None self.best_grasp = None
self.visualizer.clear() self.vis.clear()
self.visualizer.bbox(self.base_frame, bbox) self.vis.bbox(self.base_frame, bbox)
def integrate_img(self, img, extrinsic): def integrate_img(self, img, extrinsic):
self.viewpoints.append(extrinsic.inv()) self.viewpoints.append(extrinsic.inv())
@ -76,11 +76,11 @@ class BasePolicy(Policy):
self.width_hist[t, ...] = out.width self.width_hist[t, ...] = out.width
mean_qual = self.compute_mean_quality() 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.vis.scene_cloud(self.task_frame, self.tsdf.get_scene_cloud())
self.visualizer.map_cloud(self.task_frame, voxel_size, tsdf_grid) self.vis.map_cloud(self.task_frame, self.tsdf.get_map_cloud())
self.visualizer.path(self.base_frame, self.viewpoints) self.vis.path(self.base_frame, self.viewpoints)
def compute_best_grasp(self): def compute_best_grasp(self):
if self.filter_grasps: if self.filter_grasps:
@ -97,8 +97,8 @@ class BasePolicy(Policy):
grasps = self.transform_and_reject(grasps) grasps = self.transform_and_reject(grasps)
grasps = sort_grasps(grasps, self.score_fn) grasps = sort_grasps(grasps, self.score_fn)
self.visualizer.quality(self.task_frame, self.tsdf.voxel_size, qual) self.vis.quality(self.task_frame, self.tsdf.voxel_size, qual)
self.visualizer.grasps(self.base_frame, grasps) self.vis.grasps(self.base_frame, grasps)
return grasps[0] if len(grasps) > 0 else None return grasps[0] if len(grasps) > 0 else None

7
active_grasp/visualization.py
View File

@ -63,9 +63,10 @@ class Visualizer:
) )
self.draw([marker]) self.draw([marker])
def map_cloud(self, frame, voxel_size, tsdf): def map_cloud(self, frame, cloud):
points, values = grid_to_map_cloud(voxel_size, tsdf, threshold=0.0) points = np.asarray(cloud.points)
msg = to_cloud_msg(frame, points, intensities=values) distances = np.expand_dims(np.asarray(cloud.colors)[:, 0], 1)
msg = to_cloud_msg(frame, points, distances=distances)
self.map_cloud_pub.publish(msg) self.map_cloud_pub.publish(msg)
def path(self, frame, poses): def path(self, frame, poses):