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
Exemplo n.º 3
0
                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':