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
pose_array = conversions.array_to_pose_array(good_xyzs.reshape(-1,3), "base_footprint")
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
