def _round_and_show(self):
for shape in self.shapes:
si = self._shape_info[shape]
xi = si.xi
segset = zip(shape.segments, [x for x in range(shape.n_segments)],
xi)
while len(segset) > 0:
max_segtup = max(segset, key=lambda x: x[2])
seg = max_segtup[0]
index = max_segtup[1]
xi[index] = 1
new_segset = []
for segtup in segset:
other_seg = segtup[0]
other_index = segtup[1]
if other_seg == seg:
continue
elif shape.segs_intersect(other_seg, seg):
xi[other_index] = 0.0
else:
new_segset.append(segtup)
segset = new_segset
Visualizer3D.figure(size=(400, 400))
for i, segment in enumerate(shape.segments):
if xi[i] == 1.0:
Visualizer3D.mesh(segment.mesh,
style='surface',
color=indexed_color(i))
Visualizer3D.show()
def main(args):
# set logging
logging.getLogger().setLevel(logging.INFO)
rospy.init_node("ensenso_reader", anonymous=True)
num_frames = 10
sensor = RgbdSensorFactory("ensenso", cfg={"frame": "ensenso"})
sensor.start()
total_time = 0
for i in range(num_frames):
if i > 0:
start_time = time.time()
_, depth_im, _ = sensor.frames()
if i > 0:
total_time += time.time() - start_time
print("Frame %d" % (i))
print("Avg FPS: %.5f" % (float(i) / total_time))
depth_im = sensor.median_depth_img(num_img=5)
point_cloud = sensor.ir_intrinsics.deproject(depth_im)
point_cloud.remove_zero_points()
sensor.stop()
vis2d.figure()
vis2d.imshow(depth_im)
vis2d.title("Ensenso - Raw")
vis2d.show()
vis3d.figure()
vis3d.points(point_cloud, random=True, subsample=10, scale=0.0025)
vis3d.show()
def view_segmentation(mesh, n_segs):
seg_graph = SegmentGraph(FaceGraph(mesh))
seg_graph.cut_to_k_segments(n_segs)
seg_graph.reindex_segment_nodes()
seg_graph.refine_all_edges(0.4)
Visualizer3D.figure(size=(100, 100))
for i, seg_node in enumerate(seg_graph.segment_nodes):
segment = Segment(seg_node, mesh)
Visualizer3D.mesh(segment.mesh,
style='surface',
color=indexed_color(i))
Visualizer3D.show()
seg_map = {}
n_segs -= 1
while n_segs > 2:
n_segs -= 1
seg_graph.cut_to_k_segments(n_segs)
for seg_node in seg_graph.segment_nodes:
if (frozenset(seg_node.segment_indices) not in seg_map):
segment = Segment(seg_node, mesh)
seg_map[frozenset(seg_node.segment_indices)] = segment
for i, segment in enumerate(
sorted(seg_map.values(), key=lambda x: -x.cut_cost / x.perimeter)):
print segment.cut_cost / segment.perimeter
Visualizer3D.mesh(
segment.mesh,
style='surface',
color=indexed_color(i),
T_mesh_world=RigidTransform(translation=[0.15, 0, 0]))
Visualizer3D.show()
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)
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
for sh in rotations(shadow, 8):
scores[i][j] = do_stuff(pc, indices, model, sh, img_file)
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))
def show_points(points, color=(0,1,0), scale=0.005, frame_size=0.2, frame_radius=0.02):
vis.figure(bgcolor=(1,1,1), size=(500,500))
vis.points(np.array(points), color=color, scale=scale)
vis.plot3d(np.array(([0, 0, 0], [frame_size, 0, 0])).astype(np.float32), color=(1,0,0), tube_radius=frame_radius)
vis.plot3d(np.array(([0, 0, 0], [0, frame_size, 0])).astype(np.float32), color=(0,1,0), tube_radius=frame_radius)
vis.plot3d(np.array(([0, 0, 0], [0, 0, frame_size])).astype(np.float32), color=(0,0,1), tube_radius=frame_radius)
vis.show()
def visualize_alignment(self, T_obj_camera_est):
mesh = self.salient_edge_set.mesh
m_true = mesh.copy().apply_transform(self.T_obj_camera.matrix)
m_est = mesh.copy().apply_transform(T_obj_camera_est.matrix)
vis3d.figure()
vis3d.mesh(m_true, color=(0.0, 1.0, 0.0))
vis3d.mesh(m_est, color=(0.0, 0.0, 1.0))
vis3d.show()
def visualize(self):
"""Visualize the salient edges of the mesh.
"""
vis3d.figure()
for edge in self.salient_edges:
vis3d.plot3d(self.mesh.vertices[edge],
color=(0.0, 1.0, 0.0),
tube_radius=0.0005)
vis3d.mesh(self.mesh)
vis3d.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 vis(self, key):
"""Show all the models for a given Thing.
Parameters
----------
key : str
The key for the target Thing.
"""
for model in self[key].models:
vis.figure()
vis.mesh(model.mesh, style='surface')
vis.show()
def main():
# initialize logging
logging.getLogger().setLevel(31)
parser = argparse.ArgumentParser(
description='Annotate Thingiverse Dataset Models',
epilog='Written by Matthew Matl (mmatl)')
parser.add_argument('--config',
help='config filename',
default='cfg/tools/annotater.yaml')
args = parser.parse_args()
config_filename = args.config
config = YamlConfig(config_filename)
target_key = config['target_key']
default_value = config['default_value']
set_value = config['set_value']
override = config['override']
ds = ThingiverseDataset(config['dataset_dir'])
for i, thing_id in enumerate(ds.keys):
thing = None
thing_metadata = ds.metadata(thing_id)
for model_id in thing_metadata['models']:
model_data = thing_metadata['models'][model_id]
if override or target_key not in model_data['metadata']:
if thing is None:
thing = ds[thing_id]
model = thing[model_id]
logging.log(
31, u"{} ({}): {} ({})".format(thing.name, thing.id,
model.name,
model.id).encode('utf-8'))
model.metadata[target_key] = default_value
vis.figure()
vis.mesh(model.mesh, style='surface')
vis.show(animate=True,
registered_keys={
'g': (good_label_callback,
[model, target_key, set_value])
})
if thing:
ds.save(thing, only_metadata=True)
logging.log(31, '{}/{} things...'.format(i, len(ds.keys)))
def main():
logging.getLogger().setLevel(logging.INFO)
# parse args
parser = argparse.ArgumentParser(description='Register a webcam to the Photoneo PhoXi')
parser.add_argument('--config_filename', type=str, default='cfg/tools/colorize_phoxi.yaml', help='filename of a YAML configuration for registration')
args = parser.parse_args()
config_filename = args.config_filename
config = YamlConfig(config_filename)
sensor_data = config['sensors']
phoxi_config = sensor_data['phoxi']
phoxi_config['frame'] = 'phoxi'
# Initialize ROS node
rospy.init_node('colorize_phoxi', anonymous=True)
logging.getLogger().addHandler(rl.RosStreamHandler())
# Get PhoXi sensor set up
phoxi = RgbdSensorFactory.sensor(phoxi_config['type'], phoxi_config)
phoxi.start()
# Capture PhoXi and webcam images
phoxi_color_im, phoxi_depth_im, _ = phoxi.frames()
# vis2d.figure()
# vis2d.subplot(121)
# vis2d.imshow(phoxi_color_im)
# vis2d.subplot(122)
# vis2d.imshow(phoxi_depth_im)
# vis2d.show()
phoxi_pc = phoxi.ir_intrinsics.deproject(phoxi_depth_im)
colors = phoxi_color_im.data.reshape((phoxi_color_im.shape[0] * phoxi_color_im.shape[1], phoxi_color_im.shape[2])) / 255.0
vis3d.figure()
vis3d.points(phoxi_pc.data.T[::3], color=colors[::3], scale=0.001)
vis3d.show()
# Export to PLY file
vertices = phoxi.ir_intrinsics.deproject(phoxi_depth_im).data.T
colors = phoxi_color_im.data.reshape(phoxi_color_im.data.shape[0] * phoxi_color_im.data.shape[1], phoxi_color_im.data.shape[2])
f = open('pcloud.ply', 'w')
f.write('ply\nformat ascii 1.0\nelement vertex {}\nproperty float x\nproperty float y\nproperty float z\nproperty uchar red\n'.format(len(vertices)) +
'property uchar green\nproperty uchar blue\nend_header\n')
for v, c in zip(vertices,colors):
f.write('{} {} {} {} {} {}\n'.format(v[0], v[1], v[2], c[0], c[1], c[2]))
f.close()
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 main():
start_time = time.time()
img_file = '/nfs/diskstation/projects/dex-net/placing/datasets/real/sample_ims_05_22/depth_ims_numpy/image_000001.npy'
ci_file = '/nfs/diskstation/projects/dex-net/placing/datasets/real/sample_ims_05_22/camera_intrinsics.intr'
mesh_file = 'demon_helmet.obj'
indices, model, image, pc = largest_planar_surface(img_file, ci_file)
mesh, best_pose, rt = find_stable_poses(mesh_file)
shadow = find_shadow(mesh, best_pose, model[0:3])
vis3d.figure()
vis3d.points(pc, color=(1, 0, 0))
vis3d.mesh(shadow, rt)
vis3d.show()
scores, split_size = score_cells(pc, indices, model, shadow, ci_file)
ind = best_cell(scores)
# print("Scores: \n" + str(scores))
# print("\nBest cell = " + str(ind))
print("--- %s seconds ---" % (time.time() - start_time))
def main(args):
# set logging
logging.getLogger().setLevel(logging.INFO)
rospy.init_node('ensenso_reader', anonymous=True)
num_frames = 10
#sensor = PhoXiSensor(frame='phoxi',
# size='small')
sensor = EnsensoSensor(frame='ensenso')
sensor.start()
total_time = 0
for i in range(num_frames):
if i > 0:
start_time = time.time()
_, depth_im, _ = sensor.frames()
if i > 0:
total_time += time.time() - start_time
print('Frame %d' % (i))
print('Avg FPS: %.5f' % (float(i) / total_time))
depth_im = sensor.median_depth_img(num_img=5)
point_cloud = sensor.ir_intrinsics.deproject(depth_im)
point_cloud.remove_zero_points()
sensor.stop()
vis2d.figure()
vis2d.imshow(depth_im)
vis2d.title('PhoXi - Raw')
vis2d.show()
vis3d.figure()
vis3d.points(point_cloud, random=True, subsample=10, scale=0.0025)
vis3d.show()
normals = mesh.vertex_normals
of = ObjFile(MESH_FILENAME)
mesh = of.read()
ar_tag = lookup_tag(TAG)
# find the transformation from the object coordinates to world coordinates... somehow
grasp_indices, best_metric_indices = sorted_contacts(
vertices, normals, T_ar_object)
for indices in best_metric_indices[0:5]:
a = grasp_indices[indices][0]
b = grasp_indices[indices][1]
normal1, normal2 = normals[a], normals[b]
contact1, contact2 = vertices[a], vertices[b]
# visualize the mesh and contacts
vis.figure()
vis.mesh(mesh)
vis.normals(
NormalCloud(np.hstack(
(normal1.reshape(-1, 1), normal2.reshape(-1, 1))),
frame='test'),
PointCloud(np.hstack(
(contact1.reshape(-1, 1), contact2.reshape(-1, 1))),
frame='test'))
# vis.pose(T_obj_gripper, alpha=0.05)
vis.show()
if BAXTER_CONNECTED:
repeat = True
while repeat:
# come in from the top...
T_obj_gripper = contacts_to_baxter_hand_pose(
def main():
# initialize logging
logging.getLogger().setLevel(31)
# parse args
parser = argparse.ArgumentParser(
description='Annotate Thingiverse Dataset Models',
epilog='Written by Matthew Matl (mmatl)')
parser.add_argument('--config',
help='config filename',
default='cfg/tools/rescaler.yaml')
args = parser.parse_args()
# read config
config_filename = args.config
config = YamlConfig(config_filename)
# get gripper mesh
gripper_filename = config['gripper_filename']
gripper_mesh = trimesh.load_mesh(gripper_filename)
# get metadata information
identifier_key = config['identifier_key']
identifier_value = config['identifier_value']
scale_key = config['scale_key']
default_scale = config['default_scale']
override = config['override']
ds = ThingiverseDataset(config['dataset_dir'])
for i, thing_id in enumerate(ds.keys):
thing = None
thing_metadata = ds.metadata(thing_id)
changed_model_keys = []
for model_id in thing_metadata['models']:
model_data = thing_metadata['models'][model_id]
# If the identifier isn't in the model's metadata, skip it
if identifier_key not in model_data['metadata'] or model_data[
'metadata'][identifier_key] != identifier_value:
continue
# If we're overriding or the scale key hasn't been set, modify the model
if override or scale_key not in model_data['metadata']:
# Load the model
if thing is None:
thing = ds[thing_id]
model = thing[model_id]
logging.log(
31, u"{} ({}): {} ({})".format(thing.name, thing.id,
model.name,
model.id).encode('utf-8'))
changed_model_keys.append(model.id)
# Rescale back to original dimensions if overriding
if scale_key in model.metadata:
model.mesh.apply_scale(1.0 / model.metadata[scale_key])
model.metadata[scale_key] = default_scale
# Visualize the model, registering the grow/shrink callbacks
stf = SimilarityTransform(from_frame='world', to_frame='world')
rot = RigidTransform(from_frame='world', to_frame='world')
registered_keys = {
'j': (rescale_callback, ['model', rot, stf, 0.1]),
'k': (rescale_callback, ['model', rot, stf, -0.1]),
'u': (rescale_callback, ['model', rot, stf, 1.0]),
'i': (rescale_callback, ['model', rot, stf, -1.0]),
'h': (rotate_callback, ['model', rot, stf])
}
vis.figure()
vis.mesh(gripper_mesh,
T_mesh_world=RigidTransform(translation=(0, 0, -0.08),
from_frame='obj',
to_frame='world'),
style='surface',
color=(0.3, 0.3, 0.3),
name='gripper')
vis.mesh(model.mesh, style='surface', name='model')
vis.show(animate=True, registered_keys=registered_keys)
# Transform the model and update its metadata
model.mesh.apply_transform(stf.matrix)
model.metadata[scale_key] = stf.scale
if thing:
ds.save(thing, only_metadata=False, model_keys=changed_model_keys)
logging.log(31, '{}/{} things...'.format(i, len(ds.keys)))
fa.goto_pose_with_cartesian_control(
T_tool_world, cartesian_impedances=[2000, 2000, 1000, 300, 300, 300])
logging.info('Finding closest orthogonal grasp')
T_grasp_world = get_closest_grasp_pose(T_tag_world, T_ready_world)
T_lift = RigidTransform(translation=[0, 0, 0.2],
from_frame=T_ready_world.to_frame,
to_frame=T_ready_world.to_frame)
T_lift_world = T_lift * T_grasp_world
logging.info('Visualizing poses')
_, depth_im, _ = sensor.frames()
points_world = T_camera_world * intr.deproject(depth_im)
if cfg['vis_detect']:
vis3d.figure()
vis3d.pose(RigidTransform())
vis3d.points(subsample(points_world.data.T, 0.01),
color=(0, 1, 0),
scale=0.002)
vis3d.pose(T_ready_world, length=0.05)
vis3d.pose(T_camera_world, length=0.1)
vis3d.pose(T_tag_world)
vis3d.pose(T_grasp_world)
vis3d.pose(T_lift_world)
vis3d.show()
#const_rotation=np.array([[1,0,0],[0,-1,0],[0,0,-1]])
#test = RigidTransform(rotation=const_rotation,translation=T_tag_world.translation, from_frame='franka_tool', to_frame='world')
#import pdb; pdb.set_trace()
output_filename = os.path.join(
output_path, '{0}_to_world.tf'.format(T_world_obj.from_frame))
print T_world_obj
T_world_obj.save(output_filename)
if config['vis'] and VIS_SUPPORTED:
_, depth_im, _ = sensor.frames()
pc_cam = ir_intrinsics.deproject(depth_im)
pc_world = T_world_cam * pc_cam
mesh_file = ObjFile(
os.path.join(object_path, '{0}.obj'.format(args.object_name)))
mesh = mesh_file.read()
vis.figure(bgcolor=(0.7, 0.7, 0.7))
vis.mesh(mesh, T_world_obj.as_frames('obj', 'world'), style='surface')
vis.pose(T_world_obj, alpha=0.04, tube_radius=0.002, center_scale=0.01)
vis.pose(RigidTransform(from_frame='origin'),
alpha=0.04,
tube_radius=0.002,
center_scale=0.01)
vis.pose(T_world_cam, alpha=0.04, tube_radius=0.002, center_scale=0.01)
vis.pose(T_world_cam * T_cb_cam.inverse(),
alpha=0.04,
tube_radius=0.002,
center_scale=0.01)
vis.points(pc_world, subsample=20)
vis.show()
sensor.stop()
def visualize(pc, indices, color=(1, 0, 0)):
vis3d.figure()
pc_plane = pc.data.T[indices]
pc_plane = pc_plane[np.where(pc_plane[::, 1] < 0.16)]
vis3d.points(pc_plane, color=(1, 0, 0))
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()
clip_start = time.time()
low_indices = np.where(point_cloud_world.data[2, :] < min_height)[0]
point_cloud_filtered.data[2, low_indices] = min_height
# filter high
high_indices = np.where(point_cloud_world.data[2, :] > max_height)[0]
point_cloud_filtered.data[2, high_indices] = max_height
# re-project and update depth im
logging.info("Clipping took %.3f sec" % (time.time() - clip_start))
# vis
focal_point = np.mean(point_cloud_filtered.data, axis=1)
if vis_clipping:
vis3d.figure(
camera_pose=T_camera_world.as_frames("camera", "world"),
focal_point=focal_point,
)
vis3d.points(
point_cloud_world, scale=0.001, color=(1, 0, 0), subsample=10
)
vis3d.points(
point_cloud_filtered, scale=0.001, color=(0, 0, 1), subsample=10
)
vis3d.show()
pcl_start = time.time()
# subsample point cloud
# rate = int(1.0 / rescale_factor)**2
# point_cloud_filtered = point_cloud_filtered.subsample(rate, random=False)
box = Box(
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()
vertices = mesh.vertices
triangles = mesh.triangles
normals = mesh.normals
print 'Num vertices:', len(vertices)
print 'Num triangles:', len(triangles)
print 'Num normals:', len(normals)
# 1. Generate candidate pairs of contact points
# 2. Check for force closure
# 3. Convert each grasp to a hand pose
contact1 = vertices[500]
contact2 = vertices[2000]
T_obj_gripper = contacts_to_baxter_hand_pose(contact1, contact2)
print 'Translation', T_obj_gripper.translation
print 'Rotation', T_obj_gripper.quaternion
pose_msg = T_obj_gripper.pose_msg
# 3d visualization to help debug
vis.figure()
vis.mesh(mesh)
vis.points(Point(contact1, frame='test'))
vis.points(Point(contact2, frame='test'))
vis.pose(T_obj_gripper, alpha=0.05)
vis.show()
# 4. Execute on the actual robot
clip_start = time.time()
low_indices = np.where(point_cloud_world.data[2, :] < min_height)[0]
point_cloud_filtered.data[2, low_indices] = min_height
# filter high
high_indices = np.where(point_cloud_world.data[2, :] > max_height)[0]
point_cloud_filtered.data[2, high_indices] = max_height
# re-project and update depth im
#depth_im_filtered = camera_intr.project_to_image(T_camera_world.inverse() * point_cloud_filtered)
logging.info('Clipping took %.3f sec' % (time.time() - clip_start))
# vis
focal_point = np.mean(point_cloud_filtered.data, axis=1)
if vis_clipping:
vis3d.figure(camera_pose=T_camera_world.as_frames('camera', 'world'),
focal_point=focal_point)
vis3d.points(point_cloud_world,
scale=0.001,
color=(1, 0, 0),
subsample=10)
vis3d.points(point_cloud_filtered,
scale=0.001,
color=(0, 0, 1),
subsample=10)
vis3d.show()
pcl_start = time.time()
# subsample point cloud
#rate = int(1.0 / rescale_factor)**2
#point_cloud_filtered = point_cloud_filtered.subsample(rate, random=False)
def quality(self, state, actions, params=None):
"""Given a suction point, compute a score based on a best-fit 3D plane of the neighboring points.
Parameters
----------
state : :obj:`RgbdImageState`
An RgbdImageState instance that encapsulates rgbd_im, camera_intr, segmask, full_observed.
action: :obj:`SuctionPoint2D`
A suction grasp in image space that encapsulates center, approach direction, depth, camera_intr.
params: dict
Stores params used in computing suction quality.
Returns
-------
:obj:`numpy.ndarray`
Array of the quality for each grasp
"""
qualities = []
# deproject points
point_cloud_image = state.camera_intr.deproject_to_image(
state.rgbd_im.depth)
# compute negative SSE from the best fit plane for each grasp
for i, action in enumerate(actions):
if not isinstance(action, SuctionPoint2D):
raise ValueError(
'This function can only be used to evaluate suction quality'
)
points = self._points_in_window(
point_cloud_image, action,
segmask=state.segmask) # x,y in matrix A and z is vector z.
A, b = self._points_to_matrices(points)
w = self._action_to_plane(
point_cloud_image,
action) # vector w w/ a bias term represents a best-fit plane.
sse = self._sum_of_squared_residuals(w, A, b)
if params is not None and params['vis']['plane']:
from visualization import Visualizer2D as vis2d
from visualization import Visualizer3D as vis3d
mid_i = A.shape[0] / 2
pred_z = A.dot(w)
p0 = np.array([A[mid_i, 0], A[mid_i, 1], pred_z[mid_i]])
n = np.array([w[0], w[1], -1])
n = n / np.linalg.norm(n)
tx = np.array([n[1], -n[0], 0])
tx = tx / np.linalg.norm(tx)
ty = np.cross(n, tx)
R = np.array([tx, ty, n]).T
c = state.camera_intr.deproject_pixel(action.depth,
action.center)
d = Point(c.data - 0.01 * action.axis, frame=c.frame)
T_table_world = RigidTransform(rotation=R,
translation=p0,
from_frame='patch',
to_frame='world')
vis3d.figure()
vis3d.points(point_cloud_image.to_point_cloud(),
scale=0.0025,
subsample=10,
random=True,
color=(0, 0, 1))
vis3d.points(PointCloud(points.T),
scale=0.0025,
color=(1, 0, 0))
vis3d.points(c, scale=0.005, color=(1, 1, 0))
vis3d.points(d, scale=0.005, color=(1, 1, 0))
vis3d.table(T_table_world, dim=0.01)
vis3d.show()
vis2d.figure()
vis2d.imshow(state.rgbd_im.depth)
vis2d.scatter(action.center.x, action.center.y, s=50, c='b')
vis2d.show()
quality = np.exp(
-sse
) # evaluate how well best-fit plane describles all points in window.
qualities.append(quality)
return np.array(qualities)
