def tps_rpm_normals_prior(x_nd, y_md, orig_source = None, orig_target = None, n_iter=20, temp_init=.1, temp_final=.01, bend_init=.1, bend_final=.01,
rot_reg = 1e-5, outlierfrac = 1e-2, wsize = .1, EM_iter = 5, f_init = None, outlierprior = .1, beta = 1, plotting = False,
normal_weight_init = 5, normal_weight_final = .5):
#RPM with normlas as prior
_,d=x_nd.shape
temps = loglinspace(temp_init, temp_final, n_iter)
bend_coefs = loglinspace(bend_init, bend_final, n_iter)
normal_temps = loglinspace(normal_weight_init, normal_weight_final, n_iter)
if f_init is not None:
f = f_init
else:
f = fit_KrigingSpline(x_nd, x_nd, x_nd, x_nd, x_nd, normal_coef = 0)
# f.trans_g = np.median(y_md,axis=0) - np.median(x_nd,axis=0)
if orig_source is None:
orig_source = x_nd
if orig_target is None:
orig_target = y_md
exs = tps_utils.find_all_normals_naive(x_nd, wsize=wsize, flip_away=True)
eys = tps_utils.find_all_normals_naive(y_md, wsize=wsize, flip_away=True)
for i in xrange(n_iter):
for j in range(EM_iter):
print i, j
f, corr_nm = EM_step_normals(f, x_nd, y_md, exs, eys, outlierfrac, temps[i], bend_coefs[i], normal_temps[i], rot_reg, outlierprior, beta = beta)
if plotting and i%plotting==0:
p_plt.plot_tps_registration(x_nd, y_md, f)
p_plt.plot_tps_registration_normals(x_nd, y_md, exs, eys, f, wsize = .1)
return f, corr_nm
def tps_rpm_EM(x_nd, y_md, n_iter=20, temp_init=.1, temp_final=.01, bend_init=.1, bend_final=.01, rot_reg = 1e-5,
outlierfrac = 1e-2, EM_iter = 5, f_init = None, outlierprior = .1, plotting = False, angle = 0,
square_size = 0, circle_rad = 0, wsize = .1):
# RPM with multiple EM steps as an option
_,d=x_nd.shape
temps = loglinspace(temp_init, temp_final, n_iter)
bend_coefs = loglinspace(bend_init, bend_final, n_iter)
if f_init is not None:
f = f_init
else:
f = ThinPlateSpline(d)
e1 = tps_utils.find_all_normals_naive(x_nd, wsize=wsize, flip_away=True)
e2 = tps_utils.find_all_normals_naive(y_md, wsize=wsize, flip_away=True)
for i in xrange(n_iter):
for j in range(EM_iter):
print i,j
f, corr_nm = EM_step(f, x_nd, y_md, outlierfrac, temps[i], bend_coefs[i], rot_reg, outlierprior)
#ipy.embed()
if plotting and i%plotting==0:
p_plt.plot_tps_registration(x_nd, y_md, f)
p_plt.plot_tps_registration_normals(x_nd, y_md, exs, eys, f, wsize = wsize)
#ipy.embed()
return f, corr_nm
def compute_normals_cost(x_nd, y_md, orig_source, orig_target, wsize):
x_normals = tps_utils.find_all_normals_naive(x_nd, orig_source, wsize)
y_normals = tps_utils.find_all_normals_naive(y_md, orig_target, wsize)
normals_cost = ssd.cdist(x_normals, y_normals, 'sqeuclidean')
return normals_cost
def tps_n_rpm_final_hopefully(x_nd, y_md, exs = None, eys = None, Epts = None, orig_source = None, orig_target = None, n_iter=20, temp_init=.1, temp_final=.01, bend_init=.1, bend_final=.01,
rot_reg = 1e-5, outlierfrac = 1e-2, wsize = .1, EM_iter = 5, f_init = None, outlierprior = .1, beta = 1, plotting = False, jplotting = 0,
normal_coef = .1, normal_temp = .05, flip_away=True):
_,d=x_nd.shape
temps = loglinspace(temp_init, temp_final, n_iter)
bend_coefs = loglinspace(bend_init, bend_final, n_iter)
normal_temp = normal_temp
#ipy.embed()
if f_init is not None:
f = f_init
else:
f = fit_KrigingSpline(x_nd, x_nd, x_nd, x_nd, x_nd, normal_coef = 0)
# f.trans_g = np.median(y_md,axis=0) - np.median(x_nd,axis=0)
if orig_source is None:
orig_source = x_nd
if orig_target is None:
orig_target = y_md
if exs is None:
exs = tps_utils.find_all_normals_naive(x_nd, wsize=wsize, flip_away=flip_away)
if eys is None:
eys = tps_utils.find_all_normals_naive(y_md, wsize=wsize, flip_away=flip_away)
curve_cost = tps_utils.compute_curvature_weights(x_nd, y_md, wsize = wsize)
normal_temp = normal_temp
for i in xrange(n_iter):
if i == n_iter - 1:
print "--------------------------------------"
for j in range(EM_iter):
f, corr_nm, corr_nm_edge = EM_step_final(f, x_nd, y_md, exs, eys, outlierfrac, temps[i], normal_temp, bend_coefs[i], normal_coef, rot_reg, outlierprior, beta = beta, Epts = Epts, wsize = wsize)
if normal_coef != 0:
print tps_n_rpm_obj(f, corr_nm, corr_nm_edge, temps[i], bend_coefs[i], normal_temp, x_nd, y_md, exs, eys, normal_coef)
else:
print tps_rpm_obj(f, corr_nm, temps[i], bend_coefs[i], x_nd, y_md)
else:
print "--------------------------------------"
for j in range(EM_iter):
f, corr_nm = EM_step(f, x_nd, y_md, outlierfrac, temps[i], bend_coefs[i], rot_reg, outlierprior, curve_cost = curve_cost, beta = beta)
print tps_rpm_obj(f, corr_nm, temps[i], bend_coefs[i], x_nd, y_md)
if plotting and i%plotting==0:
p_plt.plot_tps_registration(x_nd, y_md, f)
p_plt.plot_tps_registration_normals(x_nd, y_md, exs, eys, f, wsize = wsize)
targ_nd = (corr_nm/np.sum(corr_nm, axis=1)[:,None]).dot(y_md)
#targ_nd_edge = tps_utils.find_all_normals_naive(y_md, wsize = wsize)
if i == n_iter - 1:
wt_n_edge = corr_nm_edge.sum(axis=1)
targ_nd_edge = (corr_nm_edge/wt_n_edge[:,None]).dot(eys)
targ_nd_edge = tps_utils.normal_corr_mult(corr_nm_edge/wt_n_edge[:,None], eys)
targ_nd_edge = tps_utils.flip_normals(x_nd, x_nd, exs, targ_nd, targ_nd_edge)
p_plt.plot_corr_normals(x_nd, exs, targ_nd, targ_nd_edge)
return f, corr_nm, corr_nm_edge
def calculate_normal_dist(x_na, y_ng, nwsize=0.04):
e_x = tps_utils.find_all_normals_naive(x_na,
nwsize,
flip_away=True,
project_lower_dim=True)
e_y = tps_utils.find_all_normals_naive(y_ng,
nwsize,
flip_away=True,
project_lower_dim=True)
return ssd.cdist(e_x, e_y, 'euclidean')
def tps_fit3_normals(x_na,
y_ng,
bend_coef,
rot_coef,
normal_coef,
wt_n,
nwsize=0.02):
if wt_n is None: wt_n = np.ones(len(x_na))
n, d = x_na.shape
K_nn = tps.tps_kernel_matrix(x_na)
# Generate the normals
e_x = tps_utils.find_all_normals_naive(x_na,
nwsize,
flip_away=True,
project_lower_dim=True)
e_y = tps_utils.find_all_normals_naive(y_ng,
nwsize,
flip_away=True,
project_lower_dim=True)
# Calculate the relevant matrices for Jacobians
if d == 3:
x_diff = np.transpose(x_na[None, :, :] - x_na[:, None, :], (0, 2, 1))
P = e_x.dot(x_diff)[range(n), range(n), :] / (K_nn + 1e-20)
# import IPython
# IPython.embed()
else:
raise NotImplementedError
Q = np.c_[np.ones((n, 1)), x_na, K_nn]
WQ = wt_n[:, None] * Q
QWQ = Q.T.dot(WQ)
H = QWQ
H[d + 1:, d + 1:] += bend_coef * K_nn
rot_coefs = np.ones(d) * rot_coef if np.isscalar(rot_coef) else rot_coef
H[1:d + 1, 1:d + 1] += np.diag(rot_coefs)
## Normals
Qnr = np.c_[np.ones((n, 1)), e_x, P]
WQnr = wt_n[:, None] * Qnr
QWQnr = Qnr.T.dot(WQnr)
H += normal_coef * QWQnr
##
f = -WQ.T.dot(y_ng) - normal_coef * WQnr.T.dot(e_y)
f[1:d + 1, 0:d] -= np.diag(rot_coefs)
A = np.r_[np.zeros((d + 1, d + 1)), np.c_[np.ones((n, 1)), x_na]].T
Theta = tps.solve_eqp1(H, f, A)
return Theta[1:d + 1], Theta[0], Theta[d + 1:]
def calculate_normal_dist2(x_na, y_ng, nwsize=0.04):
e_x = tps_utils.find_all_normals_naive(x_na,
nwsize,
flip_away=True,
project_lower_dim=True)
e_y = tps_utils.find_all_normals_naive(y_ng,
nwsize,
flip_away=True,
project_lower_dim=True)
# import IPython
# IPython.embed()
return -e_x.dot(e_y.T)
def test_normals_new(pts1, pts2=None, reduce_dim=True):
pts1 = clouds.downsample(pts1, 0.02).astype('float64')
if pts1.shape[1] == 3 and reduce_dim:
pts1 = tu.project_lower_dim(pts1)
print pts1.shape
nms1 = tu.find_all_normals_naive(pts1,
wsize=0.15,
flip_away=True,
project_lower_dim=True)
if pts2 is None:
noise = np.random.normal(0, 0.008, pts1.shape[0])
print max(noise)
pts2 = np.dot(pts1, np.array([[0, 1], [-1, 0]
])) + noise[:, None] * nms1
#pts2 = pts1 + noise[:,None]*nms1
nms2 = tu.find_all_normals_naive(pts2,
wsize=0.15,
flip_away=True,
project_lower_dim=False)
else:
#pts2 = clouds.downsample(pts2, 0.02).astype('float64')
if pts2.shape[1] == 3 and reduce_dim:
pts2 = tu.project_lower_dim(pts2)
pts2 = pts2[:pts1.shape[0], :]
print pts1.shape, pts2.shape
print np.c_[pts1, np.zeros((pts1.shape[0], 1))].shape
print np.c_[pts2, np.zeros((pts2.shape[0], 1))].shape
f1 = fit_ThinPlateSpline(pts1,
pts2,
bend_coef=0 * 0.1,
rot_coef=0 * 1e-5,
wt_n=None,
use_cvx=True)
f2 = te.tps_eval(pts1,
pts2,
bend_coef=0.1,
rot_coef=1e-5,
wt_n=None,
nwsize=0.15,
delta=0.001)
import IPython
IPython.embed()
#f2 = te.tps_fit_normals_exact_cvx(pts1, pts2, bend_coef=0.1, rot_coef=1e-5, normal_coef = 1, wt_n=None, nwsize=1.4, delta=0.2)
mlab.figure(1)
mayavi_utils.plot_warping(f1, pts1, pts2, fine=False, draw_plinks=True)
#mlab.show()
mlab.figure(2)
#mlab.clf()
mayavi_utils.plot_warping(f2, pts1, pts2, fine=False, draw_plinks=True)
print 2
mlab.show()
def test_normals_cvx(pts1):
pts1 = clouds.downsample(pts1, 0.02).astype('float64')
nms = tu.find_all_normals_naive(pts1,
wsize=0.15,
flip_away=True,
project_lower_dim=True)
noise = np.random.normal(0, 0.008, pts1.shape[0])
pts2 = pts1 + noise[:, None] * nms
# import IPython
# IPython.embed()
f1 = fit_ThinPlateSpline(pts1,
pts2,
bend_coef=.1,
rot_coef=1e-5,
wt_n=None,
use_cvx=True)
f2 = fit_ThinPlateSpline_normals(pts1,
pts2,
bend_coef=.1,
rot_coef=1e-5,
normal_coef=0.03**2,
wt_n=None,
use_cvx=True,
use_dot=True)
mlab.figure(1)
mayavi_utils.plot_warping(f1, pts1, pts2, fine=False, draw_plinks=True)
#mlab.show()
mlab.figure(2)
#mlab.clf()
mayavi_utils.plot_warping(f2, pts1, pts2, fine=False, draw_plinks=True)
mlab.show()
def find_rope_normals(pts1): normals = find_all_normals_naive(pts1, .15, flip_away=True) avg = np.average(pts1) for ex in normals: if ex[2] < avg: ex[2] = -ex[2] return normals
def tps_fit3_normals(x_na, y_ng, e_x = None, e_y = None, bend_coef = .1, rot_coef = 1e-5, normal_coef = .01, wt_n = None, nwsize=0.02):
if wt_n is None: wt_n = np.ones(len(x_na))
n,d = x_na.shape
K_nn = tps.tps_kernel_matrix(x_na)
# Generate the normals
if e_x is None:
e_x = tps_utils.find_all_normals_naive(x_na, nwsize, flip_away=True, project_lower_dim=True)
if e_y is None:
e_y = tps_utils.find_all_normals_naive(y_ng, nwsize, flip_away=True, project_lower_dim=True)
# Calculate the relevant matrices for Jacobians
if d == 3:
x_diff = np.transpose(x_na[None,:,:] - x_na[:,None,:],(0,2,1))
P = e_x.dot(x_diff)[range(n),range(n),:]/(K_nn+1e-20)
# import IPython
# IPython.embed()
else:
raise NotImplementedError
Q = np.c_[np.ones((n,1)), x_na, K_nn]
WQ = wt_n[:,None] * Q
QWQ = Q.T.dot(WQ)
H = QWQ
H[d+1:,d+1:] += bend_coef * K_nn
rot_coefs = np.ones(d) * rot_coef if np.isscalar(rot_coef) else rot_coef
H[1:d+1, 1:d+1] += np.diag(rot_coefs)
## Normals
Qnr = np.c_[np.ones((n,1)), e_x, P]
WQnr = wt_n[:,None] * Qnr
QWQnr = Qnr.T.dot(WQnr)
H += normal_coef*QWQnr
##
f = -WQ.T.dot(y_ng) - normal_coef*WQnr.T.dot(e_y)
f[1:d+1,0:d] -= np.diag(rot_coefs)
A = np.r_[np.zeros((d+1,d+1)), np.c_[np.ones((n,1)), x_na]].T
Theta = tps.solve_eqp1(H,f,A)
return Theta[1:d+1], Theta[0], Theta[d+1:]
def tps_rpm_double_corr(x_nd, y_md, exs = None, eys = None, orig_source = None, orig_target = None, n_iter=20, temp_init=.1, temp_final=.01, bend_init=.1, bend_final=.01,
rot_reg = 1e-5, outlierfrac = 1e-2, wsize = .1, EM_iter = 5, f_init = None, outlierprior = .1, beta = 1, plotting = False, jplotting = 0,
normal_weight_init = 5, normal_weight_final = .5, normal_coef_init = .001, normal_coef_final = .2, flip_away=True):
_,d=x_nd.shape
temps = loglinspace(temp_init, temp_final, n_iter)
bend_coefs = loglinspace(bend_init, bend_final, n_iter)
edge_temps = loglinspace(normal_weight_init, normal_weight_final, n_iter)
#ipy.embed()
if normal_coef_init + normal_coef_final != 0:
normal_coefs = loglinspace(normal_coef_init, normal_coef_final, n_iter)
else:
normal_coefs = np.zeros(n_iter)
if f_init is not None:
f = f_init
else:
f = fit_KrigingSpline(x_nd, x_nd, x_nd, x_nd, x_nd, normal_coef = 0)
# f.trans_g = np.median(y_md,axis=0) - np.median(x_nd,axis=0)
if orig_source is None:
orig_source = x_nd
if orig_target is None:
orig_target = y_md
if exs is None:
exs = tps_utils.find_all_normals_naive(x_nd, wsize=wsize, flip_away=flip_away)
if eys is None:
eys = tps_utils.find_all_normals_naive(y_md, wsize=wsize, flip_away=flip_away)
curve_cost = compute_curvature_cost(x_nd, y_md, orig_source, orig_target, wsize)
for i in xrange(n_iter):
for j in range(EM_iter):
print i, j
f, corr_nm, corr_nm_edge = EM_step_double_corr(f, x_nd, y_md, exs, eys, outlierfrac, temps[i], edge_temps[i], bend_coefs[i], normal_coefs[i], rot_reg, outlierprior, curve_cost = curve_cost, beta = beta, jplotting = jplotting, i=i, j = j)
if plotting and i%plotting==0:
p_plt.plot_tps_registration(x_nd, y_md, f)
p_plt.plot_tps_registration_normals(x_nd, y_md, exs, eys, f, wsize = wsize)
p_plt.plot_corr_normals(corr_nm, corr_nm_edge, x_nd, exs, y_md, eys)
return f, corr_nm, corr_nm_edge
def tps_n_rpm_final_hopefully(x_nd, y_md, exs = None, eys = None, Epts = None, orig_source = None, orig_target = None, n_iter=20, temp_init=.1, temp_final=.01, bend_init=.1, bend_final=.01,
rot_reg = 1e-5, outlierfrac = 1e-2, wsize = .1, EM_iter = 5, f_init = None, outlierprior = .1, beta = 1, plotting = False, jplotting = 0,
normal_coef = .1, normal_temp = .05, flip_away=True):
_,d=x_nd.shape
temps = loglinspace(temp_init, temp_final, n_iter)
bend_coefs = loglinspace(bend_init, bend_final, n_iter)
normal_temp = normal_temp
#ipy.embed()
if f_init is not None:
f = f_init
else:
f = fit_KrigingSpline(x_nd, x_nd, x_nd, x_nd, x_nd, normal_coef = 0)
# f.trans_g = np.median(y_md,axis=0) - np.median(x_nd,axis=0)
if orig_source is None:
orig_source = x_nd
if orig_target is None:
orig_target = y_md
if exs is None:
exs = tps_utils.find_all_normals_naive(x_nd, wsize=wsize, flip_away=flip_away)
if eys is None:
eys = tps_utils.find_all_normals_naive(y_md, wsize=wsize, flip_away=flip_away)
curve_cost = tps_utils.compute_curvature_weights(x_nd, y_md, wsize = wsize)
normal_temp = normal_temp
for i in xrange(n_iter):
if i == n_iter - 1:
print "--------------------------------------"
for j in range(EM_iter):
f, corr_nm, corr_nm_edge = EM_step_final(f, x_nd, y_md, exs, eys, outlierfrac, temps[i], normal_temp, bend_coefs[i], normal_coef, rot_reg, outlierprior, beta = beta, Epts = Epts, wsize = wsize)
else:
print "--------------------------------------"
for j in range(EM_iter):
f, corr_nm = EM_step(f, x_nd, y_md, outlierfrac, temps[i], bend_coefs[i], rot_reg, outlierprior, curve_cost = curve_cost, beta = beta)
return f, corr_nm, corr_nm_edge
def test_normals_new (pts1, pts2=None, reduce_dim=True):
pts1 = clouds.downsample(pts1, 0.02).astype('float64')
if pts1.shape[1] == 3 and reduce_dim:
pts1 = tu.project_lower_dim(pts1)
print pts1.shape
nms1 = tu.find_all_normals_naive(pts1, wsize=0.15,flip_away=True, project_lower_dim=True)
if pts2 is None:
noise = np.random.normal(0,0.008,pts1.shape[0])
print max(noise)
pts2 = np.dot(pts1,np.array([[0,1], [-1,0]])) + noise[:,None]*nms1
#pts2 = pts1 + noise[:,None]*nms1
nms2 = tu.find_all_normals_naive(pts2, wsize=0.15,flip_away=True, project_lower_dim=False)
else:
#pts2 = clouds.downsample(pts2, 0.02).astype('float64')
if pts2.shape[1] == 3 and reduce_dim:
pts2 = tu.project_lower_dim(pts2)
pts2 = pts2[:pts1.shape[0], :]
print pts1.shape, pts2.shape
print np.c_[pts1,np.zeros((pts1.shape[0],1))].shape
print np.c_[pts2,np.zeros((pts2.shape[0],1))].shape
f1 = fit_ThinPlateSpline(pts1, pts2, bend_coef=0*0.1, rot_coef=0*1e-5, wt_n=None, use_cvx=True)
f2 = te.tps_eval(pts1, pts2, bend_coef=0.1, rot_coef=1e-5, wt_n=None, nwsize=0.15, delta=0.001)
import IPython
IPython.embed()
#f2 = te.tps_fit_normals_exact_cvx(pts1, pts2, bend_coef=0.1, rot_coef=1e-5, normal_coef = 1, wt_n=None, nwsize=1.4, delta=0.2)
mlab.figure(1)
mayavi_utils.plot_warping(f1, pts1, pts2, fine=False, draw_plinks=True)
#mlab.show()
mlab.figure(2)
#mlab.clf()
mayavi_utils.plot_warping(f2, pts1, pts2, fine=False, draw_plinks=True)
print 2
mlab.show()
def test_normals_pts (pts, nms = None, wsize = None, delta=0.01, scale_factor=0.01,show=False):
"""
Test normals.
"""
if wsize is None: wsize = 0.02
if nms is None:
nms = tu.find_all_normals_naive(pts,wsize=wsize,flip_away=True,project_lower_dim=True)
pts_nm = pts + nms*delta
lines = [np.c_[p1,p2].T for p1,p2 in zip(pts,pts_nm)]
if show:
mayavi_utils.disp_pts(pts, pts_nm, scale_factor=scale_factor)
mayavi_utils.plot_lines(lines)
if show:
mlab.show()
def test_normals():
"""
Test normals.
"""
x = np.arange(100).astype('f') / 10.0
y = np.sin(x)
xy = np.r_[np.atleast_2d(x), np.atleast_2d(y)].T
nms = tu.find_all_normals_naive(xy, wsize=0.15, flip_away=True)
xy_nm = xy + nms * 0.2
z = x * 0
xyz = np.c_[xy, z]
xyz_nm = np.c_[xy_nm, z]
lines = [np.c_[p1, p2].T for p1, p2 in zip(xyz, xyz_nm)]
mayavi_utils.disp_pts(xyz, xyz_nm)
mayavi_utils.plot_lines(lines)
mlab.show()
def test_normals ():
"""
Test normals.
"""
x = np.arange(100).astype('f')/10.0
y = np.sin(x)
xy = np.r_[np.atleast_2d(x),np.atleast_2d(y)].T
nms = tu.find_all_normals_naive(xy,wsize=0.15,flip_away=True)
xy_nm = xy + nms*0.2
z = x*0
xyz = np.c_[xy,z]
xyz_nm = np.c_[xy_nm,z]
lines = [np.c_[p1,p2].T for p1,p2 in zip(xyz,xyz_nm)]
mayavi_utils.disp_pts(xyz, xyz_nm)
mayavi_utils.plot_lines(lines)
mlab.show()
def test_normals_cvx (pts1):
pts1 = clouds.downsample(pts1, 0.02).astype('float64')
nms = tu.find_all_normals_naive(pts1, wsize=0.15,flip_away=True, project_lower_dim=True)
noise = np.random.normal(0,0.008,pts1.shape[0])
pts2 = pts1 + noise[:,None]*nms
# import IPython
# IPython.embed()
f1 = fit_ThinPlateSpline(pts1, pts2, bend_coef=.1, rot_coef = 1e-5, wt_n=None, use_cvx = True)
f2 = fit_ThinPlateSpline_normals(pts1, pts2, bend_coef=.1, rot_coef = 1e-5, normal_coef = 0.03**2, wt_n=None, use_cvx = True, use_dot=True)
mlab.figure(1)
mayavi_utils.plot_warping(f1, pts1, pts2, fine=False, draw_plinks=True)
#mlab.show()
mlab.figure(2)
#mlab.clf()
mayavi_utils.plot_warping(f2, pts1, pts2, fine=False, draw_plinks=True)
mlab.show()
def test_normals3():
"""
Test normals.
"""
angs = np.linspace(0, np.pi, 100)
x = np.sin(angs) + np.random.random((1, angs.shape[0])) * 0.05
y = np.cos(angs) + np.random.random((1, angs.shape[0])) * 0.05
xy = np.r_[np.atleast_2d(x), np.atleast_2d(y)].T
nms = tu.find_all_normals_naive(xy, wsize=0.15, flip_away=True)
xy_nm = xy + nms * 0.03
z = angs * 0
xyz = np.c_[xy, z]
xyz_nm = np.c_[xy_nm, z]
lines = [np.c_[p1, p2].T for p1, p2 in zip(xyz, xyz_nm)]
mayavi_utils.disp_pts(xyz, xyz_nm)
mayavi_utils.plot_lines(lines)
mlab.show()
def test_normals3 ():
"""
Test normals.
"""
angs = np.linspace(0, np.pi, 100)
x = np.sin(angs) + np.random.random((1,angs.shape[0]))*0.05
y = np.cos(angs) + np.random.random((1,angs.shape[0]))*0.05
xy = np.r_[np.atleast_2d(x),np.atleast_2d(y)].T
nms = tu.find_all_normals_naive(xy,wsize=0.15,flip_away=True)
xy_nm = xy + nms*0.03
z = angs*0
xyz = np.c_[xy,z]
xyz_nm = np.c_[xy_nm,z]
lines = [np.c_[p1,p2].T for p1,p2 in zip(xyz,xyz_nm)]
mayavi_utils.disp_pts(xyz, xyz_nm)
mayavi_utils.plot_lines(lines)
mlab.show()
def test_normals_pts(pts,
nms=None,
wsize=None,
delta=0.01,
scale_factor=0.01,
show=False):
"""
Test normals.
"""
if wsize is None: wsize = 0.02
if nms is None:
nms = tu.find_all_normals_naive(pts,
wsize=wsize,
flip_away=True,
project_lower_dim=True)
pts_nm = pts + nms * delta
lines = [np.c_[p1, p2].T for p1, p2 in zip(pts, pts_nm)]
if show:
mayavi_utils.disp_pts(pts, pts_nm, scale_factor=scale_factor)
mayavi_utils.plot_lines(lines)
if show:
mlab.show()
def tps_rpm_bij_normals_naive(x_nd, y_md, n_iter = 20, reg_init = .1, reg_final = .001, rad_init = .1, rad_final = .005, rot_reg = 1e-3,
nwsize = None, neps = None, plotting = False, plot_cb = None):
"""
tps-rpm algorithm mostly as described by chui and rangaran
Adding points for normals to fit tps to.
Nothing fancy, just the baseline.
reg_init/reg_final: regularization on curvature
rad_init/rad_final: radius for correspondence calculation (meters)
plotting: 0 means don't plot. integer n means plot every n iterations
"""
_,d=x_nd.shape
regs = loglinspace(reg_init, reg_final, n_iter)
rads = loglinspace(rad_init, rad_final, n_iter)
f = ThinPlateSpline(d)
f.trans_g = np.median(y_md,axis=0) - np.median(x_nd,axis=0)
g = ThinPlateSpline(d)
g.trans_g = -f.trans_g
# Calculating window sizes to find normals for points
ndx = nlg.norm(x_nd.min(axis=0)-x_nd.max(axis=0))/x_nd.shape[0]
ndy = nlg.norm(y_md.min(axis=0)-y_md.max(axis=0))/y_md.shape[0]
nd = (ndx + ndy)/2
if nwsize is None: nwsize = nd*2
if neps is None:
neps = nd/2
else:
ndx = neps
ndy = neps
# Adds all normal points, at some small distance from points
x_nms = x_nd + ndx/2*tps_utils.find_all_normals_naive(x_nd, ndx*2, flip_away= True, project_lower_dim=True)
y_nms = y_md + ndy/2*tps_utils.find_all_normals_naive(y_md, ndy*2, flip_away= True, project_lower_dim=True)
# The points to fit tps with
xpts = np.r_[x_nd,x_nms]
ypts = np.r_[y_md,y_nms]
# r_N = None
for i in xrange(n_iter):
xwarped_nd = f.transform_points(x_nd)
ywarped_md = g.transform_points(y_md)
fwddist_nm = ssd.cdist(xwarped_nd, y_md,'euclidean')
invdist_nm = ssd.cdist(x_nd, ywarped_md,'euclidean')
r = rads[i]
prob_nm = np.exp( -(fwddist_nm + invdist_nm) / (2*r) )
corr_nm, r_N, _ = balance_matrix3(prob_nm, 10, 1e-1, 2e-1)
corr_nm += 1e-9
wt_n = corr_nm.sum(axis=1)
wt_m = corr_nm.sum(axis=0)
xtarg_nd = (corr_nm/wt_n[:,None]).dot(y_md)
ytarg_md = (corr_nm/wt_m[None,:]).T.dot(x_nd)
xt_nms = xtarg_nd + neps*tps_utils.find_all_normals_naive(xtarg_nd, nwsize, flip_away=True, project_lower_dim=True)
yt_nms = ytarg_md + neps*tps_utils.find_all_normals_naive(ytarg_md, nwsize, flip_away=True, project_lower_dim=True)
xtarg_pts = np.r_[xtarg_nd,xt_nms]
ytarg_pts = np.r_[ytarg_md,yt_nms]
wt_n_nm = np.r_[wt_n,wt_n]#/2
wt_m_nm = np.r_[wt_m,wt_m]#/2
if plotting and i%plotting==0 and plot_cb is not None:
plot_cb(x_nd, y_md, xtarg_pts, corr_nm, wt_n, f)
f = fit_ThinPlateSpline(xpts, xtarg_pts, bend_coef = regs[i], wt_n=wt_n_nm, rot_coef = rot_reg)
g = fit_ThinPlateSpline(ypts, ytarg_pts, bend_coef = regs[i], wt_n=wt_m_nm, rot_coef = rot_reg)
f._cost = tps.tps_cost(f.lin_ag, f.trans_g, f.w_ng, f.x_na, xtarg_pts, regs[i], wt_n=wt_n_nm)/wt_n_nm.mean()
g._cost = tps.tps_cost(g.lin_ag, g.trans_g, g.w_ng, g.x_na, ytarg_pts, regs[i], wt_n=wt_m_nm)/wt_m_nm.mean()
return f,g
def tps_fit3_normals_cvx(x_na, y_ng, e_x = None, e_y = None, bend_coef = .1, rot_coef = 1e-5, normal_coef = .01, wt_n = None, nwsize=0.02, use_dot=False):
if wt_n is None: wt_n = np.ones(len(x_na))
n,d = x_na.shape
K_nn = tps.tps_kernel_matrix(x_na)
_,_,VT = nlg.svd(np.c_[x_na,np.ones((x_na.shape[0],1))].T)
Nmat = VT.T[:,d+1:]
rot_coefs = np.diag(np.ones(d) * rot_coef if np.isscalar(rot_coef) else rot_coef)
# Generate the normals
if e_x is None:
e_x = tps_utils.find_all_normals_naive(x_na, nwsize, flip_away=True, project_lower_dim=True)
if e_y is None:
e_y = tps_utils.find_all_normals_naive(y_ng, nwsize, flip_away=True, project_lower_dim=True)
if d == 3:
x_diff = np.transpose(x_na[None,:,:] - x_na[:,None,:],(0,2,1))
Pmat = e_x.dot(x_diff)[range(n),range(n),:]/(K_nn+1e-20)
else:
raise NotImplementedError
A = cp.Variable(Nmat.shape[1],d)
B = cp.Variable(d,d)
c = cp.Variable(d,1)
X = co.matrix(x_na)
Y = co.matrix(y_ng)
EX = co.matrix(e_x)
EY = co.matrix(e_y)
K = co.matrix(K_nn)
N = co.matrix(Nmat)
P = co.matrix(Pmat)
W = co.matrix(np.diag(wt_n))
R = co.matrix(rot_coefs)
ones = co.matrix(np.ones((n,1)))
constraints = []
# For correspondences
V1 = cp.Variable(n,d)
constraints.append(V1 == Y-K*N*A-X*B - ones*c.T)
V2 = cp.Variable(n,d)
constraints.append(V2 == cp.sqrt(W)*V1)
# For normals
if use_dot:
# import IPython
# IPython.embed()
N1 = cp.Variable(n,n)
constraints.append(N1 == (P*N*A-EX*B)*EY.T)
# N2 = cp.Variable(n)
# constraints.extend([N2[i] == N1[i,i] for i in xrange(n)])
else:
N1 = cp.Variable(n,d)
constraints.append(N1 == EY-P*N*A-EX*B)
N2 = cp.Variable(n,d)
constraints.append(N2 == cp.sqrt(W)*N1)
# For bending cost
Vb = []
Q = [] # for quadratic forms
for i in range(d):
Vb.append(cp.Variable(Nmat.shape[1],1))
constraints.append(Vb[-1] == A[:,i])
Q.append(cp.quad_form(Vb[-1], N.T*K*N))
# For rotation cost
V3 = cp.Variable(d,d)
constraints.append(V3 == cp.sqrt(R)*B)
# Orthogonality constraints for bending
constraints.extend([X.T*A == 0, ones.T*A == 0])
# TPS objective
if use_dot:
objective = cp.Minimize(cp.sum_squares(V2) - normal_coef*sum([N1[i,i] for i in xrange(n)])
+ bend_coef*sum(Q) + cp.sum_squares(V3))
else:
objective = cp.Minimize(cp.sum_squares(V2) + normal_coef*cp.sum_squares(N2) + bend_coef*sum(Q) + cp.sum_squares(V3))
p = cp.Problem(objective, constraints)
p.solve()
# import IPython
# IPython.embed()
return np.array(B.value), np.squeeze(np.array(c.value)) , np.array(A.valuefi)
def main():
from tn_testing.test_tps import gen_half_sphere, gen_half_sphere_pulled_in, gen_house, gen_box_circle, gen_circle_points
from tn_eval.tps_utils import find_all_normals_naive
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
from tn_testing import grabs_two
from tn_rapprentice.plotting_plt import plot_house, plot_box_circle
from big_rope_pcloud import old_cloud, new_cloud
from tn_rapprentice.final_test_case import x_nd, y_md, Exs, Eys, Epts
"""
pts1 = gen_half_sphere(1, 30)
pts1r = np.random.permutation(pts1)
pts2 = gen_half_sphere_pulled_in(1, 30, 20, .5)
e1 = find_all_normals_naive(pts1, wsize = .7,flip_away=True)
e1r = find_all_normals_naive(pts1r, wsize = .7, flip_away=True)
e2 = find_all_normals_naive(pts2, wsize = .7, flip_away=True)
pts1rr = rotate_point_cloud3d(pts1, 90)
e1rr = find_all_normals_naive(pts1rr, wsize=.7, flip_away=True)
pts2r = rotate_point_cloud3d(pts1, 90)
"""
# ipy.embed()
plt.ion()
number_points = 30
square_size1 = 1
square_size2 = 1
circle_rad1 = .5
circle_rad2 = .5
angle = 0
x0s, x1s, x2s, x3s, x4s = np.array([0,0]), np.array([1,0]), np.array([1,1]), np.array([.5, 3]), np.array([0,1])
x0t, x1t, x2t, x3t, x4t = np.array([0,0]), np.array([1,0]), np.array([1,.5]), np.array([.5, 1.5]), np.array([0,.5])
pts1 = gen_house(x0s, x1s, x2s, x3s, x4s, number_points)
pts2 = gen_house(x0t, x1t, x2t, x3t, x4t, number_points)
#pts1 = gen_box_circle(square_size1, circle_rad1, number_points = number_points)
#pts2 = gen_box_circle(square_size2, circle_rad2, number_points = number_points)
#pts1 = gen_circle_points(.5, 30)
#pts2 = gen_circle_points(.5, 30)
#pts1 = grabs_two.old_cloud[:,:2]
#pts2 = grabs_two.new_cloud[:,:2]
#pts1 = x_nd
#pts2 = y_md
#pts1r = np.random.permutation(pts1)
pts1r = rotate_point_cloud2d(pts1, angle)
EM_iter = 5
beta = 1 #20 works for 90 rotation
jplotting = 0
plotting = 1
temp_init = 1
temp_final = .0005
bend_init = 1e5 #1e2 works for 90 rotation
bend_final = 1e-1
wsize = .2
normal_coef = 1e-1
normal_temp = .001
exs = find_all_normals_naive(pts1r, wsize = wsize, flip_away=True)
eys = find_all_normals_naive(pts2, wsize = wsize, flip_away=True)
#exs = Exs
#eys = Eys
#ipy.embed()
n = 8 , #'I'
if 1 in n:
f1 , corr1 = tps_rpm_curvature_prior1(pts1r, pts2, n_iter = 20, EM_iter = EM_iter, temp_init = temp_init, temp_final = temp_final, bend_init = bend_init, bend_final = bend_final, wsize = wsize, beta = beta, plotting = plotting, angle = angle, square_size = square_size1, circle_rad = circle_rad1)
#plot_house(f1, x0sr, x1sr, x2sr, x3sr, x4sr, number_points)
#plot_box_circle(f1, square_size1, circle_rad1, angle, number_points = number_points)
#plot_grabs_two(f1, pts1r, angle)
p_plt.plot_tps_registration(pts1r, pts2, f1)
if 3 in n:
f3, corr3 = tps_rpm_bij(pts1r,pts2, reg_init = temp_init, rad_init = bend_init)
plot_house(f3, x0sr, x1sr, x2sr, x3sr, x4sr, number_points)
#plot_box_circle(f3, square_size1, circle_rad1, angle, number_points = number_points)
#plot_grabs_two(f3, pts1r, angle)
if 4 in n:
f4,corr4 = tps_rpm_EM(pts1r, pts2, n_iter = 20, EM_iter = EM_iter, temp_init = temp_init, temp_final = temp_final, bend_init = bend_init, bend_final = bend_final, plotting = plotting, wsize = wsize)
#plot_house(f4, x0sr, x1sr, x2sr, x3sr, x4sr, number_points)
#plot_box_circle(f4, square_size1, circle_rad1, angle, number_points = number_points)
p_plt.plot_tps_registration(pts1r, pts2, f4)
if 8 in n:
f8, corr8, corr_edge8 = tps_n_rpm_final_hopefully(pts1r, pts2, n_iter = 20, EM_iter = EM_iter, temp_init = temp_init, temp_final=temp_final, normal_temp = normal_temp, bend_init = bend_init, bend_final = bend_final, normal_coef = normal_coef, plotting = plotting, jplotting = jplotting, beta = beta, wsize = wsize)
#plot_house(f8, x0sr, x1sr, x2sr, x3sr, x4sr, number_points)
plot_box_circle(f8, square_size1, circle_rad1, angle, number_points = number_points)
#plot_grabs_two(f6, pts1r, angle)
#p_plt.plot_tps_registration(pts1r, pts2, f8)
if 'I' in n:
g = ThinPlateSpline(2)
#plot_house(g, x0t, x1t, x2t, x3t, x4t, number_points)
plot_box_circle(g, square_size1, circle_rad1, 0, number_points = number_points)
#plot_grabs_two(g, pts2, 0)
plt.show()
ipy.embed()
def tps_rpm_bij_normals_max2(x_nd, y_md, n_iter = 20, reg_init = .1, reg_final = .001, rad_init = .1, rad_final = .005, rot_reg = 1e-3, normal_coef=0.0001,
nwsize=.15, plotting = False, plot_cb = None):
"""
tps-rpm algorithm mostly as described by chui and rangaran
reg_init/reg_final: regularization on curvature
rad_init/rad_final: radius for correspondence calculation (meters)
plotting: 0 means don't plot. integer n means plot every n iterations
"""
_,d=x_nd.shape
regs = loglinspace(reg_init, reg_final, n_iter)
rads = loglinspace(rad_init, rad_final, n_iter)
f = ThinPlateSpline(d)
f.trans_g = np.median(y_md,axis=0) - np.median(x_nd,axis=0)
g = ThinPlateSpline(d)
g.trans_g = -f.trans_g
# r_N = None
for i in xrange(n_iter):
xwarped_nd = f.transform_points(x_nd)
ywarped_md = g.transform_points(y_md)
fwddist_nm = ssd.cdist(xwarped_nd, y_md,'euclidean')
invdist_nm = ssd.cdist(x_nd, ywarped_md,'euclidean')
r = rads[i]
prob_nm = np.exp( -(fwddist_nm + invdist_nm / (2*r)))
corr_nm, r_N, _ = balance_matrix3(prob_nm, 10, 1e-1, 2e-1)
corr_nm += 1e-9
wt_n = corr_nm.sum(axis=1)
wt_m = corr_nm.sum(axis=0)
wt_n = np.array(wt_n, dtype='float')
wt_m = np.array(wt_m, dtype='float')
xtarg_nd = (corr_nm/wt_n[:,None]).dot(y_md)
ytarg_md = (corr_nm/wt_m[None,:]).T.dot(x_nd)
# if plotting and i%plotting==0 and plot_cb is not None:
# plot_cb(x_nd, y_md, xtarg_nd, corr_nm, wt_n, f)
# f = fit_ThinPlateSpline_normals(x_nd, xtarg_nd, bend_coef = regs[i], wt_n=wt_n, rot_coef = rot_reg, normal_coef=normal_coef, nwsize = nwsize)
# g = fit_ThinPlateSpline_normals(y_md, ytarg_md, bend_coef = regs[i], wt_n=wt_m, rot_coef = rot_reg, normal_coef=normal_coef, nwsize = nwsize)
f = fit_ThinPlateSpline(x_nd, xtarg_nd, bend_coef = regs[i], wt_n=wt_n, rot_coef = rot_reg)#, normal_coef=normal_coef, nwsize = nwsize)
g = fit_ThinPlateSpline(y_md, ytarg_md, bend_coef = regs[i], wt_n=wt_m, rot_coef = rot_reg)#, normal_coef=normal_coef, nwsize = nwsize)
e_x = tps_utils.find_all_normals_naive(x_nd, nwsize, flip_away = True, project_lower_dim=(d==3))
e_y = tps_utils.find_all_normals_naive(y_md, nwsize, flip_away = True, project_lower_dim=(d==3))
e_xt = tps_utils.find_all_normals_naive(xtarg_nd, nwsize, flip_away = True, project_lower_dim=(d==3))
e_yt = tps_utils.find_all_normals_naive(ytarg_md, nwsize, flip_away = True, project_lower_dim=(d==3))
f = tps_eval(x_nd, xtarg_nd, e_x, e_xt, bend_coef = regs[i], wt_n=wt_n, rot_coef = rot_reg)
g = tps_eval(y_md, ytarg_md, e_y, e_yt, bend_coef = regs[i], wt_n=wt_m, rot_coef = rot_reg)
# f._cost = tps.tps_cost(f.lin_ag, f.trans_g, f.w_ng, f.x_na, xtarg_nd, regs[i], wt_n=wt_n)/wt_n.mean()
# g._cost = tps.tps_cost(g.lin_ag, g.trans_g, g.w_ng, g.x_na, ytarg_md, regs[i], wt_n=wt_m)/wt_m.mean()
return f,g
def calculate_normal_dist2 (x_na, y_ng, nwsize=0.04):
e_x = tps_utils.find_all_normals_naive(x_na, nwsize, flip_away=True, project_lower_dim=True)
e_y = tps_utils.find_all_normals_naive(y_ng, nwsize, flip_away=True, project_lower_dim=True)
# import IPython
# IPython.embed()
return -e_x.dot(e_y.T)
def tps_fit3_normals_cvx(x_na,
y_ng,
bend_coef,
rot_coef,
normal_coef,
wt_n,
nwsize=0.02,
use_dot=False):
if wt_n is None: wt_n = np.ones(len(x_na))
n, d = x_na.shape
K_nn = tps.tps_kernel_matrix(x_na)
_, _, VT = nlg.svd(np.c_[x_na, np.ones((x_na.shape[0], 1))].T)
Nmat = VT.T[:, d + 1:]
rot_coefs = np.diag(
np.ones(d) * rot_coef if np.isscalar(rot_coef) else rot_coef)
# Generate the normals
e_x = tps_utils.find_all_normals_naive(x_na,
nwsize,
flip_away=True,
project_lower_dim=True)
e_y = tps_utils.find_all_normals_naive(y_ng,
nwsize,
flip_away=True,
project_lower_dim=True)
if d == 3:
x_diff = np.transpose(x_na[None, :, :] - x_na[:, None, :], (0, 2, 1))
Pmat = e_x.dot(x_diff)[range(n), range(n), :] / (K_nn + 1e-20)
else:
raise NotImplementedError
A = cp.Variable(Nmat.shape[1], d)
B = cp.Variable(d, d)
c = cp.Variable(d, 1)
X = co.matrix(x_na)
Y = co.matrix(y_ng)
EX = co.matrix(e_x)
EY = co.matrix(e_y)
K = co.matrix(K_nn)
N = co.matrix(Nmat)
P = co.matrix(Pmat)
W = co.matrix(np.diag(wt_n))
R = co.matrix(rot_coefs)
ones = co.matrix(np.ones((n, 1)))
constraints = []
# For correspondences
V1 = cp.Variable(n, d)
constraints.append(V1 == Y - K * N * A - X * B - ones * c.T)
V2 = cp.Variable(n, d)
constraints.append(V2 == cp.sqrt(W) * V1)
# For normals
if use_dot:
# import IPython
# IPython.embed()
N1 = cp.Variable(n, n)
constraints.append(N1 == (P * N * A - EX * B) * EY.T)
# N2 = cp.Variable(n)
# constraints.extend([N2[i] == N1[i,i] for i in xrange(n)])
else:
N1 = cp.Variable(n, d)
constraints.append(N1 == EY - P * N * A - EX * B)
N2 = cp.Variable(n, d)
constraints.append(N2 == cp.sqrt(W) * N1)
# For bending cost
Vb = []
Q = [] # for quadratic forms
for i in range(d):
Vb.append(cp.Variable(Nmat.shape[1], 1))
constraints.append(Vb[-1] == A[:, i])
Q.append(cp.quad_form(Vb[-1], N.T * K * N))
# For rotation cost
V3 = cp.Variable(d, d)
constraints.append(V3 == cp.sqrt(R) * B)
# Orthogonality constraints for bending
constraints.extend([X.T * A == 0, ones.T * A == 0])
# TPS objective
if use_dot:
objective = cp.Minimize(
cp.sum_squares(V2) -
normal_coef * sum([N1[i, i] for i in xrange(n)]) +
bend_coef * sum(Q) + cp.sum_squares(V3))
else:
objective = cp.Minimize(
cp.sum_squares(V2) + normal_coef * cp.sum_squares(N2) +
bend_coef * sum(Q) + cp.sum_squares(V3))
p = cp.Problem(objective, constraints)
p.solve()
# import IPython
# IPython.embed()
return np.array(B.value), np.squeeze(np.array(c.value)), np.array(A.value)
def calculate_normal_dist (x_na, y_ng, nwsize=0.04):
e_x = tps_utils.find_all_normals_naive(x_na, nwsize, flip_away=True, project_lower_dim=True)
e_y = tps_utils.find_all_normals_naive(y_ng, nwsize, flip_away=True, project_lower_dim=True)
return ssd.cdist(e_x,e_y,'euclidean')
def tps_rpm_bij_normals_naive(x_nd,
y_md,
n_iter=20,
reg_init=.1,
reg_final=.001,
rad_init=.1,
rad_final=.005,
rot_reg=1e-3,
nwsize=None,
neps=None,
plotting=False,
plot_cb=None):
"""
tps-rpm algorithm mostly as described by chui and rangaran
Adding points for normals to fit tps to.
Nothing fancy, just the baseline.
reg_init/reg_final: regularization on curvature
rad_init/rad_final: radius for correspondence calculation (meters)
plotting: 0 means don't plot. integer n means plot every n iterations
"""
_, d = x_nd.shape
regs = loglinspace(reg_init, reg_final, n_iter)
rads = loglinspace(rad_init, rad_final, n_iter)
f = ThinPlateSpline(d)
f.trans_g = np.median(y_md, axis=0) - np.median(x_nd, axis=0)
g = ThinPlateSpline(d)
g.trans_g = -f.trans_g
# Calculating window sizes to find normals for points
ndx = nlg.norm(x_nd.min(axis=0) - x_nd.max(axis=0)) / x_nd.shape[0]
ndy = nlg.norm(y_md.min(axis=0) - y_md.max(axis=0)) / y_md.shape[0]
nd = (ndx + ndy) / 2
if nwsize is None: nwsize = nd * 2
if neps is None:
neps = nd / 2
else:
ndx = neps
ndy = neps
# Adds all normal points, at some small distance from points
x_nms = x_nd + ndx / 2 * tps_utils.find_all_normals_naive(
x_nd, ndx * 2, flip_away=True, project_lower_dim=True)
y_nms = y_md + ndy / 2 * tps_utils.find_all_normals_naive(
y_md, ndy * 2, flip_away=True, project_lower_dim=True)
# The points to fit tps with
xpts = np.r_[x_nd, x_nms]
ypts = np.r_[y_md, y_nms]
# r_N = None
for i in xrange(n_iter):
xwarped_nd = f.transform_points(x_nd)
ywarped_md = g.transform_points(y_md)
fwddist_nm = ssd.cdist(xwarped_nd, y_md, 'euclidean')
invdist_nm = ssd.cdist(x_nd, ywarped_md, 'euclidean')
r = rads[i]
prob_nm = np.exp(-(fwddist_nm + invdist_nm) / (2 * r))
corr_nm, r_N, _ = balance_matrix3(prob_nm, 10, 1e-1, 2e-1)
corr_nm += 1e-9
wt_n = corr_nm.sum(axis=1)
wt_m = corr_nm.sum(axis=0)
xtarg_nd = (corr_nm / wt_n[:, None]).dot(y_md)
ytarg_md = (corr_nm / wt_m[None, :]).T.dot(x_nd)
xt_nms = xtarg_nd + neps * tps_utils.find_all_normals_naive(
xtarg_nd, nwsize, flip_away=True, project_lower_dim=True)
yt_nms = ytarg_md + neps * tps_utils.find_all_normals_naive(
ytarg_md, nwsize, flip_away=True, project_lower_dim=True)
xtarg_pts = np.r_[xtarg_nd, xt_nms]
ytarg_pts = np.r_[ytarg_md, yt_nms]
wt_n_nm = np.r_[wt_n, wt_n] #/2
wt_m_nm = np.r_[wt_m, wt_m] #/2
if plotting and i % plotting == 0 and plot_cb is not None:
plot_cb(x_nd, y_md, xtarg_pts, corr_nm, wt_n, f)
f = fit_ThinPlateSpline(xpts,
xtarg_pts,
bend_coef=regs[i],
wt_n=wt_n_nm,
rot_coef=rot_reg)
g = fit_ThinPlateSpline(ypts,
ytarg_pts,
bend_coef=regs[i],
wt_n=wt_m_nm,
rot_coef=rot_reg)
f._cost = tps.tps_cost(
f.lin_ag, f.trans_g, f.w_ng, f.x_na, xtarg_pts, regs[i],
wt_n=wt_n_nm) / wt_n_nm.mean()
g._cost = tps.tps_cost(
g.lin_ag, g.trans_g, g.w_ng, g.x_na, ytarg_pts, regs[i],
wt_n=wt_m_nm) / wt_m_nm.mean()
return f, g
