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()