def registration_cost(xyz0, xyz1):
scaled_xyz0, _ = registration.unit_boxify(xyz0)
scaled_xyz1, _ = registration.unit_boxify(xyz1)
#TODO: n_iter was 10, reg_final was 0.01
f, g = registration.tps_rpm_bij(scaled_xyz0, scaled_xyz1, n_iter=10,
reg_init=10, reg_final=cost_reg_final)
cost = registration.tps_reg_cost(f) + registration.tps_reg_cost(g)
return cost
def registration_cost(xyz0, xyz1):
scaled_xyz0, _ = registration.unit_boxify(xyz0)
scaled_xyz1, _ = registration.unit_boxify(xyz1)
f, g = registration.tps_rpm_bij(scaled_xyz0,
scaled_xyz1,
rot_reg=1e-3,
n_iter=30)
cost = registration.tps_reg_cost(f) + registration.tps_reg_cost(g)
return cost
def registration_cost(xyz_src, xyz_targ, src_interest_pts=None):
if not src_interest_pts:
weights = None
else:
weights = compute_weights(xyz_src, src_interest_pts)
scaled_xyz_src, src_params = registration.unit_boxify(xyz_src)
scaled_xyz_targ, targ_params = registration.unit_boxify(xyz_targ)
f,g = registration.tps_rpm_bij(scaled_xyz_src, scaled_xyz_targ, plot_cb=None,
plotting=0, rot_reg=np.r_[1e-4, 1e-4, 1e-1],
n_iter=100, reg_init=10, reg_final=.1, outlierfrac=1e-2,
x_weights=weights)
cost = registration.tps_reg_cost(f) + registration.tps_reg_cost(g)
return f, src_params, g, targ_params, cost
def registration_cost(xyz0, xyz1):
scaled_xyz0, _ = registration.unit_boxify(xyz0)
scaled_xyz1, _ = registration.unit_boxify(xyz1)
#import matplotlib.pylab as plt
#plt.scatter(scaled_xyz0[:,0], scaled_xyz0[:,1], c='r' )
#plt.hold(True)
#plt.scatter(scaled_xyz1[:,0], scaled_xyz1[:,1], c='b' )
#plt.show()
#print xyz0.shape, xyz1.shape
f, g = registration.tps_rpm_bij(scaled_xyz0, scaled_xyz1, n_iter=10, rot_reg=1e-3)
cost = registration.tps_reg_cost(f) + registration.tps_reg_cost(g)
return cost
def warp_hmats(xyz_src, xyz_targ, hmat_list):
if not use_rapprentice:
return hmat_list
scaled_xyz_src, src_params = registration.unit_boxify(xyz_src)
scaled_xyz_targ, targ_params = registration.unit_boxify(xyz_targ)
f,g = registration.tps_rpm_bij(scaled_xyz_src, scaled_xyz_targ, plot_cb = None,
plotting=0,rot_reg=np.r_[1e-4,1e-4,1e-1],
n_iter=50, reg_init=10, reg_final=.1)
cost = registration.tps_reg_cost(f) + registration.tps_reg_cost(g)
f = registration.unscale_tps(f, src_params, targ_params)
trajs = {}
for k, hmats in hmat_list:
trajs[k] = f.transform_hmats(hmats)
return [trajs, cost]
def plot_tps_registrations_proj_2d(x_clouds, y_clouds, fs, res, texts):
# set interactive
plt.ion()
k = len(x_clouds) # number of demonstrations
fig, axes = plt.subplots(num='2d projection plot', facecolor='white', figsize=(18, 18), nrows=k, ncols=4)
if len(axes.shape) == 1:
axes= axes[None,:]
for i, (x_cloud, y_cloud, f, text) in enumerate(zip(x_clouds, y_clouds, fs, texts)):
x_nd = x_cloud[:,:3]
x_color = x_cloud[:,3:]
y_md = y_cloud[:,:3]
y_color = y_cloud[:,3:]
axes[i,0].clear()
axes[i,0].axis('off')
axes[i,0].set_aspect('equal')
axes[i,0].scatter(x_nd[:,0], x_nd[:,1], c=x_color, edgecolors='none', marker=',', s=5)
grid_means = .5 * (x_nd.max(axis=0) + x_nd.min(axis=0))
grid_mins = grid_means - (x_nd.max(axis=0) - x_nd.min(axis=0))
grid_maxs = grid_means + (x_nd.max(axis=0) - x_nd.min(axis=0))
x_median = np.median(x_nd, axis=0)
plt.sca(axes[i,0])
plotting_plt.plot_warped_grid_proj_2d(lambda xyz: xyz, grid_mins[:2], grid_maxs[:2], z=x_median[2], xres=res[0], yres=res[1], draw=False)
axes[i,1].clear()
axes[i,1].axis('off')
axes[i,1].set_aspect('equal')
axes[i,1].scatter(y_md[:,0], y_md[:,1], c=y_color, edgecolors='none', marker=',', s=5)
axes[i,2].clear()
axes[i,2].set_xlim(axes[i,1].get_xlim())
axes[i,2].set_ylim(axes[i,1].get_ylim())
axes[i,2].axis('off')
axes[i,2].set_aspect('equal')
xwarped_nd = f.transform_points(x_nd)
axes[i,2].scatter(xwarped_nd[:,0], xwarped_nd[:,1], c=x_color, edgecolors='none', marker=',', s=5)
plt.sca(axes[i,2])
plotting_plt.plot_warped_grid_proj_2d(f.transform_points, grid_mins[:2], grid_maxs[:2], z=x_median[2], xres=res[0], yres=res[1], draw=False)
axes[i,3].clear()
axes[i,3].axis('off')
axes[i,3].text(0.5, 0.5, '%s\n cost %.4f\n reg_cost %.4f'%(text, f._cost, registration.tps_reg_cost(f)),
horizontalalignment='center',
verticalalignment='center')
fig.tight_layout()
plt.draw()
def registration_cost(xyz0, xyz1):
scaled_xyz0, _ = registration.unit_boxify(xyz0)
scaled_xyz1, _ = registration.unit_boxify(xyz1)
f,g = registration.tps_rpm_bij(scaled_xyz0, scaled_xyz1, rot_reg=1e-3, n_iter=30)
cost = registration.tps_reg_cost(f) + registration.tps_reg_cost(g)
return cost
cloud1 = cloud1[:, :2]
scaled_cloud0, src_params = reg.unit_boxify(cloud0)
scaled_cloud1, targ_params = reg.unit_boxify(cloud1)
print "downsampled to %i and %i pts" % (len(scaled_cloud0),
len(scaled_cloud1))
tstart = time()
fest_scaled, gest_scaled = reg.tps_rpm_bij(scaled_cloud0,
scaled_cloud1,
n_iter=10,
reg_init=10,
reg_final=.01)
print "%.3f elapsed" % (time() - tstart)
cost = reg.tps_reg_cost(fest_scaled) + reg.tps_reg_cost(
gest_scaled)
print "cost: %.3f" % cost
import matplotlib.pyplot as plt
plt.clf()
# plt.plot(xys[:,1], xys[:,0],'r.')
# plt.plot(fxys[:,1], fxys[:,0],'b.')
# plt.plot(fxys_est[:,1], fxys_est[:,0],'g.')
scaled_fcloud = fest_scaled.transform_points(scaled_cloud0)
plt.plot(scaled_cloud0[:, 1], scaled_cloud0[:, 0], 'r.')
plt.plot(scaled_cloud1[:, 1], scaled_cloud1[:, 0], 'b.')
plt.plot(scaled_fcloud[:, 1], scaled_fcloud[:, 0], 'g.')
if dim == 2:
for cloud0 in clouds:
for cloud1 in clouds:
if dim ==2 :
cloud0 = cloud0[:,:2]
cloud1 = cloud1[:,:2]
scaled_cloud0, src_params = reg.unit_boxify(cloud0)
scaled_cloud1, targ_params = reg.unit_boxify(cloud1)
print "downsampled to %i and %i pts"%(len(scaled_cloud0),len(scaled_cloud1))
tstart = time()
fest_scaled,gest_scaled = reg.tps_rpm_bij(scaled_cloud0, scaled_cloud1, n_iter=10, reg_init = 10, reg_final=.01)
print "%.3f elapsed"%(time() - tstart)
cost = reg.tps_reg_cost(fest_scaled) + reg.tps_reg_cost(gest_scaled)
print "cost: %.3f"%cost
import matplotlib.pyplot as plt
plt.clf()
# plt.plot(xys[:,1], xys[:,0],'r.')
# plt.plot(fxys[:,1], fxys[:,0],'b.')
# plt.plot(fxys_est[:,1], fxys_est[:,0],'g.')
scaled_fcloud = fest_scaled.transform_points(scaled_cloud0)
plt.plot(scaled_cloud0[:,1], scaled_cloud0[:,0],'r.')
plt.plot(scaled_cloud1[:,1], scaled_cloud1[:,0],'b.')
plt.plot(scaled_fcloud[:,1], scaled_fcloud[:,0],'g.')
if dim == 2:
def main():
parser = argparse.ArgumentParser()
parser.add_argument('actionfile', type=str)
parser.add_argument('cloudfile', type=str, help="result file containing the encountered clouds")
parser.add_argument('type', type=str, choices=['explore', 'compare'], help="")
parser.add_argument("--tasks", type=int, nargs='*', metavar="i_task")
parser.add_argument("--taskfile", type=str)
parser.add_argument("--i_start", type=int, default=-1, metavar="i_task")
parser.add_argument("--i_end", type=int, default=-1, metavar="i_task")
parser.add_argument("--steps", type=int, nargs='*', metavar="i_step")
parser.add_argument("--draw_rows", type=int, default=8, help="plot only the draw_rows registrations with the smallest cost")
parser.add_argument("--ds_size", type=float, default=0.025, metavar="meters")
args = parser.parse_args()
actionfile = h5py.File(args.actionfile, 'r')
cloudfile = h5py.File(args.cloudfile, 'r')
cloud_items = eval_util.get_holdout_items(cloudfile, args.tasks, args.taskfile, args.i_start, args.i_end)
if args.type == 'explore':
fs = []
reg_costs = []
old_clouds_ds = []
new_clouds_ds = []
texts = []
for i_task, _ in cloud_items:
n_steps = len(cloudfile[i_task]) - (1 if 'init' in cloudfile[i_task] else 0)
steps = [i_step for i_step in args.steps if i_step in range(n_steps)] if args.steps else range(n_steps)
for i_step in steps:
if args.steps and i_step not in args.steps:
continue
print "task %s step %i" % (i_task, i_step)
old_cloud = cloudfile[i_task][str(i_step)]['demo_cloud'][()]
old_cloud_ds = clouds.downsample(old_cloud, args.ds_size)
new_cloud = cloudfile[i_task][str(i_step)]['cloud'][()]
new_cloud_ds = clouds.downsample(new_cloud, args.ds_size)
f, corr = tps_registration.tps_rpm(old_cloud_ds[:,:3], new_cloud_ds[:,:3], n_iter=N_ITER_CHEAP, em_iter=EM_ITER_CHEAP, vis_cost_xy = tps_registration.ab_cost(old_cloud_ds, new_cloud_ds), user_data={'old_cloud':old_cloud_ds, 'new_cloud':new_cloud_ds})
reg_cost = registration.tps_reg_cost(f)
fs.append(f)
reg_costs.append(reg_cost)
old_clouds_ds.append(old_cloud_ds)
new_clouds_ds.append(new_cloud_ds)
texts.append("task %s step %i" % (i_task, i_step))
if args.draw_rows == len(fs) or (i_task == cloud_items[-1][0] and i_step == steps[-1]):
# sort based on reg_costs
fs = [f for (cost, f) in sorted(zip(reg_costs, fs))]
old_clouds_ds = [cloud for (cost, cloud) in sorted(zip(reg_costs, old_clouds_ds))]
new_clouds_ds = [cloud for (cost, cloud) in sorted(zip(reg_costs, new_clouds_ds))]
texts = [text for (cost, text) in sorted(zip(reg_costs, texts))]
print "plotting"
plot_tps_registrations_proj_2d(old_clouds_ds, new_clouds_ds, fs, (.1, .1, .04), texts)
ipy.embed()
fs[:] = []
reg_costs[:] = []
old_clouds_ds[:] = []
new_clouds_ds[:] = []
texts[:] = []
elif args.type == 'compare':
for i_task, _ in cloud_items:
for i_step in range(len(cloudfile[i_task]) - (1 if 'init' in cloudfile[i_task] else 0)):
if args.steps and i_step not in args.steps:
continue
print "task %s step %i" % (i_task, i_step)
new_cloud = cloudfile[i_task][str(i_step)]['cloud'][()]
new_cloud_ds = clouds.downsample(new_cloud, args.ds_size)
fs = []
reg_costs = []
old_clouds_ds = []
new_clouds_ds = []
texts = []
print "computing costs"
for action in actionfile.keys():
old_cloud = actionfile[action]['cloud_xyz']
old_cloud_ds = clouds.downsample(old_cloud, args.ds_size)
f, corr = tps_registration.tps_rpm(old_cloud_ds[:,:3], new_cloud_ds[:,:3], n_iter=N_ITER_CHEAP, em_iter=EM_ITER_CHEAP, vis_cost_xy = tps_registration.ab_cost(old_cloud_ds, new_cloud_ds), user_data={'old_cloud':old_cloud_ds, 'new_cloud':new_cloud_ds})
reg_cost = registration.tps_reg_cost(f)
fs.append(f)
reg_costs.append(reg_cost)
old_clouds_ds.append(old_cloud_ds)
new_clouds_ds.append(new_cloud_ds)
texts.append("task %s step %i action %s" % (i_task, i_step, action))
# sort based on reg_costs
fs = [f for (cost, f) in sorted(zip(reg_costs, fs))]
old_clouds_ds = [cloud for (cost, cloud) in sorted(zip(reg_costs, old_clouds_ds))]
if args.draw_rows != -1:
fs = fs[:args.draw_rows]
old_clouds_ds = old_clouds_ds[:args.draw_rows]
print "plotting"
plot_tps_registrations_proj_2d(old_clouds_ds, new_clouds_ds, fs, (.1, .1, .04), texts)
ipy.embed()
actionfile.close()
cloudfile.close()
