perturb_name = str(n_perturb_existed)
perturb_group = demo_group.create_group(perturb_name)
perturb_group['cloud_xyz'] = xyz
if has_hitch:
perturb_group['hitch_pos'] = np.squeeze(hitch_pos)
print "add perturb demo %d"%(n_perturb_existed)
n_perturb_existed += 1
# start actual perturbation
n_perturbed = 0
n_perturbed_attempt = 0
while n_perturbed < args.perturb_num and n_perturbed_attempt < args.max_perturb_attempt:
if has_hitch:
new_object_xyz = sample_random_rope(demofile[demo_name]['seg00']['object'], args.interactive)
new_object_xyz = clouds.downsample(new_object_xyz, .01)
new_xyz = np.r_[new_object_xyz, demofile[demo_name]['seg00']['hitch']]
else:
new_xyz = sample_random_rope(xyz, args.interactive)
new_xyz = clouds.downsample(new_xyz, .01)
if args.interactive:
mlab.figure(0)
mlab.clf()
mlab.points3d(new_xyz[:,0], new_xyz[:,1], new_xyz[:,2], color=(1,0,0), scale_factor=.005)
if yes_or_no("Are you happy with this perturbation?"):
perturb_name = str(n_perturb_existed)
perturb_group = demo_group.create_group(perturb_name)
pass
if yes_or_no("Do you want to skip perturbing this demo?"):
print "skip perturbing demo"
continue
# start actual perturbation
n_perturbed = 0
n_perturbed_attempt = 0
while n_perturbed <= args.perturb_num and n_perturbed_attempt < args.max_perturb_attempt:
if args.has_hitch:
new_object_xyz = sample_random_rope(object_xyz, True)
new_object_xyz = clouds.downsample(new_object_xyz, .01)
new_xyz = np.r_[new_object_xyz, hitch_xyz]
else:
new_xyz = sample_random_rope(object_xyz, True)
new_xyz = clouds.downsample(new_xyz, .01)
mlab.figure(0)
mlab.clf()
mlab.points3d(new_xyz[:,0], new_xyz[:,1], new_xyz[:,2], color=(1,0,0), scale_factor=.005)
if yes_or_no("Are you happy with this perturbation?"):
perturb_name = str(n_perturb_existed)
perturb_group = demo_group.create_group(perturb_name)
perturb_group['cloud_xyz'] = new_xyz
print "add perturb demo %d"%(n_perturb_existed)
def main(demo_type, n_base, n_perts, load_sm = False):
demofile = h5py.File(osp.join(demo_files_dir, demo_type, demo_type+'.h5'), 'r')
if load_sm:
sm_file = osp.join(hd_data_dir, similarity_costs_dir, matrix_file%demo_type)
with open(sm_file, 'r') as f: sm = pickle.load(f)
else:
sm = extract_init(demo_type)
seg_num = 0
keys = {}
for demo_name in demofile:
if demo_name != "ar_demo":
for seg_name in demofile[demo_name]:
if seg_name == 'seg00':
keys[seg_num] = (demo_name, seg_name)
#print demo_name, seg_name
seg_num += 1
n_clusters = n_base - len(BASIC_DEMOS)
labels = spectral_clustering(sm, n_clusters = n_clusters, eigen_solver='arpack',assign_labels='discretize')
names = {i:[] for i in xrange(n_clusters)}
images = {i:[] for i in xrange(n_clusters)}
demos = {i:[] for i in xrange(n_clusters)}
for i in xrange(len(labels)):
label = labels[i]
names[label].append(keys[i])
images[label].append(np.asarray(demofile[keys[i][0]][keys[i][1]]["rgb"]))
demos[label].append(i)
rows = []
i = 0
inc = True
print "Press q to exit, left/right arrow keys to navigate"
while True:
if len(images[i]) == 0:
if i == n_clusters-1: inc = False
elif i == 0: inc = True
if inc: i = min(i+1,n_clusters-1)
else: i = max(i-1,0)
continue
print "Label %i"%(i+1)
print names[i]
import math
ncols = 7
nrows = int(math.ceil(1.0*len(images[i])/ncols))
row = cpu.tile_images(images[i], nrows, ncols)
rows.append(np.asarray(row))
cv2.imshow("clustering result", row)
kb = cv2.waitKey()
if kb == 1113939 or kb == 65363:
i = min(i+1,n_clusters-1)
inc = True
elif kb == 1113937 or kb == 65361:
i = max(i-1,0)
inc = False
elif kb == 1048689 or kb == 113:
break
bests = find_best(demos, sm)
for i in BASIC_DEMOS:
if i in bests:
bests.remove(i)
# add basic demos
bests = BASIC_DEMOS+bests
print bests
best_xyz = []
best_images = {i:None for i in bests}
for best in bests:
xyz = np.asarray(demofile[keys[best][0]][keys[best][1]]["cloud_xyz"])
best_xyz.append(xyz)
best_images[best] = np.asarray(demofile[keys[best][0]][keys[best][1]]["rgb"])
print"Found %i clouds."%len(bests)
print "These are the demos being saved."
ncols = 10
nrows = int(math.ceil(1.0*len(bests)/ncols))
row = cpu.tile_images(best_images.values(), nrows, ncols)
cv2.imshow("best", row)
kb = cv2.waitKey()
if not yes_or_no("Do these look fine to you?"):
return
remaining = n_base-len(bests)
print "remaining:", remaining
while remaining > 0:
fig = pylab.figure()
xyz = best_xyz[remaining-1]
while True:
fig.clf()
perturbed_xyz = sample_random_rope(xyz, True)
ax = fig.gca(projection='3d')
ax.set_autoscale_on(False)
ax.plot(perturbed_xyz[:,0], perturbed_xyz[:,1], perturbed_xyz[:,2], 'o')
fig.show()
cv2.imshow("pert", best_images[bests[remaining-1]])
kb = cv2.waitKey()
if yes_or_no("Does this pert. look fine to you?"):
best_xyz.append(perturbed_xyz)
remaining -= 1
break
if n_perts != 0:
fig = pylab.figure()
fig2 = pylab.figure()
for i in xrange(len(best_xyz)):
xyz = best_xyz[i]
n_p = n_perts
while n_p > 0:
perturbed_xyz = sample_random_rope(xyz, True)
ax = fig.gca(projection='3d')
ax.set_autoscale_on(False)
ax.plot(perturbed_xyz[:,0], perturbed_xyz[:,1], perturbed_xyz[:,2], 'o')
fig.show()
if i < len(bests):
cv2.imshow("pert", best_images[bests[i]])
kb = cv2.waitKey()
else:
ax = fig2.gca(projection='3d')
ax.set_autoscale_on(False)
ax.plot(xyz[:,0], xyz[:,1], xyz[:,2], 'o')
fig2.show()
if not yes_or_no("Does this pert. look fine to you?"):
best_xyz.append(perturbed_xyz)
n_p -= 1
pickle.dump(best_xyz, open(osp.join(demo_files_dir, perturbation_file), 'wa'))
return
