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

Detect surface crossings

Browse Source
This commit is contained in:
Michel Breyer 2021-08-18 10:05:04 +02:00
parent a55b02b9ed
commit 37ac25796f
1 changed files with 14 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
active_grasp/nbv.py
View File

@ -69,6 +69,12 @@ class NextBestView(BasePolicy):
view = self.T_task_base * view # We'll work in the task frame from now on 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
def get_voxel_at(p):
index = (p / voxel_size).astype(int)
return index if (index >= 0).all() and (index < 40).all() else None
for u in range(u_min, u_max): for u in range(u_min, u_max):
for v in range(v_min, v_max): for v in range(v_min, v_max):
origin = view.translation origin = view.translation
@ -78,7 +84,7 @@ class NextBestView(BasePolicy):
self.visualizer.rays(self.task_frame, origin, [direction]) self.visualizer.rays(self.task_frame, origin, [direction])
rospy.sleep(0.1) rospy.sleep(0.1)
t = t_min t, tsdf_prev = t_min, -1.0
while t < t_max: while t < t_max:
p = origin + t * direction p = origin + t * direction
t += t_step t += t_step
@ -86,6 +92,13 @@ class NextBestView(BasePolicy):
self.visualizer.point(self.task_frame, p) self.visualizer.point(self.task_frame, p)
rospy.sleep(0.1) rospy.sleep(0.1)
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]
if tsdf * tsdf_prev < 0 and tsdf_prev > -1: # Crossed a surface
break
tsdf_prev = tsdf
return 1.0 return 1.0
def compute_cost(self, view): def compute_cost(self, view):