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)