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