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)
