def tps_nr_fit_enhanced(x_na, y_ng, bend_coef, nr_ma, nr_coef):
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 = tps_kernel_matrix(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):
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 = tps_kernel_matrix(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(x_na, y_ng, bend_coef, nr_ma, nr_coef, method="newton"):
N, D = x_na.shape
lin_ag, trans_g, w_ng = tps_fit2(x_na, y_ng, bend_coef, 1e-3)
#return lin_ag, trans_g, w_ng
##for testing that it takes one step when nonrigidity cost is zero:
#lin_ag, trans_g, w_ng = tps_fit(x_na, y_ng, bend_coef, 0)
#res_cost, bend_cost, nr_cost, fval = tps_nr_cost_eval(lin_ag, trans_g, w_ng, x_na, nr_ma, bend_coef, nr_coef, return_tuple=True)
#print "CORRECT COST, res,bend,nr,total = %.3e, %.3e, %.3e, %.3e"%(res_cost, bend_cost, nr_cost, fval)
#lin_ag += np.random.randn(*lin_ag.shape)*5
#res_cost, bend_cost, nr_cost, fval = tps_nr_cost_eval(lin_ag, trans_g, w_ng, x_na, nr_ma, bend_coef, nr_coef, return_tuple=True)
#print "NOISE ADDED COST, res,bend,nr,total = %.ef, %.3e, %.3e, %.3e"%(res_cost, bend_cost, nr_cost, fval)
_u, _s, _vh = np.linalg.svd(np.c_[x_na, np.ones((N, 1))],
full_matrices=True)
N_nq = _u[:, 4:] # null of data
#w_ng = N_nq.dot(N_nq.T.dot(w_ng))
K_nn = tps_kernel_matrix(x_na)
Q_nn = np.c_[x_na, np.ones((N, 1)), K_nn.dot(N_nq)]
QQ_nn = np.dot(Q_nn.T, Q_nn)
Bend_nn = np.zeros((N, N))
Bend_nn[4:, 4:] = -N_nq.T.dot(K_nn.dot(N_nq))
n_iter = 60
for i in xrange(n_iter):
X_ng = np.r_[lin_ag, trans_g[None, :], N_nq.T.dot(w_ng)]
res_cost, bend_cost, nr_cost, fval = tps_nr_cost_eval(
lin_ag,
trans_g,
w_ng,
x_na,
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_zng = tps_nr_grad(nr_ma,
lin_ag,
trans_g,
w_ng,
x_na,
return_tuple=True)
nrerr_z = tps_nr_err(nr_ma, lin_ag, trans_g, w_ng, x_na)
if method == "newton":
fullstep_ng = np.empty((N, D))
for g in xrange(D):
J_zn = np.c_[Jl_zcg[:, g::D], Jt_zg[:, g::D],
Jw_zng[:, g::D].dot(N_nq)]
JJ_nn = np.dot(J_zn.T, J_zn)
A = nr_coef * JJ_nn + QQ_nn + bend_coef * Bend_nn
X0 = X_ng[:, g]
B = nr_coef * np.dot(J_zn.T,
np.dot(J_zn, X0) - nrerr_z) + Q_nn.T.dot(
y_ng[:, g])
fullstep_ng[:, g] = np.linalg.solve(A, B) - X_ng[:, g]
elif method == "gradient":
# def eval_partial(cand_X_ng):
# cand_X_ng = cand_X_ng.reshape(-1,3)
# cand_lin_ag, cand_trans_g, cand_w_ng = cand_X_ng[:D], cand_X_ng[D], N_nq.dot(cand_X_ng[D+1:])
# fval_cand = tps_nr_cost_eval(cand_lin_ag, cand_trans_g, cand_w_ng, x_na, y_ng, nr_ma, bend_coef, nr_coef)
# return fval_cand
# def eval_partial2(cand_X_ng):
# return ((Q_nn.dot(X_ng) - y_ng)**2).sum()
# def eval_partial3(cand_X_ng):
# cand_X_ng = cand_X_ng.reshape(-1,3)
# cand_lin_ag, cand_trans_g, cand_w_ng = cand_X_ng[:D], cand_X_ng[D], N_nq.dot(cand_X_ng[D+1:])
# return ((y_ng - tps_eval(x_na, cand_lin_ag, cand_trans_g, cand_w_ng, x_na))**2).sum()
grad_ng = np.empty((N, D))
for g in xrange(D - 1, -1, -1):
Jnr_zn = np.c_[Jl_zcg[:, g::D], Jt_zg[:, g::D],
Jw_zng[:, g::D].dot(N_nq)]
grad_ng[:,g] = 2 * nr_coef * nrerr_z.dot(Jnr_zn) \
+ 2 * Q_nn.T.dot(Q_nn.dot(X_ng[:,g]) - y_ng[:,g]) \
+ 2 * bend_coef * Bend_nn.dot(X_ng[:,g])
#assert np.allclose(eval_partial2(X_ng), eval_partial3(X_ng))
#assert np.allclose(eval_partial(X_ng), eval_partial2(X_ng))
#grad0_ng = ndt.Gradient(eval_partial)(X_ng.flatten()).reshape(-1,3)
fullstep_ng = -grad_ng
#assert np.allclose(grad0_ng, grad_ng)
cost_improved = False
for stepsize in 3.**np.arange(0, -10, -1):
cand_X_ng = X_ng + fullstep_ng * stepsize
cand_lin_ag, cand_trans_g, cand_w_ng = cand_X_ng[:D], cand_X_ng[
D], N_nq.dot(cand_X_ng[D + 1:])
fval_cand = tps_nr_cost_eval(cand_lin_ag, cand_trans_g, cand_w_ng,
x_na, y_ng, nr_ma, bend_coef, nr_coef)
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_ng = cand_w_ng
return lin_ag, trans_g, w_ng
def tps_nr_fit(x_na, y_ng, bend_coef, nr_ma, nr_coef, method="newton"):
N,D = x_na.shape
lin_ag, trans_g, w_ng = tps_fit2(x_na, y_ng, bend_coef, 1e-3)
#return lin_ag, trans_g, w_ng
##for testing that it takes one step when nonrigidity cost is zero:
#lin_ag, trans_g, w_ng = tps_fit(x_na, y_ng, bend_coef, 0)
#res_cost, bend_cost, nr_cost, fval = tps_nr_cost_eval(lin_ag, trans_g, w_ng, x_na, nr_ma, bend_coef, nr_coef, return_tuple=True)
#print "CORRECT COST, res,bend,nr,total = %.3e, %.3e, %.3e, %.3e"%(res_cost, bend_cost, nr_cost, fval)
#lin_ag += np.random.randn(*lin_ag.shape)*5
#res_cost, bend_cost, nr_cost, fval = tps_nr_cost_eval(lin_ag, trans_g, w_ng, x_na, nr_ma, bend_coef, nr_coef, return_tuple=True)
#print "NOISE ADDED COST, res,bend,nr,total = %.ef, %.3e, %.3e, %.3e"%(res_cost, bend_cost, nr_cost, fval)
_u,_s,_vh = np.linalg.svd(np.c_[x_na, np.ones((N,1))], full_matrices=True)
N_nq = _u[:,4:] # null of data
#w_ng = N_nq.dot(N_nq.T.dot(w_ng))
K_nn = tps_kernel_matrix(x_na)
Q_nn = np.c_[x_na, np.ones((N,1)),K_nn.dot(N_nq)]
QQ_nn = np.dot(Q_nn.T, Q_nn)
Bend_nn = np.zeros((N,N))
Bend_nn[4:, 4:] = - N_nq.T.dot(K_nn.dot(N_nq))
n_iter=60
for i in xrange(n_iter):
X_ng = np.r_[lin_ag, trans_g[None,:], N_nq.T.dot(w_ng)]
res_cost, bend_cost, nr_cost, fval = tps_nr_cost_eval(lin_ag, trans_g, w_ng, x_na, 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_zng = tps_nr_grad(nr_ma, lin_ag, trans_g, w_ng, x_na, return_tuple=True)
nrerr_z = tps_nr_err(nr_ma, lin_ag, trans_g, w_ng, x_na)
if method == "newton":
fullstep_ng = np.empty((N,D))
for g in xrange(D):
J_zn = np.c_[Jl_zcg[:,g::D], Jt_zg[:,g::D], Jw_zng[:,g::D].dot(N_nq)]
JJ_nn = np.dot(J_zn.T, J_zn)
A = nr_coef*JJ_nn + QQ_nn + bend_coef*Bend_nn
X0 = X_ng[:,g]
B = nr_coef*np.dot(J_zn.T, np.dot(J_zn, X0) - nrerr_z) + Q_nn.T.dot(y_ng[:,g])
fullstep_ng[:,g] = np.linalg.solve(A,B) - X_ng[:,g]
elif method == "gradient":
# def eval_partial(cand_X_ng):
# cand_X_ng = cand_X_ng.reshape(-1,3)
# cand_lin_ag, cand_trans_g, cand_w_ng = cand_X_ng[:D], cand_X_ng[D], N_nq.dot(cand_X_ng[D+1:])
# fval_cand = tps_nr_cost_eval(cand_lin_ag, cand_trans_g, cand_w_ng, x_na, y_ng, nr_ma, bend_coef, nr_coef)
# return fval_cand
# def eval_partial2(cand_X_ng):
# return ((Q_nn.dot(X_ng) - y_ng)**2).sum()
# def eval_partial3(cand_X_ng):
# cand_X_ng = cand_X_ng.reshape(-1,3)
# cand_lin_ag, cand_trans_g, cand_w_ng = cand_X_ng[:D], cand_X_ng[D], N_nq.dot(cand_X_ng[D+1:])
# return ((y_ng - tps_eval(x_na, cand_lin_ag, cand_trans_g, cand_w_ng, x_na))**2).sum()
grad_ng = np.empty((N,D))
for g in xrange(D-1,-1,-1):
Jnr_zn = np.c_[Jl_zcg[:,g::D], Jt_zg[:,g::D], Jw_zng[:,g::D].dot(N_nq)]
grad_ng[:,g] = 2 * nr_coef * nrerr_z.dot(Jnr_zn) \
+ 2 * Q_nn.T.dot(Q_nn.dot(X_ng[:,g]) - y_ng[:,g]) \
+ 2 * bend_coef * Bend_nn.dot(X_ng[:,g])
#assert np.allclose(eval_partial2(X_ng), eval_partial3(X_ng))
#assert np.allclose(eval_partial(X_ng), eval_partial2(X_ng))
#grad0_ng = ndt.Gradient(eval_partial)(X_ng.flatten()).reshape(-1,3)
fullstep_ng = -grad_ng
#assert np.allclose(grad0_ng, grad_ng)
cost_improved = False
for stepsize in 3.**np.arange(0,-10,-1):
cand_X_ng = X_ng + fullstep_ng*stepsize
cand_lin_ag, cand_trans_g, cand_w_ng = cand_X_ng[:D], cand_X_ng[D], N_nq.dot(cand_X_ng[D+1:])
fval_cand = tps_nr_cost_eval(cand_lin_ag, cand_trans_g, cand_w_ng, x_na, y_ng, nr_ma, bend_coef, nr_coef)
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_ng = cand_w_ng
return lin_ag, trans_g, w_ng
