def do_stuff(pc, indices, model, rotated_shadow, img_file):
scene = Scene()
camera = VirtualCamera(ci, cp)
scene.camera = camera
# Works
shadow_obj = SceneObject(rotated_shadow)
scene.add_object('shadow', shadow_obj)
wd = scene.wrapped_render([RenderMode.DEPTH])[0]
wd_bi = wd.to_binary()
vis2d.figure()
vis2d.imshow(wd_bi)
vis2d.show()
# Doesn't work yet
plane = pc.data.T[indices]
plane_pc = PointCloud(plane.T, pc.frame)
di = ci.project_to_image(plane_pc)
bi = di.to_binary()
vis2d.figure()
vis2d.imshow(bi)
vis2d.show()
# Works
both = bi.mask_binary(wd_bi)
vis2d.figure()
vis2d.imshow(both)
vis2d.show()
def fine_grid_search(pc, indices, model, shadow, splits):
length, width, height = shadow.extents
split_size = max(length, width)
pc_data, ind = get_pc_data(pc, indices)
maxes = np.max(pc_data, axis=0)
mins = np.min(pc_data, axis=0)
bin_base = mins[2]
plane_normal = model[0:3]
#splits = 3
step_size = split_size / splits
plane_data = get_plane_data(pc, indices)
plane_pc = PointCloud(plane_data.T, pc.frame)
plane_pc = cp.inverse().apply(plane_pc)
di = ci.project_to_image(plane_pc)
bi = di.to_binary()
bi = bi.inverse()
scene = Scene()
camera = VirtualCamera(ci, cp)
scene.camera = camera
shadow_obj = SceneObject(shadow)
scene.add_object('shadow', shadow_obj)
orig_tow = shadow_obj.T_obj_world
numx = (int(np.round((maxes[0]-mins[0])/split_size)) - 1) * splits + 1
numy = (int(np.round((maxes[1]-mins[1])/split_size)) - 1) * splits + 1
scores = np.zeros((numx, numy))
for i in range(numx):
x = mins[0] + i*step_size
for j in range(numy):
y = mins[1] + j*step_size
for tow in transforms(pc, pc_data, shadow, x, y, x+split_size, y+split_size, 8, orig_tow):
shadow_obj.T_obj_world = tow
scores[i][j] = under_shadow(scene, bi)
shadow_obj.T_obj_world = orig_tow
print("\nScores: \n" + str(scores))
best = best_cell(scores)
print("\nBest Cell: " + str(best) + ", with score = " + str(scores[best[0]][best[1]]))
#-------
# Visualize best placement
vis3d.figure()
x = mins[0] + best[0]*step_size
y = mins[1] + best[1]*step_size
cell_indices = np.where((x < pc_data[:,0]) & (pc_data[:,0] < x+split_size) & (y < pc_data[:,1]) & (pc_data[:,1] < y+split_size))[0]
points = pc_data[cell_indices]
rest = pc_data[np.setdiff1d(np.arange(len(pc_data)), cell_indices)]
vis3d.points(points, color=(0,1,1))
vis3d.points(rest, color=(1,0,1))
vis3d.show()
#--------
return best, scene
def grid_search(pc, indices, model, shadow, img_file):
length, width, height = shadow.extents
split_size = max(length, width)
pc_data, ind = get_pc_data(pc, indices)
maxes = np.max(pc_data, axis=0)
mins = np.min(pc_data, axis=0)
bin_base = mins[2]
plane_normal = model[0:3]
scores = np.zeros((int(np.round((maxes[0] - mins[0]) / split_size)),
int(np.round((maxes[1] - mins[1]) / split_size))))
for i in range(int(np.round((maxes[0] - mins[0]) / split_size))):
x = mins[0] + i * split_size
for j in range(int(np.round((maxes[1] - mins[1]) / split_size))):
y = mins[1] + j * split_size
#binarized_overlap_image(pc, x, y, x+split_size, y+split_size, shadow, plane_normal, indices, model)
for sh in rotations(shadow, 8):
#overlap_size = binarized_overlap_image(pc, x, y, x+split_size, y+split_size, sh, plane_normal, indices, model)
#scores[i][j] = -1*overlap_size
scene = Scene()
camera = VirtualCamera(ci, cp)
scene.camera = camera
scores[i][j] = under_shadow(pc, pc_data, indices, model, sh, x,
x + split_size, y, y + split_size,
scene)
print("\nScores: \n" + str(scores))
best = best_cell(scores)
print("\nBest Cell: " + str(best) + ", with score = " +
str(scores[best[0]][best[1]]))
#-------
# Visualize best placement
vis3d.figure()
x = mins[0] + best[0] * split_size
y = mins[1] + best[1] * split_size
cell_indices = np.where((x < pc_data[:, 0])
& (pc_data[:, 0] < x + split_size)
& (y < pc_data[:, 1])
& (pc_data[:, 1] < y + split_size))[0]
points = pc_data[cell_indices]
rest = pc_data[np.setdiff1d(np.arange(len(pc_data)), cell_indices)]
vis3d.points(points, color=(0, 1, 1))
vis3d.points(rest, color=(1, 0, 1))
vis3d.show()
def create_scene(camera, workspace_objects):
# Start with an empty scene
scene = Scene()
# Create a VirtualCamera
virt_cam = VirtualCamera(camera.intrinsics, camera.pose)
# Add the camera to the scene
scene.camera = virt_cam
mp = MaterialProperties(
color=np.array([0.3,0.3,0.3]),
k_a=0.5, k_d=0.3, k_s=0.0, alpha=10.0
)
if camera.geometry is not None:
so = SceneObject(camera.geometry, camera.pose.copy(), mp)
scene.add_object(camera.name, so)
return scene
skew=0.0,
height=480,
width=640)
# Set up the camera pose (z axis faces away from scene, x to right, y up)
cp = RigidTransform(rotation=np.array([[0.0, 0.0, 1.0], [0.0, -1.0, 0.0],
[1.0, 0.0, 0.0]]),
translation=np.array([-0.3, 0.0, 0.0]),
from_frame='camera',
to_frame='world')
# Create a VirtualCamera
camera = VirtualCamera(ci, cp)
# Add the camera to the scene
scene.camera = camera
#====================================
# Render images
#====================================
# Render raw numpy arrays containing color and depth
color_image_raw, depth_image_raw = scene.render(render_color=True)
# Alternatively, just render a depth image
depth_image_raw = scene.render(render_color=False)
# Alternatively, collect wrapped images
wrapped_color, wrapped_depth, wrapped_segmask = scene.wrapped_render(
[RenderMode.COLOR, RenderMode.DEPTH, RenderMode.SEGMASK])
def fast_grid_search(pc, indices, model, shadow, img_file):
length, width, height = shadow.extents
split_size = max(length, width)
pc_data, ind = get_pc_data(pc, indices)
maxes = np.max(pc_data, axis=0)
mins = np.min(pc_data, axis=0)
bin_base = mins[2]
plane_normal = model[0:3]
di_temp = ci.project_to_image(pc)
vis2d.figure()
vis2d.imshow(di_temp)
vis2d.show()
plane_data = pc.data.T[indices]
#all_indices = np.where([(plane_data[::,2] > 0.795) & (plane_data[::,2] < 0.862)])
#all_indices = np.where((plane_data[::,1] < 0.16) & (plane_data[::,1] > -0.24) & (plane_data[::,0] > -0.3) & (plane_data[::,0] < 0.24))[0]
#plane_data = plane_data[all_indices]
plane_pc = PointCloud(plane_data.T, pc.frame)
di = ci.project_to_image(plane_pc)
bi = di.to_binary()
scene = Scene()
camera = VirtualCamera(ci, cp)
scene.camera = camera
# Get shadow depth img.
shadow_obj = SceneObject(shadow)
scene.add_object('shadow', shadow_obj)
orig_tow = shadow_obj.T_obj_world
scores = np.zeros((int(np.round((maxes[0] - mins[0]) / split_size)),
int(np.round((maxes[1] - mins[1]) / split_size))))
for i in range(int(np.round((maxes[0] - mins[0]) / split_size))):
x = mins[0] + i * split_size
for j in range(int(np.round((maxes[1] - mins[1]) / split_size))):
y = mins[1] + j * split_size
for tow in transforms(pc, pc_data, shadow, x, y, x + split_size,
y + split_size, 8):
shadow_obj.T_obj_world = tow
scores[i][j] = under_shadow(pc, pc_data, indices, model,
shadow, x, x + split_size, y,
y + split_size, scene, bi)
shadow_obj.T_obj_world = orig_tow
print("\nScores: \n" + str(scores))
best = best_cell(scores)
print("\nBest Cell: " + str(best) + ", with score = " +
str(scores[best[0]][best[1]]))
#-------
# Visualize best placement
vis3d.figure()
x = mins[0] + best[0] * split_size
y = mins[1] + best[1] * split_size
cell_indices = np.where((x < pc_data[:, 0])
& (pc_data[:, 0] < x + split_size)
& (y < pc_data[:, 1])
& (pc_data[:, 1] < y + split_size))[0]
points = pc_data[cell_indices]
rest = pc_data[np.setdiff1d(np.arange(len(pc_data)), cell_indices)]
vis3d.points(points, color=(0, 1, 1))
vis3d.points(rest, color=(1, 0, 1))
vis3d.show()
def fast_grid_search(pc, indices, model, shadow):
length, width, height = shadow.extents
split_size = max(length, width)
pc_data, ind = get_pc_data(pc, indices)
maxes = np.max(pc_data, axis=0)
mins = np.min(pc_data, axis=0)
bin_base = mins[2]
plane_normal = model[0:3]
#di_temp = ci.project_to_image(pc)
#vis2d.figure()
#vis2d.imshow(di_temp)
#vis2d.show()
#plane_data = pc.data.T[indices]
#plane_pc = PointCloud(plane_data.T, pc.frame)
#di = ci.project_to_image(plane_pc)
#bi = di.to_binary()
plane_data = get_plane_data(pc, indices)
plane_pc = PointCloud(plane_data.T, pc.frame)
#vis3d.figure()
#vis3d.points(plane_pc)
#vis3d.show()
plane_pc = cp.inverse().apply(plane_pc)
di = ci.project_to_image(plane_pc)
bi = di.to_binary()
bi = bi.inverse()
#vis2d.figure()
#vis2d.imshow(bi)
#vis2d.show()
scene = Scene()
camera = VirtualCamera(ci, cp)
scene.camera = camera
shadow_obj = SceneObject(shadow)
scene.add_object('shadow', shadow_obj)
orig_tow = shadow_obj.T_obj_world
#tr = transforms(pc, pc_data, shadow, mins[0], mins[1], mins[0]+split_size, mins[1]+split_size, 8, orig_tow)
#shadow_obj.T_obj_world = tr[0]
wd = scene.wrapped_render([RenderMode.DEPTH])[0]
wd_bi = wd.to_binary()
#vis2d.figure()
#vis2d.imshow(wd_bi)
#vis2d.show()
scores = np.zeros((int(np.round((maxes[0]-mins[0])/split_size)), int(np.round((maxes[1]-mins[1])/split_size))))
for i in range(int(np.round((maxes[0]-mins[0])/split_size))):
x = mins[0] + i*split_size
for j in range(int(np.round((maxes[1]-mins[1])/split_size))):
y = mins[1] + j*split_size
for tow in transforms(pc, pc_data, shadow, x, y, x+split_size, y+split_size, 8, orig_tow):
shadow_obj.T_obj_world = tow
scores[i][j] = under_shadow(scene, bi)
shadow_obj.T_obj_world = orig_tow
print("\nScores: \n" + str(scores))
best = best_cell(scores)
print("\nBest Cell: " + str(best) + ", with score = " + str(scores[best[0]][best[1]]))
#-------
# Visualize best placement
vis3d.figure()
x = mins[0] + best[0]*split_size
y = mins[1] + best[1]*split_size
cell_indices = np.where((x < pc_data[:,0]) & (pc_data[:,0] < x+split_size) & (y < pc_data[:,1]) & (pc_data[:,1] < y+split_size))[0]
points = pc_data[cell_indices]
rest = pc_data[np.setdiff1d(np.arange(len(pc_data)), cell_indices)]
vis3d.points(points, color=(0,1,1))
vis3d.points(rest, color=(1,0,1))
vis3d.show()
