def extract_table_pca(rgb, depth, T_w_k): xyz = extract_green(rgb, depth, T_w_k) n_rows, n_cols = xyz.shape n_samples = min(n_rows, 1000) sample_indices = random.sample(xrange(n_rows), n_samples) sampled_xyz = xyz[sample_indices, :] #import IPython #IPython.embed() normal = clouds.clouds_plane(sampled_xyz) center_xyz = np.median(sampled_xyz, axis=0) centered_sampled_xyz = sampled_xyz - center_xyz[None,:] mean_t = np.mean(centered_sampled_xyz.dot(normal)) a = normal[0] b = normal[1] c = normal[2] d = -center_xyz.dot(normal) - mean_t xx, yy, zz = plot_plane(a, b, c, d) return (xx, yy, zz), (a, b, c, d)
if demo_name in perturb_demofile.keys(): demo_group = perturb_demofile[demo_name] else: demo_group = perturb_demofile.create_group(demo_name) n_perturb_existed = len(demo_group.keys()) # number of perturbations object_xyz = cloud_proc_func(rgb_imgs[0], depth_imgs[0], np.eye(4)) object_xyz = clouds.downsample(object_xyz, .01) hitch_xyz = None hitch_pos = None if args.has_hitch: hitch_normal = clouds.clouds_plane(object_xyz) hitch_xyz, hitch_pos = hitch_proc_func(rgb_imgs[0], depth_imgs[0], np.eye(4), hitch_normal) hitch_xyz = clouds.downsample(hitch_xyz, .01) xyz = np.r_[object_xyz, hitch_xyz] else: xyz = object_xyz mlab.figure(0) mlab.clf() mlab.points3d(xyz[:,0], xyz[:,1], xyz[:,2], color=(1,0,0), scale_factor=.005) if yes_or_no("Do you want to add this original demo?"): perturb_name = str(n_perturb_existed) perturb_group = demo_group.create_group(perturb_name) perturb_group['cloud_xyz'] = xyz
elif q != 'c': continue print "gen point clouds: %s %s"%(demo_name, seg_name) cloud = cloud_proc_func(rgb_image, np.asarray(seg_info["depth"]), np.eye(4)) if args.prompt and args.visualize: # mlab.figure(0) # mlab.clf() # mlab.points3d(xyz[:,0], xyz[:,1], xyz[:,2], color=(0,0,1), scale_factor=.005) xyz2 = cloud if args.has_hitch: if not hitch_found: hitch_normal = clouds.clouds_plane(cloud) hitch, hitch_pos = hitch_proc_func(rgb_image, np.asarray(seg_info["depth"]), np.eye(4), hitch_normal) hitch_found = True xyz2 = np.r_[xyz2, hitch] fig.clf() ax = fig.gca(projection='3d') ax.set_autoscale_on(False) xyzm2 = np.mean(xyz2, axis=0) ax.plot(xyz2[:,0]-xyzm2[0], xyz2[:,1]-xyzm2[1]+1.0, xyz2[:,2]-xyzm2[2], 'o') #ax.plot(xyz2[:,0], xyz2[:,1], xyz2[:,2], 'o') fig.show() print demo_name, seg_name print "Before", xyz.shape print "After", xyz2.shape q = raw_input("Again: Hit c to change the pc. q to quit") if q == 'q':