print ""
print "assumed top and bottom space is:", top_bottom_space
print "assumed left and right space is:", left_right_space
combo = read_pcd_file(args[1], ['xyzrgb', 'xyz', 'xy', 'yz', 'xz', 'x', 'y', 'z'])
xyzrgb, xyz, xy, yz, xz, x, y, z = combo
if render:
render_3d_scatter(xyzrgb, proportion=0.1).set_title("original cloud")
print ""
#=================back======================
ret = sw_back.remove_back(xyz, xy, x, y, z)
min_z, min_dist, sampled_z, all_dists = ret
if min_dist < plane_removal_threshold:
remove_idxs = remove_plane_idx(xyz, min_z, 'xy')
combo = select_each(combo, remove_idxs)
xyzrgb, xyz, xy, yz, xz, x, y, z = combo
print "Back removed at", min_z
print "removing points behind back plane"
_, in_front_idxs = filter_val_idx(lambda z: z<min_z, z)
combo = select_each(combo, in_front_idxs)
xyzrgb, xyz, xy, yz, xz, x, y, z = combo
else:
print "No back detected"
#plt.plot(sampled_z, all_dists)
''' x_image = x/z*lift + width/2 y_image = y/z*lift + height/2 if not 0<=x_image<=width: print "x!", x, y, z, x_image if not 0<=y_image<=height: print "y!", x, y, z, y_image return x_image, y_image img = cv2.imread(sys.argv[1]) hsv = cv2.cvtColor(img, cv2.cv.CV_BGR2HSV) hue = hsv[:,:,0] xyzrgb, xyz, xy, x, y, z = read_pcd_file(sys.argv[2], ['xyzrgb', 'xyz', 'xy', 'x', 'y', 'z']) min_z, min_dist, zs, dists = sw_back.remove_back(xyz, xy, x, y, z, num_windows=10) _, tmp_points_idx = filter_val_idx(lambda z:abs(z-min_z)<0.03, z) xyzrgb_tmp = select(xyzrgb, tmp_points_idx) xyz_tmp = select(xyz, tmp_points_idx) xy = select(xy, tmp_points_idx) x = select(x, tmp_points_idx) y = select(y, tmp_points_idx) z = select(z, tmp_points_idx) a, b, c, d = fit_plane(xyz_tmp) _, all_points_idx = filter_val_idx(lambda p:dist_point_plane(p, a, b, c, d)<0.03, xyz) xyzrgb_final = select(xyzrgb, all_points_idx) eligible_points = [] for p in xyzrgb_final: r, g, b = pcl_float_to_rgb(p[3])