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 clf():
"""Clear the current figure
"""
Visualizer3D._scene = Scene(
background_color=Visualizer3D._scene.background_color)
Visualizer3D._scene.ambient_light = AmbientLight(color=[1.0, 1.0, 1.0],
strength=1.0)
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
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 figure(bgcolor=(1, 1, 1), size=(1000, 1000)):
"""Create a blank figure.
Parameters
----------
bgcolor : (3,) float
Color of the background with values in [0,1].
size : (2,) int
Width and height of the figure in pixels.
"""
Visualizer3D._scene = Scene(background_color=np.array(bgcolor))
Visualizer3D._scene.ambient_light = AmbientLight(color=[1.0, 1.0, 1.0],
strength=1.0)
Visualizer3D._init_size = np.array(size)
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
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
import numpy as np
import trimesh
from autolab_core import RigidTransform
from perception import CameraIntrinsics, RenderMode, ColorImage, DepthImage
import os
#os.environ['MESHRENDER_EGL_OFFSCREEN'] = 't'
from meshrender import Scene, MaterialProperties, AmbientLight, PointLight, SceneObject, VirtualCamera, DirectionalLight, SceneViewer, UniformPlanarWorksurfaceImageRandomVariable, InstancedSceneObject
# Start with an empty scene
scene = Scene()
#====================================
# Add objects to the scene
#====================================
# Begin by loading meshes
pawn_mesh = trimesh.load_mesh('./models/pawn.obj')
#pawn_mesh = trimesh.load_mesh('./models/pawn_large.obj')
bar_mesh = trimesh.load_mesh('./models/bar_clamp.obj')
# Set up each 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')
bar_pose = RigidTransform(rotation=np.eye(3),
translation=np.array([0.1, 0.07, 0.00]),
from_frame='obj',
to_frame='world')
tf_filename = '%s_to_world.tf' %(sensor_frame)
T_camera_world = RigidTransform.load(os.path.join(sensor_config['calib_dir'], sensor_frame, tf_filename))
sensor_poses[sensor_name] = T_camera_world
# 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
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
import numpy as np
import trimesh
from autolab_core import RigidTransform
from perception import CameraIntrinsics, RenderMode
from meshrender import Scene, MaterialProperties, AmbientLight, PointLight, SceneObject, VirtualCamera
# Start with an empty scene
scene = Scene()
#====================================
# Add objects to the scene
#====================================
# Begin by loading meshes
cube_mesh = trimesh.load_mesh('cube.obj')
sphere_mesh = trimesh.load_mesh('sphere.obj')
# Set up each object's pose in the world
cube_pose = RigidTransform(
rotation=np.eye(3),
translation=np.array([0.0, 0.0, 0.0]),
from_frame='obj',
to_frame='world'
)
sphere_pose = RigidTransform(
rotation=np.eye(3),
translation=np.array([1.0, 1.0, 0.0]),
from_frame='obj',
to_frame='world'
)
render_scene = args.render_scene
print(mesh_path)
logging.getLogger().setLevel(logging.INFO)
logging.info('Loading mesh...')
mesh = trimesh.load(mesh_path)
# copy mesh to pdc location
mesh.export(file_obj=args.output_folder + "/processed/fusion_mesh.ply",
file_type="ply")
logging.info('Computing stable poses...')
# transforms, probs = trimesh.poses.compute_stable_poses(mesh)
scene = Scene()
# wrap the object to be rendered as a SceneObject
# rotation, translation = RigidTransform.rotation_and_translation_from_matrix(transforms[stable_pose])
# T_obj_table = RigidTransform(rotation=rotation, translation=translation)
# default pose
default_pose = RigidTransform(rotation=np.eye(3),
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,
os.path.join(sensor_config["calib_dir"], sensor_frame, tf_filename)
)
sensor_poses[sensor_name] = T_camera_world
# 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(["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
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()
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)
class Visualizer3D:
"""
Class containing static methods for visualization.
The interface is styled after pyplot.
Should be thought of as a namespace rather than a class.
"""
_scene = Scene(background_color=np.array([1.0, 1.0, 1.0]))
_scene.ambient_light = AmbientLight(color=[1.0, 1.0, 1.0], strength=1.0)
_init_size = np.array([640, 480])
_save_directory = None
@staticmethod
def figure(bgcolor=(1, 1, 1), size=(1000, 1000)):
"""Create a blank figure.
Parameters
----------
bgcolor : (3,) float
Color of the background with values in [0,1].
size : (2,) int
Width and height of the figure in pixels.
"""
Visualizer3D._scene = Scene(background_color=np.array(bgcolor))
Visualizer3D._scene.ambient_light = AmbientLight(color=[1.0, 1.0, 1.0],
strength=1.0)
Visualizer3D._init_size = np.array(size)
@staticmethod
def show(animate=False, axis=np.array([0., 0., 1.]), clf=True, **kwargs):
"""Display the current figure and enable interaction.
Parameters
----------
animate : bool
Whether or not to animate the scene.
axis : (3,) float or None
If present, the animation will rotate about the given axis in world coordinates.
Otherwise, the animation will rotate in azimuth.
clf : bool
If true, the Visualizer is cleared after showing the figure.
kwargs : dict
Other keyword arguments for the SceneViewer instance.
"""
x = SceneViewer(Visualizer3D._scene,
size=Visualizer3D._init_size,
animate=animate,
animate_axis=axis,
save_directory=Visualizer3D._save_directory,
**kwargs)
if x.save_directory:
Visualizer3D._save_directory = x.save_directory
if clf:
Visualizer3D.clf()
@staticmethod
def render(n_frames=1, axis=np.array([0., 0., 1.]), clf=True, **kwargs):
"""Render frames from the viewer.
Parameters
----------
n_frames : int
Number of frames to render. If more than one, the scene will animate.
axis : (3,) float or None
If present, the animation will rotate about the given axis in world coordinates.
Otherwise, the animation will rotate in azimuth.
clf : bool
If true, the Visualizer is cleared after rendering the figure.
kwargs : dict
Other keyword arguments for the SceneViewer instance.
Returns
-------
list of perception.ColorImage
A list of ColorImages rendered from the viewer.
"""
v = SceneViewer(Visualizer3D._scene,
size=Visualizer3D._init_size,
animate=(n_frames > 1),
animate_axis=axis,
max_frames=n_frames,
**kwargs)
if clf:
Visualizer3D.clf()
return v.saved_frames
@staticmethod
def save(filename,
n_frames=1,
axis=np.array([0., 0., 1.]),
clf=True,
**kwargs):
"""Save frames from the viewer out to a file.
Parameters
----------
filename : str
The filename in which to save the output image. If more than one frame,
should have extension .gif.
n_frames : int
Number of frames to render. If more than one, the scene will animate.
axis : (3,) float or None
If present, the animation will rotate about the given axis in world coordinates.
Otherwise, the animation will rotate in azimuth.
clf : bool
If true, the Visualizer is cleared after rendering the figure.
kwargs : dict
Other keyword arguments for the SceneViewer instance.
"""
if n_frames > 1 and os.path.splitext(filename)[1] != '.gif':
raise ValueError('Expected .gif file for multiple-frame save.')
v = SceneViewer(Visualizer3D._scene,
size=Visualizer3D._init_size,
animate=(n_frames > 1),
animate_axis=axis,
max_frames=n_frames,
**kwargs)
data = [m.data for m in v.saved_frames]
if len(data) > 1:
imageio.mimwrite(filename,
data,
fps=v._animate_rate,
palettesize=128,
subrectangles=True)
else:
imageio.imwrite(filename, data[0])
if clf:
Visualizer3D.clf()
@staticmethod
def save_loop(filename,
framerate=30,
time=3.0,
axis=np.array([0., 0., 1.]),
clf=True,
**kwargs):
"""Off-screen save a GIF of one rotation about the scene.
Parameters
----------
filename : str
The filename in which to save the output image (should have extension .gif)
framerate : int
The frame rate at which to animate motion.
time : float
The number of seconds for one rotation.
axis : (3,) float or None
If present, the animation will rotate about the given axis in world coordinates.
Otherwise, the animation will rotate in azimuth.
clf : bool
If true, the Visualizer is cleared after rendering the figure.
kwargs : dict
Other keyword arguments for the SceneViewer instance.
"""
n_frames = framerate * time
az = 2.0 * np.pi / n_frames
Visualizer3D.save(filename,
max_frames=n_frames,
axis=axis,
clf=clf,
animate_rate=framerate,
animate_az=az)
@staticmethod
def clf():
"""Clear the current figure
"""
Visualizer3D._scene = Scene(
background_color=Visualizer3D._scene.background_color)
Visualizer3D._scene.ambient_light = AmbientLight(color=[1.0, 1.0, 1.0],
strength=1.0)
@staticmethod
def close(*args, **kwargs):
"""Close the current figure
"""
pass
@staticmethod
def get_object_keys():
"""Return the visualizer's object keys.
Returns
-------
list of str
The keys for the visualizer's objects.
"""
return Visualizer3D._scene.objects.keys()
@staticmethod
def get_object(name):
"""Return a SceneObject corresponding to the given name.
Returns
-------
meshrender.SceneObject
The corresponding SceneObject.
"""
return Visualizer3D._scene.objects[name]
@staticmethod
def points(points,
T_points_world=None,
color=np.array([0, 1, 0]),
scale=0.01,
n_cuts=20,
subsample=None,
random=False,
name=None):
"""Scatter a point cloud in pose T_points_world.
Parameters
----------
points : autolab_core.BagOfPoints or (n,3) float
The point set to visualize.
T_points_world : autolab_core.RigidTransform
Pose of points, specified as a transformation from point frame to world frame.
color : (3,) or (n,3) float
Color of whole cloud or per-point colors
scale : float
Radius of each point.
n_cuts : int
Number of longitude/latitude lines on sphere points.
subsample : int
Parameter of subsampling to display fewer points.
name : str
A name for the object to be added.
"""
if isinstance(points, BagOfPoints):
if points.dim != 3:
raise ValueError('BagOfPoints must have dimension 3xN!')
else:
if type(points) is not np.ndarray:
raise ValueError(
'Points visualizer expects BagOfPoints or numpy array!')
if len(points.shape) == 1:
points = points[:, np.newaxis].T
if len(points.shape) != 2 or points.shape[1] != 3:
raise ValueError(
'Numpy array of points must have dimension (N,3)')
frame = 'points'
if T_points_world:
frame = T_points_world.from_frame
points = PointCloud(points.T, frame=frame)
color = np.array(color)
if subsample is not None:
num_points = points.num_points
points, inds = points.subsample(subsample, random=random)
if color.shape[0] == num_points and color.shape[1] == 3:
color = color[inds, :]
# transform into world frame
if points.frame != 'world':
if T_points_world is None:
T_points_world = RigidTransform(from_frame=points.frame,
to_frame='world')
points_world = T_points_world * points
else:
points_world = points
point_data = points_world.data
if len(point_data.shape) == 1:
point_data = point_data[:, np.newaxis]
point_data = point_data.T
mpcolor = color
if len(color.shape) > 1:
mpcolor = color[0]
mp = MaterialProperties(color=np.array(mpcolor),
k_a=0.5,
k_d=0.3,
k_s=0.0,
alpha=10.0,
smooth=True)
# For each point, create a sphere of the specified color and size.
sphere = trimesh.creation.uv_sphere(scale, [n_cuts, n_cuts])
raw_pose_data = np.tile(np.eye(4), (points.num_points, 1))
raw_pose_data[3::4, :3] = points.data.T
instcolor = None
if color.shape[0] == points.num_points and color.shape[1] == 3:
instcolor = color
obj = InstancedSceneObject(sphere,
raw_pose_data=raw_pose_data,
colors=instcolor,
material=mp)
if name is None:
name = str(uuid.uuid4())
Visualizer3D._scene.add_object(name, obj)
@staticmethod
def mesh(mesh,
T_mesh_world=RigidTransform(from_frame='obj', to_frame='world'),
style='surface',
smooth=False,
color=(0.5, 0.5, 0.5),
name=None):
"""Visualize a 3D triangular mesh.
Parameters
----------
mesh : trimesh.Trimesh
The mesh to visualize.
T_mesh_world : autolab_core.RigidTransform
The pose of the mesh, specified as a transformation from mesh frame to world frame.
style : str
Triangular mesh style, either 'surface' or 'wireframe'.
smooth : bool
If true, the mesh is smoothed before rendering.
color : 3-tuple
Color tuple.
name : str
A name for the object to be added.
"""
if not isinstance(mesh, trimesh.Trimesh):
raise ValueError('Must provide a trimesh.Trimesh object')
mp = MaterialProperties(color=np.array(color),
k_a=0.5,
k_d=0.3,
k_s=0.1,
alpha=10.0,
smooth=smooth,
wireframe=(style == 'wireframe'))
obj = SceneObject(mesh, T_mesh_world, mp)
if name is None:
name = str(uuid.uuid4())
Visualizer3D._scene.add_object(name, obj)
@staticmethod
def mesh_stable_pose(mesh,
T_obj_table,
T_table_world=RigidTransform(from_frame='table',
to_frame='world'),
style='wireframe',
smooth=False,
color=(0.5, 0.5, 0.5),
dim=0.15,
plot_table=True,
plot_com=False,
name=None):
"""Visualize a mesh in a stable pose.
Parameters
----------
mesh : trimesh.Trimesh
The mesh to visualize.
T_obj_table : autolab_core.RigidTransform
Pose of object relative to table.
T_table_world : autolab_core.RigidTransform
Pose of table relative to world.
style : str
Triangular mesh style, either 'surface' or 'wireframe'.
smooth : bool
If true, the mesh is smoothed before rendering.
color : 3-tuple
Color tuple.
dim : float
The side-length for the table.
plot_table : bool
If true, a table is visualized as well.
plot_com : bool
If true, a ball is visualized at the object's center of mass.
name : str
A name for the object to be added.
Returns
-------
autolab_core.RigidTransform
The pose of the mesh in world frame.
"""
T_obj_table = T_obj_table.as_frames('obj', 'table')
T_obj_world = T_table_world * T_obj_table
Visualizer3D.mesh(mesh,
T_obj_world,
style=style,
smooth=smooth,
color=color,
name=name)
if plot_table:
Visualizer3D.table(T_table_world, dim=dim)
if plot_com:
Visualizer3D.points(Point(np.array(mesh.center_mass), 'obj'),
T_obj_world,
scale=0.01)
return T_obj_world
@staticmethod
def pose(T_frame_world, alpha=0.1, tube_radius=0.005, center_scale=0.01):
"""Plot a 3D pose as a set of axes (x red, y green, z blue).
Parameters
----------
T_frame_world : autolab_core.RigidTransform
The pose relative to world coordinates.
alpha : float
Length of plotted x,y,z axes.
tube_radius : float
Radius of plotted x,y,z axes.
center_scale : float
Radius of the pose's origin ball.
"""
R = T_frame_world.rotation
t = T_frame_world.translation
x_axis_tf = np.array([t, t + alpha * R[:, 0]])
y_axis_tf = np.array([t, t + alpha * R[:, 1]])
z_axis_tf = np.array([t, t + alpha * R[:, 2]])
Visualizer3D.points(t, color=(1, 1, 1), scale=center_scale)
Visualizer3D.plot3d(x_axis_tf,
color=(1, 0, 0),
tube_radius=tube_radius)
Visualizer3D.plot3d(y_axis_tf,
color=(0, 1, 0),
tube_radius=tube_radius)
Visualizer3D.plot3d(z_axis_tf,
color=(0, 0, 1),
tube_radius=tube_radius)
@staticmethod
def table(
T_table_world=RigidTransform(from_frame='table', to_frame='world'),
dim=0.16,
color=(0, 0, 0)):
"""Plot a table mesh in 3D.
Parameters
----------
T_table_world : autolab_core.RigidTransform
Pose of table relative to world.
dim : float
The side-length for the table.
color : 3-tuple
Color tuple.
"""
table_vertices = np.array([[dim, dim, 0], [dim, -dim,
0], [-dim, dim, 0],
[-dim, -dim, 0]]).astype('float')
table_tris = np.array([[0, 1, 2], [1, 2, 3]])
table_mesh = trimesh.Trimesh(table_vertices, table_tris)
table_mesh.apply_transform(T_table_world.matrix)
Visualizer3D.mesh(table_mesh,
style='surface',
smooth=True,
color=color)
@staticmethod
def plot3d(points,
color=(0.5, 0.5, 0.5),
tube_radius=0.005,
n_components=30,
name=None):
"""Plot a 3d curve through a set of points using tubes.
Parameters
----------
points : (n,3) float
A series of 3D points that define a curve in space.
color : (3,) float
The color of the tube.
tube_radius : float
Radius of tube representing curve.
n_components : int
The number of edges in each polygon representing the tube.
name : str
A name for the object to be added.
"""
points = np.asanyarray(points)
mp = MaterialProperties(color=np.array(color),
k_a=0.5,
k_d=0.3,
k_s=0.0,
alpha=10.0,
smooth=True)
# Generate circular polygon
vec = np.array([0, 1]) * tube_radius
angle = np.pi * 2.0 / n_components
rotmat = np.array([[np.cos(angle), -np.sin(angle)],
[np.sin(angle), np.cos(angle)]])
perim = []
for i in range(n_components):
perim.append(vec)
vec = np.dot(rotmat, vec)
poly = Polygon(perim)
# Sweep it out along the path
mesh = trimesh.creation.sweep_polygon(poly, points)
obj = SceneObject(mesh, material=mp)
if name is None:
name = str(uuid.uuid4())
Visualizer3D._scene.add_object(name, obj)
import numpy as np import trimesh from meshrender import Scene, MaterialProperties, AmbientLight, PointLight, SceneObject, VirtualCamera # Start with an empty scene scene = Scene() #create trimesh verts = np.zeros((3, 3)) verts[0] = np.array([1, 0, 0]) verts[1] = np.array([0, 1, 0]) verts[2] = np.array([0, 0, 1]) faces = np.zeros((1, 3)) faces[0] = np.array([0, 1, 2]) mesh = trimesh.base.Trimesh(vertices=verts, faces=faces)
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()