def callback(self, xk):
s_z0tgt = xk[:2]
s_theta0 = xk[2]
s_z1tgt = xk[3:5]
s_theta1 = xk[5]
mug0mapped = local_trans(mug0src, z0src, s_z0tgt, s_theta0)
mug1mapped = local_trans(mug1src, z1src, s_z1tgt, s_theta1)
mugsmapped = np.r_[mug0mapped, mug1mapped]
self.counter += 1
if self.counter % 10 == 0:
print "plotting"
if self.green_plot is not None:
ax = gca()
ax.lines.remove(self.green_plot)
ax.collections = []
self.green_plot, = plot(mugsmapped[:,1], mugsmapped[:,0], 'g.')
def warp_fn(x):
return warp(x, s_z0tgt, s_z1tgt, s_theta0, s_theta1)
plot_warped_grid_2d(warp_fn, uvs_src.min(axis=0), uvs_src.max(axis=0))
draw()
else:
print "not plotting"
def callback(self, xk):
s_z0tgt = xk[:2]
s_theta0 = xk[2]
s_z1tgt = xk[3:5]
s_theta1 = xk[5]
mug0mapped = local_trans(mug0src, z0src, s_z0tgt, s_theta0)
mug1mapped = local_trans(mug1src, z1src, s_z1tgt, s_theta1)
mugsmapped = np.r_[mug0mapped, mug1mapped]
self.counter += 1
if self.counter % 10 == 0:
print "plotting"
if self.green_plot is not None:
ax = gca()
ax.lines.remove(self.green_plot)
ax.collections = []
self.green_plot, = plot(mugsmapped[:, 1], mugsmapped[:, 0], 'g.')
def warp_fn(x):
return warp(x, s_z0tgt, s_z1tgt, s_theta0, s_theta1)
plot_warped_grid_2d(warp_fn, uvs_src.min(axis=0),
uvs_src.max(axis=0))
draw()
else:
print "not plotting"
def test_fitting():
matobj = sio.loadmat(osp.join(osp.dirname(lfd.__file__), "matlab", "pointset_pair.mat"))
points0 = matobj["xy1"]
points1 = matobj["xy2"]
tps = registration.ThinPlateSpline()
tps.fit(points0, points1,.001,.001)
points0_tf = tps.transform_points(points0)
plt.plot(points0[:,0], points0[:,1],'r.')
plt.plot(points1[:,0], points1[:,1],'b.')
plt.plot(points0_tf[:,0], points0_tf[:,1], 'g.')
registration.plot_warped_grid_2d(tps.transform_points, points0.min(axis=0), points0.max(axis=0))
# src0, src1 = np.load(osp.join(data_dir, "clouds0.npy"))
src0 = randn(100, 3) * np.array([[2, 1, 1]]) + np.array([[10, 0, 0]])
src1 = randn(100, 3) + np.array([[10, 10, 0]])
targ0 = randn(100, 3) * np.array([[2, 1, 1]]) + np.array([[30, 0, 0]])
targ1 = randn(100, 3) + np.array([[-10, 0, 0]])
src = np.r_[src0, src1]
# targ0, targ1 = np.load(osp.join(data_dir, "clouds1.npy"))
targ = np.r_[targ0, targ1]
f = pr.InterpolatedTransformation()
f.fit([src0, src1], [targ0, targ1], transform_type="rigid_planar")
from lfd.warping import draw_grid
# draw_grid(rviz, lambda x: f.transform_points(x[:,:2]), src.min(axis=0), src.max(axis=0),
# "base_footprint", xres = .03, yres = .03, zres = .02)
from lfd.registration import plot_warped_grid_2d
plot(src0[:, 1], src0[:, 0], 'r.')
plot(src1[:, 1], src1[:, 0], 'r.')
plot(targ0[:, 1], targ0[:, 0], 'b.')
plot(targ1[:, 1], targ1[:, 0], 'b.')
msrc = f.transform_points(src)
plot(msrc[:, 1], msrc[:, 0], 'g.')
f2d = lambda x: f.transform_points(np.c_[x, np.zeros((len(x), 1))])[:, :2]
plot_warped_grid_2d(f2d, src.min(axis=0)[:2], src.max(axis=0)[:2], grid_res=.5)
axis('equal')
show()
targ0 = randn(100,3) * np.array([[2,1,1]]) + np.array([[30,0,0]])
targ1 = randn(100,3) + np.array([[-10,0,0]])
src = np.r_[src0, src1]
# targ0, targ1 = np.load(osp.join(data_dir, "clouds1.npy"))
targ = np.r_[targ0, targ1]
f = pr.InterpolatedTransformation()
f.fit([src0, src1], [targ0, targ1], transform_type = "rigid_planar")
from lfd.warping import draw_grid
# draw_grid(rviz, lambda x: f.transform_points(x[:,:2]), src.min(axis=0), src.max(axis=0),
# "base_footprint", xres = .03, yres = .03, zres = .02)
from lfd.registration import plot_warped_grid_2d
plot(src0[:,1], src0[:,0], 'r.')
plot(src1[:,1], src1[:,0], 'r.')
plot(targ0[:,1], targ0[:,0], 'b.')
plot(targ1[:,1], targ1[:,0], 'b.')
msrc = f.transform_points(src)
plot(msrc[:,1], msrc[:,0],'g.')
f2d = lambda x: f.transform_points(np.c_[x, np.zeros((len(x),1))])[:,:2]
plot_warped_grid_2d(f2d, src.min(axis=0)[:2], src.max(axis=0)[:2], grid_res = .5)
axis('equal')
show()
