From 54bd24ff75f479b3fddb6ef46970cf014e6e1e34 Mon Sep 17 00:00:00 2001 From: Michel Breyer Date: Sun, 12 Sep 2021 12:02:34 +0200 Subject: [PATCH] Clean up ray casting --- active_grasp/nbv.py | 21 ++++++++++----------- active_grasp/policy.py | 2 +- 2 files changed, 11 insertions(+), 12 deletions(-) diff --git a/active_grasp/nbv.py b/active_grasp/nbv.py index c195814..9e1f11a 100644 --- a/active_grasp/nbv.py +++ b/active_grasp/nbv.py @@ -9,6 +9,7 @@ from .policy import MultiViewPolicy class NextBestView(MultiViewPolicy): def __init__(self): super().__init__() + self.min_z_dist = rospy.get_param("~camera/min_z_dist") self.max_views = 40 def activate(self, bbox, view_sphere): @@ -57,11 +58,15 @@ class NextBestView(MultiViewPolicy): return False def ig_fn(self, view, downsample=20): + tsdf_grid, voxel_size = self.tsdf.get_grid(), self.tsdf.voxel_size + tsdf_grid = -1.0 + 2.0 * tsdf_grid # Open3D maps tsdf to [0,1] + fx = self.intrinsic.fx / downsample fy = self.intrinsic.fy / downsample cx = self.intrinsic.cx / downsample cy = self.intrinsic.cy / downsample + # Project bbox onto the image plane to get better bounds T_cam_base = view.inv() corners = np.array([T_cam_base.apply(p) for p in self.bbox.corners]).T u = (fx * corners[0] / corners[2] + cx).round().astype(int) @@ -69,47 +74,41 @@ class NextBestView(MultiViewPolicy): u_min, u_max = u.min(), u.max() v_min, v_max = v.min(), v.max() - t_min = 0.1 + t_min = self.min_z_dist t_max = corners[2].max() # TODO This bound might be a bit too short - t_step = 0.01 + t_step = np.sqrt(3) * voxel_size # TODO replace with line rasterization view = self.T_task_base * view # We'll work in the task frame from now on - - tsdf_grid, voxel_size = self.tsdf.get_grid(), self.tsdf.voxel_size + origin = view.translation def get_voxel_at(p): index = (p / voxel_size).astype(int) return index if (index >= 0).all() and (index < 40).all() else None voxel_indices = [] - for u in range(u_min, u_max): for v in range(v_min, v_max): - origin = view.translation direction = np.r_[(u - cx) / fx, (v - cy) / fy, 1.0] direction = view.rotation.apply(direction / np.linalg.norm(direction)) # self.vis.rays(self.task_frame, origin, [direction]) - # rospy.sleep(0.01) t, tsdf_prev = t_min, -1.0 while t < t_max: p = origin + t * direction t += t_step # self.vis.point(self.task_frame, p) - # rospy.sleep(0.01) index = get_voxel_at(p) if index is not None: i, j, k = index - tsdf = -1 + 2 * tsdf_grid[i, j, k] # Open3D maps tsdf to [0,1] + tsdf = tsdf_grid[i, j, k] if tsdf * tsdf_prev < 0 and tsdf_prev > -1: # Crossed a surface break - # TODO check whether the voxel lies within the bounding box ? voxel_indices.append(index) tsdf_prev = tsdf # Count rear side voxels i, j, k = np.unique(voxel_indices, axis=0).T tsdfs = tsdf_grid[i, j, k] - ig = np.logical_and(tsdfs > 0.0, tsdfs < 0.5).sum() + ig = np.logical_and(tsdfs > -1.0, tsdfs < 0.0).sum() return ig diff --git a/active_grasp/policy.py b/active_grasp/policy.py index a98075c..de3186d 100644 --- a/active_grasp/policy.py +++ b/active_grasp/policy.py @@ -115,7 +115,7 @@ class SingleViewPolicy(Policy): class MultiViewPolicy(Policy): def activate(self, bbox, view_sphere): super().activate(bbox, view_sphere) - self.T = 5 # Window size of grasp prediction history + self.T = 10 # Window size of grasp prediction history self.qual_hist = np.zeros((self.T,) + (40,) * 3, np.float32) def integrate(self, img, x):