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 generate_images(self, salient_edge_set, n_samples=1):
"""Generate depth image, normal image, and binary edge mask tuples.
Parameters
----------
salient_edge_set : SalientEdgeSet
A salient edge set to generate images of.
n_samples : int
The number of samples to generate.
Returns
-------
depth_ims : (n,) list of perception.DepthImage
Randomly-rendered depth images of object.
normal_ims : (n,) list of perception.PointCloudImage
Normals for the given image
edge_masks : (n,) list of perception.BinaryImage
Masks for pixels on the salient edges of the object.
"""
# Compute stable poses of mesh
mesh = salient_edge_set.mesh
stp_pose_tfs, probs = mesh.compute_stable_poses()
probs = probs / sum(probs)
# Generate n renders
depth_ims, normal_ims, edge_masks = [], [], []
scene = Scene()
so = SceneObject(mesh, RigidTransform(from_frame='obj', to_frame='world'))
scene.add_object('object', so)
for i in range(n_samples):
# Sample random stable pose.
tf_id = np.random.choice(np.arange(len(probs)), p=probs)
tf = stp_pose_tfs[tf_id]
T_obj_world = RigidTransform(tf[:3,:3], tf[:3,3], from_frame='obj', to_frame='world')
so.T_obj_world = T_obj_world
# Create the random uniform workspace sampler
ws_cfg = self._config['worksurface_rv_config']
uvs = UniformPlanarWorksurfaceImageRandomVariable('object', scene, [RenderMode.DEPTH], frame='camera', config=ws_cfg)
# Sample and extract the depth image, camera intrinsics, and T_obj_camera
sample = uvs.sample()
depth_im = sample.renders[RenderMode.DEPTH]
cs = sample.camera
ci = CameraIntrinsics(frame='camera', fx=cs.focal, fy=cs.focal, cx=cs.cx, cy=cs.cy,
skew=0.0, height=ws_cfg['im_height'], width=ws_cfg['im_width'])
T_obj_camera = cs.T_camera_world.inverse().dot(T_obj_world)
edge_mask = self._compute_edge_mask(salient_edge_set, depth_im, ci, T_obj_camera)
point_cloud_im = ci.deproject_to_image(depth_im)
normal_im = point_cloud_im.normal_cloud_im()
depth_ims.append(depth_im)
normal_ims.append(normal_im)
edge_masks.append(edge_mask)
return depth_ims, normal_ims, edge_masks
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 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
def load_3d_model(model_path):
# Start with an empty scene
scene = Scene()
# Add objects to the scene
# Begin by loading meshes
pawn_mesh = trimesh.load_mesh(model_path)
# Set up object's pose in the world
pawn_pose = RigidTransform(rotation=np.eye(3),
translation=np.array([0.0, 0.0, 0.0]),
from_frame='obj',
to_frame='world')
# Set up each object's material properties
pawn_material = MaterialProperties(color=np.array([1.0, 1.0, 1.0]),
k_a=1.0,
k_d=1.0,
k_s=0.0,
alpha=1.0,
smooth=False,
wireframe=False)
# Create SceneObjects for each object
pawn_obj = SceneObject(pawn_mesh, pawn_pose, pawn_material)
# Add the SceneObjects to the scene
scene.add_object('pawn', pawn_obj)
return scene, pawn_mesh
# k_d = 0.3,
# k_s = 0.1,
# alpha = 10.0,
# smooth=False
#)
bar_material = pawn_material
# Create SceneObjects for each object
pawn_obj = SceneObject(pawn_mesh, pawn_pose, pawn_material)
bar_obj = SceneObject(bar_mesh, bar_pose, bar_material)
pawn_inst_obj = InstancedSceneObject(pawn_mesh, [pawn_pose, bar_pose],
colors=np.array([[0, 0, 1], [0, 1, 0]]),
material=pawn_material)
# Add the SceneObjects to the scene
scene.add_object('pawn', pawn_inst_obj)
scene.add_object('bar', bar_obj)
#====================================
# Add lighting to the scene
#====================================
# Create an ambient light
ambient = AmbientLight(color=np.array([1.0, 1.0, 1.0]), strength=1.0)
# Create a point light
points = []
#for i in range(6):
# points.append(
# PointLight(
# setup sensor
sensor = RgbdSensorFactory.sensor(sensor_type, sensor_config)
sensors[sensor_name] = sensor
# start the sensor
sensor.start()
camera_intr = sensor.ir_intrinsics
camera_intr = camera_intr.resize(im_rescale_factor)
camera_intrs[sensor_name] = camera_intr
# render image of static workspace
scene = Scene()
camera = VirtualCamera(camera_intr, T_camera_world)
scene.camera = camera
for obj_key, scene_obj in workspace_objects.iteritems():
scene.add_object(obj_key, scene_obj)
workspace_ims[sensor_name] = scene.wrapped_render([RenderMode.DEPTH])[0]
# fix dataset config
dataset_config['fields']['raw_color_ims']['height'] = camera_intr.height
dataset_config['fields']['raw_color_ims']['width'] = camera_intr.width
dataset_config['fields']['raw_depth_ims']['height'] = camera_intr.height
dataset_config['fields']['raw_depth_ims']['width'] = camera_intr.width
dataset_config['fields']['color_ims']['height'] = camera_intr.height
dataset_config['fields']['color_ims']['width'] = camera_intr.width
dataset_config['fields']['depth_ims']['height'] = camera_intr.height
dataset_config['fields']['depth_ims']['width'] = camera_intr.width
dataset_config['fields']['segmasks']['height'] = camera_intr.height
dataset_config['fields']['segmasks']['width'] = camera_intr.width
# open dataset
def generate_examples(self, salient_edge_set_filename, n_samples=1):
"""Generate RegistrationExamples for evaluating the algorithm.
Parameters
----------
salient_edge_set_filename : str
A file containing the salient edge set to generate images of.
n_samples : int
The number of samples to generate.
Returns
-------
list of RegistrationExample
A list of RegistrationExamples.
"""
# Compute stable poses of mesh
salient_edge_set = SalientEdgeSet.load(salient_edge_set_filename)
mesh = salient_edge_set.mesh
stp_pose_tfs, probs = mesh.compute_stable_poses()
probs = probs / sum(probs)
# Generate n renders
examples = []
scene = Scene()
so = SceneObject(mesh,
RigidTransform(from_frame='obj', to_frame='world'))
scene.add_object('object', so)
for i in range(n_samples):
# Sample random stable pose.
tf_id = np.random.choice(np.arange(len(probs)), p=probs)
tf = stp_pose_tfs[tf_id]
T_obj_world = RigidTransform(tf[:3, :3],
tf[:3, 3],
from_frame='obj',
to_frame='world')
so.T_obj_world = T_obj_world
# Create the random uniform workspace sampler
ws_cfg = self._config['worksurface_rv_config']
uvs = UniformPlanarWorksurfaceImageRandomVariable(
'object',
scene, [RenderMode.DEPTH],
frame='camera',
config=ws_cfg)
# Sample and extract the depth image, camera intrinsics, and T_obj_camera
sample = uvs.sample()
depth_im = sample.renders[RenderMode.DEPTH]
cs = sample.camera
ci = CameraIntrinsics(frame='camera',
fx=cs.focal,
fy=cs.focal,
cx=cs.cx,
cy=cs.cy,
skew=0.0,
height=ws_cfg['im_height'],
width=ws_cfg['im_width'])
T_obj_camera = cs.T_camera_world.inverse().dot(T_obj_world)
examples.append(
RegistrationExample(salient_edge_set_filename, depth_im, ci,
T_obj_camera))
return examples
)
sphere_material = MaterialProperties(
color = np.array([0.1, 0.1, 0.5]),
k_a = 0.3,
k_d = 1.0,
k_s = 1.0,
alpha = 10.0,
smooth=True
)
# Create SceneObjects for each object
cube_obj = SceneObject(cube_mesh, cube_pose, cube_material)
sphere_obj = SceneObject(sphere_mesh, sphere_pose, sphere_material)
# Add the SceneObjects to the scene
scene.add_object('cube', cube_obj)
scene.add_object('sphere', sphere_obj)
#====================================
# Add lighting to the scene
#====================================
# Create an ambient light
ambient = AmbientLight(
color=np.array([1.0, 1.0, 1.0]),
strength=1.0
)
# Create a point light
point = PointLight(
location=np.array([1.0, 2.0, 3.0]),
translation=np.array([0.0, 0.0, 0.0]),
from_frame='obj',
to_frame='world')
obj_material_properties = MaterialProperties(
color=np.array([66, 134, 244]) / 255.,
# color = 5.0*np.array([0.1, 0.1, 0.1]),
k_a=0.3,
k_d=0.5,
k_s=0.2,
alpha=10.0,
smooth=False,
wireframe=False)
obj = SceneObject(mesh, default_pose, obj_material_properties)
scene.add_object('to_render', obj)
print("ADDED OBJECT SUCCESSFULLY")
# table_obj_properties = MaterialProperties(
# color = np.array([0, 0, 0]),
# )
# wrap the table as a SceneObject
# table_mesh = trimesh.load(PLANE_MESH)
# T_table_world = RigidTransform.load(PLANE_POSE)
# table = SceneObject(table_mesh, T_table_world, table_obj_properties)
# scene.add_object('table', table)
# add light
ambient = AmbientLight(color=np.array([1.0, 1.0, 1.0]), strength=1.0)
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()
class Generator:
def __init__(self):
DATASET_DIR = pt.abspath('.')
OUTPUT_DIR = pt.abspath('./data')
self.sampler = ModelSampler(DATASET_DIR)
self.scene = Scene()
self.local_scene = Scene()
self.grip_scene = Scene()
self.dataset_dir = DATASET_DIR
self.output_dir = OUTPUT_DIR
self.image_dir = 'color-input-synth'
self.depth_dir = 'depth-input-synth'
self.seg_dir = 'label-synth'
clear_dir(pt.join(self.output_dir, self.image_dir))
clear_dir(pt.join(self.output_dir, self.depth_dir))
clear_dir(pt.join(self.output_dir, self.seg_dir))
ci = CameraIntrinsics(frame='camera',
fx=617.0,
fy=617.0,
cx=320.0,
cy=240.0,
skew=0.0,
height=480,
width=640)
# Set up the camera pose (z axis faces away from scene, x to right, y up)
cp1 = RigidTransform(rotation=trimesh.transformations.rotation_matrix(
np.deg2rad(-30), [1, 0, 0])[:3, :3]
@ trimesh.transformations.rotation_matrix(
np.deg2rad(180), [0, 1, 0])[:3, :3],
translation=np.array([0.0, 0.75, 1.0]),
from_frame='camera',
to_frame='world')
cp2 = RigidTransform(rotation=trimesh.transformations.rotation_matrix(
np.deg2rad(37), [1, 0, 0])[:3, :3],
translation=np.array([0.0, 0.0, 1.0]),
from_frame='camera',
to_frame='world')
camera1 = VirtualCamera(ci, cp1)
camera2 = VirtualCamera(ci, cp2)
# Add the camera to the scene
self.scene.camera = camera1
self.local_scene.camera = camera1
self.grip_scene.camera = camera1
def clear_scene(self, scene):
obj_names = scene.objects.keys()
light_names = scene.lights.keys()
for obj_name in list(obj_names):
scene.remove_object(obj_name)
for light_name in list(light_names):
scene.remove_light(light_name)
def save_sample(self, idx, color, depth, segmask):
image_filename = pt.join(self.output_dir, self.image_dir,
'{:05d}.png'.format(idx))
depth_filename = pt.join(self.output_dir, self.depth_dir,
'{:05d}.png'.format(idx))
seg_filename = pt.join(self.output_dir, self.seg_dir,
'{:05d}.png'.format(idx))
cv.imwrite(image_filename, color.data)
cv.imwrite(depth_filename,
(10000 * depth.data).astype(np.uint16)) #in 0.1mm
cv.imwrite(seg_filename, segmask)
def process_depths(self, depths, grip_depths):
'''Process raw depths to generate true segmask
'''
assert (len(depths) > 0)
self.depths = depths
self.grip_depths = grip_depths
ds = np.sum(np.stack(depths), axis=0)
gds = np.sum(np.stack(grip_depths), axis=0)
ds[ds == 0.0] = 255
ds[ds != 255] = 0
ds[gds != 0] = 1
ds = ds.astype(np.uint8)
return ds
def generate_scene(self):
depths = []
grip_depths = []
self.scene.add_object('ground', self.sampler.sample_ground_obj())
for model_obj, grip_obj, model_name in self.sampler.sample_scene_objs(
):
self.scene.add_object(model_name, model_obj)
self.local_scene.add_object(model_name, model_obj)
self.grip_scene.add_object(model_name, grip_obj)
depth = self.local_scene.render(render_color=False)
depth_grip = self.grip_scene.render(render_color=False)
depths.append(depth)
grip_depths.append(depth_grip)
self.clear_scene(self.local_scene)
self.clear_scene(self.grip_scene)
# Create an ambient light
#self.depths = depths
ambient = self.sampler.sample_ambient_light()
self.scene.ambient_light = ambient # only one ambient light per scene
directional_lights = self.sampler.sample_direc_lights()
for i, directional_light in enumerate(directional_lights):
self.scene.add_light('direc_{}'.format(i), directional_light)
return self.process_depths(depths, grip_depths)
def prepare_batch(self, num=3):
#if dir exist data will be replaced!
imdir = pt.join(self.output_dir, self.image_dir)
dpdir = pt.join(self.output_dir, self.depth_dir)
segdir = pt.join(self.output_dir, self.seg_dir)
clear_dir(imdir)
clear_dir(dpdir)
clear_dir(segdir)
for i in range(num):
segmask = self.generate_scene()
wrapped_color, wrapped_depth = self.scene.wrapped_render(
[RenderMode.COLOR, RenderMode.DEPTH])
self.save_sample(i, wrapped_color, wrapped_depth, segmask)
self.clear_scene(self.scene)
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()
