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':
