def filter_pc2(cloud_pc2): cloud_xyz = (pc2xyzrgb(cloud_pc2)[0]).reshape(-1,3) cloud_xyz_down = voxel_downsample(cloud_xyz, .02) graph = ri.points_to_graph(cloud_xyz_down, .03) cc = ri.largest_connected_component(graph) good_xyzs = np.array([graph.node[node_id]["xyz"] for node_id in cc.nodes()]) pose_array = juc.array_to_pose_array(good_xyzs, "base_footprint") Globals.handles.append(Globals.rviz.draw_curve(pose_array, rgba = (1,1,0,1), type=Marker.CUBE_LIST, width=.001, ns="segmentation")) raw_input("press enter when done looking") del Globals.handles[:] return xyz2pc(good_xyzs, cloud_pc2.header.frame_id)
def get_good_inds(orig_xyz, good_xyz): good_inds = [] for i, xyz in enumerate(orig_xyz): if array_contains(good_xyzs, xyz): good_inds.append(i) return good_inds for object_name in object_names: pc_sel = seg_svc.call(ProcessCloudRequest(cloud_in = pc)).cloud_out xyz, rgb = ros_utils.pc2xyzrgb(pc_sel) xyz = xyz.reshape(-1,3) rgb = rgb.reshape(-1,3) if args.do_filtering: xyz_down = voxel_downsample(xyz, .02) graph = ri.points_to_graph(xyz_down, .03) cc = ri.largest_connected_component(graph) good_xyzs = np.array([graph.node[node_id]["xyz"] for node_id in cc.nodes()]) good_inds = get_good_inds(xyz, good_xyzs) good_rgbs = rgb[good_inds] else: good_xyzs, good_rgbs = xyz, rgb outfile.create_group(object_name) outfile[object_name]["xyz"] = good_xyzs #outfile[object_name]["rgb"] = good_rgbs if args.plotting: rviz = ros_utils.RvizWrapper.create() rospy.sleep(.5) from brett2.ros_utils import Marker
def get_good_inds(orig_xyz, good_xyz): good_inds = [] for i, xyz in enumerate(orig_xyz): if array_contains(good_xyzs, xyz): good_inds.append(i) return good_inds for object_name in object_names: pc_sel = seg_svc.call(ProcessCloudRequest(cloud_in=pc)).cloud_out xyz, rgb = ros_utils.pc2xyzrgb(pc_sel) xyz = xyz.reshape(-1, 3) rgb = rgb.reshape(-1, 3) if args.do_filtering: xyz_down = voxel_downsample(xyz, .02) graph = ri.points_to_graph(xyz_down, .03) cc = ri.largest_connected_component(graph) good_xyzs = np.array( [graph.node[node_id]["xyz"] for node_id in cc.nodes()]) good_inds = get_good_inds(xyz, good_xyzs) good_rgbs = rgb[good_inds] else: good_xyzs, good_rgbs = xyz, rgb outfile.create_group(object_name) outfile[object_name]["xyz"] = good_xyzs #outfile[object_name]["rgb"] = good_rgbs if args.plotting: rviz = ros_utils.RvizWrapper.create() rospy.sleep(.5)