def test_fit_nr(): from jds_image_proc.clouds import voxel_downsample from brett2.ros_utils import RvizWrapper import lfd.registration as lr import lfd.warping as lw if rospy.get_name() == "/unnamed": rospy.init_node("test_rigidity", disable_signals=True) from time import sleep sleep(1) rviz = RvizWrapper.create() pts0 = np.loadtxt("test/rope_control_points.txt") pts1 = np.loadtxt("test/bad_rope.txt") pts_rigid = voxel_downsample(pts0[:10], .02) lr.Globals.setup() np.seterr(all='ignore') np.set_printoptions(suppress=True) lin_ag, trans_g, w_eg, x_ea = tps_nr_fit_enhanced(pts0, pts1, 0.01, pts_rigid, 0.001, method="newton",plotting=1) #lin_ag2, trans_g2, w_ng2 = tps_fit(pts0, pts1, .01, .01) #assert np.allclose(w_ng, w_ng2) def eval_partial(x_ma): return tps_eval(x_ma, lin_ag, trans_g, w_eg, x_ea) lr.plot_orig_and_warped_clouds(eval_partial, pts0, pts1, res=.008) #handles = lw.draw_grid(rviz, eval_partial, pts0.min(axis=0), pts0.max(axis=0), 'base_footprint') grads = tps_grad(pts_rigid, lin_ag, trans_g, w_eg, x_ea) print "worst violation:",np.max(np.abs([grad.T.dot(grad)-np.eye(3) for grad in grads]))
def tps_nr_fit_enhanced(x_na, y_ng, bend_coef, nr_ma, nr_coef, method="newton", plotting=0): N,D = x_na.shape M = nr_ma.shape[0] E = N + 4*M F = E - M Q = N + 3*M - 4 s = .1 # tetrahedron sidelength (meters) u = 1/(2*np.sqrt(2)) tetra_pts = [] for pt in nr_ma: tetra_pts.append(s*np.r_[-.5, 0, -u]+pt) tetra_pts.append(s*np.r_[+.5, 0, -u]+pt) tetra_pts.append(s*np.r_[0, -.5, +u]+pt) tetra_pts.append(s*np.r_[0, +.5, +u]+pt) x_ea = np.r_[x_na, tetra_pts] badsub_ex = np.c_[x_ea, np.ones((E,1)), np.r_[np.zeros((N,M)), np.repeat(np.eye(M), 4, axis=0)]] lin_ag, trans_g, w_ng = tps_fit2(x_na, y_ng, bend_coef, 1e-3) w_eg = np.r_[w_ng, np.zeros((4*M, D))] assert badsub_ex.shape[0] >= badsub_ex.shape[1] _u,_s,_vh = np.linalg.svd(badsub_ex, full_matrices=True) assert badsub_ex.shape[1] == _s.size N_eq = _u[:,badsub_ex.shape[1]:] # null of data assert N_eq.shape == (E,Q) assert E == N + 4*M assert F == Q + 4 # e is number of kernels # q is number of nonrigid dofs # f is total number of dofs K_ee = ssd.squareform(ssd.pdist(x_ea)) K_ne = K_ee[:N, :] Q_nf = np.c_[x_na, np.ones((N,1)),K_ne.dot(N_eq)] QQ_ff = np.dot(Q_nf.T, Q_nf) Bend_ff = np.zeros((F,F)) Bend_ff[4:, 4:] = - N_eq.T.dot(K_ee.dot(N_eq)) # K_qq assert Q_nf.shape == (N, F) assert w_eg.shape == (E, D) n_iter=40 for i in xrange(n_iter): if plotting and i%plotting==0: import lfd.registration as lr lr.Globals.setup() def eval_partial(x_ma): return tps_eval(x_ma, lin_ag, trans_g, w_eg, x_ea) lr.plot_orig_and_warped_clouds(eval_partial, x_na, y_ng, res=.008) X_fg = np.r_[lin_ag, trans_g[None,:], N_eq.T.dot(w_eg)] res_cost, bend_cost, nr_cost, fval = tps_nr_cost_eval_general(lin_ag, trans_g, w_eg, x_ea, y_ng, nr_ma, bend_coef, nr_coef, return_tuple=True) if VERBOSE: print colorize("iteration %i, cost %.3e"%(i, fval), 'red'), if VERBOSE: print "= %.3e (res) + %.3e (bend) + %.3e (nr)"%(res_cost, bend_cost, nr_cost) Jl_zcg, Jt_zg, Jw_zeg = tps_nr_grad(nr_ma, lin_ag, trans_g, w_eg, x_ea, return_tuple=True) nrerr_z = tps_nr_err(nr_ma, lin_ag, trans_g, w_eg, x_ea) fullstep_fg = np.empty((F,D)) for g in xrange(D): J_zf = np.c_[Jl_zcg[:,g::D], Jt_zg[:,g::D], Jw_zeg[:,g::D].dot(N_eq)] JJ_ff = np.dot(J_zf.T, J_zf) A_ff = nr_coef*JJ_ff + QQ_ff + bend_coef*Bend_ff X0 = X_fg[:,g] B_f = nr_coef*np.dot(J_zf.T, np.dot(J_zf, X0) - nrerr_z) + Q_nf.T.dot(y_ng[:,g]) fullstep_fg[:,g] = np.linalg.solve(A_ff,B_f) - X_fg[:,g] cost_improved = False for stepsize in 3.**np.arange(0,-10,-1): cand_X_fg = X_fg + fullstep_fg*stepsize cand_lin_ag, cand_trans_g, cand_w_eg = cand_X_fg[:D], cand_X_fg[D], N_eq.dot(cand_X_fg[D+1:]) fval_cand = tps_nr_cost_eval_general(cand_lin_ag, cand_trans_g, cand_w_eg, x_ea, y_ng, nr_ma, bend_coef, nr_coef, return_tuple=False) if VERBOSE: print "stepsize: %.1g, fval: %.3e"%(stepsize, fval_cand) if fval_cand < fval: cost_improved = True break if not cost_improved: if VERBOSE: print "couldn't improve objective" break lin_ag = cand_lin_ag trans_g = cand_trans_g w_eg = cand_w_eg return lin_ag, trans_g, w_eg, x_ea
def tps_nr_fit_enhanced(x_na, y_ng, bend_coef, nr_ma, nr_coef, method="newton", plotting=0): N, D = x_na.shape M = nr_ma.shape[0] E = N + 4 * M F = E - M Q = N + 3 * M - 4 s = .1 # tetrahedron sidelength (meters) u = 1 / (2 * np.sqrt(2)) tetra_pts = [] for pt in nr_ma: tetra_pts.append(s * np.r_[-.5, 0, -u] + pt) tetra_pts.append(s * np.r_[+.5, 0, -u] + pt) tetra_pts.append(s * np.r_[0, -.5, +u] + pt) tetra_pts.append(s * np.r_[0, +.5, +u] + pt) x_ea = np.r_[x_na, tetra_pts] badsub_ex = np.c_[x_ea, np.ones((E, 1)), np.r_[np.zeros((N, M)), np.repeat(np.eye(M), 4, axis=0)]] lin_ag, trans_g, w_ng = tps_fit2(x_na, y_ng, bend_coef, 1e-3) w_eg = np.r_[w_ng, np.zeros((4 * M, D))] assert badsub_ex.shape[0] >= badsub_ex.shape[1] _u, _s, _vh = np.linalg.svd(badsub_ex, full_matrices=True) assert badsub_ex.shape[1] == _s.size N_eq = _u[:, badsub_ex.shape[1]:] # null of data assert N_eq.shape == (E, Q) assert E == N + 4 * M assert F == Q + 4 # e is number of kernels # q is number of nonrigid dofs # f is total number of dofs K_ee = ssd.squareform(ssd.pdist(x_ea)) K_ne = K_ee[:N, :] Q_nf = np.c_[x_na, np.ones((N, 1)), K_ne.dot(N_eq)] QQ_ff = np.dot(Q_nf.T, Q_nf) Bend_ff = np.zeros((F, F)) Bend_ff[4:, 4:] = -N_eq.T.dot(K_ee.dot(N_eq)) # K_qq assert Q_nf.shape == (N, F) assert w_eg.shape == (E, D) n_iter = 40 for i in xrange(n_iter): if plotting and i % plotting == 0: import lfd.registration as lr lr.Globals.setup() def eval_partial(x_ma): return tps_eval(x_ma, lin_ag, trans_g, w_eg, x_ea) lr.plot_orig_and_warped_clouds(eval_partial, x_na, y_ng, res=.008) X_fg = np.r_[lin_ag, trans_g[None, :], N_eq.T.dot(w_eg)] res_cost, bend_cost, nr_cost, fval = tps_nr_cost_eval_general( lin_ag, trans_g, w_eg, x_ea, y_ng, nr_ma, bend_coef, nr_coef, return_tuple=True) if VERBOSE: print colorize("iteration %i, cost %.3e" % (i, fval), 'red'), if VERBOSE: print "= %.3e (res) + %.3e (bend) + %.3e (nr)" % ( res_cost, bend_cost, nr_cost) Jl_zcg, Jt_zg, Jw_zeg = tps_nr_grad(nr_ma, lin_ag, trans_g, w_eg, x_ea, return_tuple=True) nrerr_z = tps_nr_err(nr_ma, lin_ag, trans_g, w_eg, x_ea) fullstep_fg = np.empty((F, D)) for g in xrange(D): J_zf = np.c_[Jl_zcg[:, g::D], Jt_zg[:, g::D], Jw_zeg[:, g::D].dot(N_eq)] JJ_ff = np.dot(J_zf.T, J_zf) A_ff = nr_coef * JJ_ff + QQ_ff + bend_coef * Bend_ff X0 = X_fg[:, g] B_f = nr_coef * np.dot(J_zf.T, np.dot(J_zf, X0) - nrerr_z) + Q_nf.T.dot( y_ng[:, g]) fullstep_fg[:, g] = np.linalg.solve(A_ff, B_f) - X_fg[:, g] cost_improved = False for stepsize in 3.**np.arange(0, -10, -1): cand_X_fg = X_fg + fullstep_fg * stepsize cand_lin_ag, cand_trans_g, cand_w_eg = cand_X_fg[:D], cand_X_fg[ D], N_eq.dot(cand_X_fg[D + 1:]) fval_cand = tps_nr_cost_eval_general(cand_lin_ag, cand_trans_g, cand_w_eg, x_ea, y_ng, nr_ma, bend_coef, nr_coef, return_tuple=False) if VERBOSE: print "stepsize: %.1g, fval: %.3e" % (stepsize, fval_cand) if fval_cand < fval: cost_improved = True break if not cost_improved: if VERBOSE: print "couldn't improve objective" break lin_ag = cand_lin_ag trans_g = cand_trans_g w_eg = cand_w_eg return lin_ag, trans_g, w_eg, x_ea
pts_rigid = pts0 lin_ag, trans_g, w_ng = tps_nr_fit_enhanced(pts0, pts1, 0.01, pts0, .1, method="newton", plotting=4) #lin_ag2, trans_g2, w_ng2 = tps_fit(pts0, pts1, .01, .01) #assert np.allclose(w_ng, w_ng2) def eval_partial(x_ma): return tps_eval(x_ma, lin_ag, trans_g, w_ng, pts0) lr.plot_orig_and_warped_clouds(eval_partial, pts0, pts1, res=.008) #handles = lw.draw_grid(rviz, eval_partial, pts0.min(axis=0), pts0.max(axis=0), 'base_footprint') grads = tps_grad(pts_rigid, lin_ag, trans_g, w_ng, pts0) print "worst violation:", np.max( np.abs([grad.T.dot(grad) - np.eye(3) for grad in grads])) #for grad in grads: #print # L0, T0, W0 = tps_fit(pts0, pts1, 0) # L1, T1, W1 = tps_fit2(pts0, pts1) #J1q = J1.reshape(gM, 3, 3, gN,3) #Jq = J.reshape(gM, 3, 3, gN, 3) #J1 = nr_grads(pts_eval, tps.a_Dd, tps.w_nd, tps.x_nd) #_, rots = tps.transform_frames(pts0, np.eye(3)[None,:,:], orthogonalize=False)
assert np.allclose(lin_ag, lin2_ag) assert np.allclose(trans_g, trans2_g) assert np.allclose(w_ng, w2_ng) if 1: lr.Globals.setup() np.seterr(all='ignore') np.set_printoptions(suppress=True) pts_rigid = pts0 lin_ag, trans_g, w_ng = tps_nr_fit_enhanced(pts0, pts1, 0.01, pts0, .1, method="newton",plotting=4) #lin_ag2, trans_g2, w_ng2 = tps_fit(pts0, pts1, .01, .01) #assert np.allclose(w_ng, w_ng2) def eval_partial(x_ma): return tps_eval(x_ma, lin_ag, trans_g, w_ng, pts0) lr.plot_orig_and_warped_clouds(eval_partial, pts0, pts1, res=.008) #handles = lw.draw_grid(rviz, eval_partial, pts0.min(axis=0), pts0.max(axis=0), 'base_footprint') grads = tps_grad(pts_rigid, lin_ag, trans_g, w_ng, pts0) print "worst violation:",np.max(np.abs([grad.T.dot(grad)-np.eye(3) for grad in grads])) #for grad in grads: #print # L0, T0, W0 = tps_fit(pts0, pts1, 0) # L1, T1, W1 = tps_fit2(pts0, pts1) #J1q = J1.reshape(gM, 3, 3, gN,3) #Jq = J.reshape(gM, 3, 3, gN, 3) #J1 = nr_grads(pts_eval, tps.a_Dd, tps.w_nd, tps.x_nd) #_, rots = tps.transform_frames(pts0, np.eye(3)[None,:,:], orthogonalize=False)