def registration(xys, fxys):
if args.to3d:
xys = np.c_[xys, np.zeros((len(xys)))]
fxys = np.c_[fxys, np.zeros((len(fxys)))]
scaled_xys, src_params = reg.unit_boxify(xys)
scaled_fxys, targ_params = reg.unit_boxify(fxys)
downsample = voxel_downsample if args.to3d else pixel_downsample
scaled_ds_xys = downsample(scaled_xys, .03)
scaled_ds_fxys = downsample(scaled_fxys, .03)
print "downsampled to %i and %i pts" % (len(scaled_ds_xys),
len(scaled_ds_fxys))
tstart = time()
# fest_scaled = reg.tps_rpm(scaled_ds_xys, scaled_ds_fxys, n_iter=10, reg_init = 10, reg_final=.01)
fest_scaled, _ = reg.tps_rpm_bij(scaled_ds_xys,
scaled_ds_fxys,
n_iter=10,
reg_init=10,
reg_final=.01)
print "time: %.4f" % (time() - tstart)
fest = reg.unscale_tps(fest_scaled, src_params, targ_params)
fxys_est = fest.transform_points(xys)
if len(fxys_est) == len(fxys):
print "error:", np.abs(fxys_est - fxys).mean()
if args.plotting:
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_ds_fxys_est = fest_scaled.transform_points(scaled_ds_xys)
plt.plot(scaled_ds_xys[:, 1], scaled_ds_xys[:, 0], 'r.')
plt.plot(scaled_ds_fxys[:, 1], scaled_ds_fxys[:, 0], 'b.')
plt.plot(scaled_ds_fxys_est[:, 1], scaled_ds_fxys_est[:, 0], 'g.')
def to2d(f):
def f2d(x):
return f(np.c_[x, np.zeros((len(x), 1))])[:, :2]
return f2d
transform_func = to2d(fest_scaled.transform_points
) if args.to3d else fest_scaled.transform_points
plot_warped_grid_2d(transform_func, [-.5, -.5], [.5, .5])
plt.draw()
plt.ginput()
def registration(xys, fxys):
if args.to3d:
xys = np.c_[xys, np.zeros((len(xys)))]
fxys = np.c_[fxys, np.zeros((len(fxys)))]
scaled_xys, src_params = reg.unit_boxify(xys)
scaled_fxys, targ_params = reg.unit_boxify(fxys)
downsample = voxel_downsample if args.to3d else pixel_downsample
scaled_ds_xys = downsample(scaled_xys, .03)
scaled_ds_fxys = downsample(scaled_fxys, .03)
print "downsampled to %i and %i pts"%(len(scaled_ds_xys),len(scaled_ds_fxys))
tstart = time()
# fest_scaled = reg.tps_rpm(scaled_ds_xys, scaled_ds_fxys, n_iter=10, reg_init = 10, reg_final=.01)
fest_scaled,_ = reg.tps_rpm_bij(scaled_ds_xys, scaled_ds_fxys, n_iter=10, reg_init = 10, reg_final=.01)
print "time: %.4f"%(time()-tstart)
fest = reg.unscale_tps(fest_scaled, src_params, targ_params)
fxys_est = fest.transform_points(xys)
if len(fxys_est) == len(fxys): print "error:", np.abs(fxys_est - fxys).mean()
if args.plotting:
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_ds_fxys_est = fest_scaled.transform_points(scaled_ds_xys)
plt.plot(scaled_ds_xys[:,1], scaled_ds_xys[:,0],'r.')
plt.plot(scaled_ds_fxys[:,1], scaled_ds_fxys[:,0],'b.')
plt.plot(scaled_ds_fxys_est[:,1], scaled_ds_fxys_est[:,0],'g.')
def to2d(f):
def f2d(x):
return f(np.c_[x, np.zeros((len(x),1))])[:,:2]
return f2d
transform_func = to2d(fest_scaled.transform_points) if args.to3d else fest_scaled.transform_points
plot_warped_grid_2d(transform_func, [-.5,-.5], [.5,.5])
plt.draw()
plt.ginput()
def plot_cb(x_nd, y_md, xtarg_nd, corr_nm, wt_n, f):
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.')
fx_nd = f.transform_points(x_nd)
plt.plot(x_nd[:,1], x_nd[:,0],'r.')
plt.plot(fx_nd[:,1], fx_nd[:,0],'g.')
plt.plot(y_md[:,1], y_md[:,0],'b.')
def to2d(f):
def f2d(x):
return f(np.c_[x, np.zeros((len(x),1))])[:,:2]
return f2d
transform_func = to2d(f.transform_points) if args.to3d else f.transform_points
plot_warped_grid_2d(transform_func, [-.5,-.5], [.5,.5])
plt.draw()
plt.ginput()
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:
from rapprentice.plotting_plt import plot_warped_grid_2d
plot_warped_grid_2d(fest_scaled.transform_points, [-.5, -.5],
[.5, .5])
elif dim == 3:
env = openravepy.RaveGetEnvironment(1)
if env is None: env = openravepy.Environment()
from rapprentice.plotting_openrave import draw_grid
draw_grid(fest_scaled, [-.5, -.5, -.5], [.5, .5, .5])
plt.draw()
raw_input()
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:
from rapprentice.plotting_plt import plot_warped_grid_2d
plot_warped_grid_2d(fest_scaled.transform_points, [-.5,-.5], [.5,.5])
elif dim == 3:
env= openravepy.RaveGetEnvironment(1)
if env is None: env = openravepy.Environment()
from rapprentice.plotting_openrave import draw_grid
draw_grid(fest_scaled, [-.5,-.5,-.5], [.5,.5,.5])
plt.draw()
raw_input()