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])