def test_cvx(pts1):
pts1 = clouds.downsample(pts1, 0.02).astype('float64')
pts2 = np.random.normal(0, 0.004, pts1.shape) + pts1
f1 = fit_ThinPlateSpline(pts1,
pts2,
bend_coef=.1,
rot_coef=1e-5,
wt_n=None,
use_cvx=False)
f2 = fit_ThinPlateSpline(pts1,
pts2,
bend_coef=.1,
rot_coef=1e-5,
wt_n=None,
use_cvx=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_normals_new2():
pts1 = gen_circle_points(0.5, 30)
pts2 = gen_circle_points_pulled_in(
0.5, 30, 4, 0.2) #gen_circle_points(0.5, 30) + np.array([0.1,0.1])
#test_normals_pts(np.c_[pts2,np.zeros((pts2.shape[0],1))], wsize=0.15,delta=0.15)
# 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.1,
rot_coef=1e-5,
wt_n=None,
use_cvx=True)
f2 = fit_ThinPlateSpline(pts1,
pts2,
bend_coef=0.1,
rot_coef=1e-5,
wt_n=None,
use_cvx=False)
import IPython
IPython.embed()
mlab.figure(1)
mayavi_utils.plot_warping(f1, pts1, pts2, fine=False, draw_plinks=True)
mlab.show()
def tps_rpm_bij_normals(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=0.04, plotting = False, plot_cb = None, pts1 = 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')
fwddist_normals_nm = calculate_normal_dist2(xwarped_nd, y_md,nwsize)
invdist_nm = ssd.cdist(x_nd, ywarped_md,'euclidean')
invdist_normals_nm = calculate_normal_dist2(x_nd, ywarped_md, nwsize)
#import IPython
#IPython.embed()
r = rads[i]
prob_nm = np.exp( -(fwddist_nm + invdist_nm + normal_coef*(fwddist_normals_nm + invdist_normals_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)
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)
# print (f.transform_points(pts1))
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 test_normals_new4 (n=2,l=0.5,dim=2):
pts1, pts2, e1, e2 = create_flap_points_normals(n,l,dim)
delta = 1e-2
f1 = fit_ThinPlateSpline(pts1, pts2, bend_coef=0.1, rot_coef=1e-5, wt_n=None, use_cvx=True)
#f1 = te.tps_fit_normals_cvx(pts1, pts2, e1, e2, bend_coef=0.1, rot_coef=1e-5, normal_coef=0.1, wt_n=None, nwsize=0.15, delta=0.0001)
#f2 = fit_ThinPlateSpline(pts1, pts2, bend_coef=0.1, rot_coef=1e-5, wt_n=None, use_cvx=True)
#f2 = te.tps_eval(pts1, pts2, e1, e2, bend_coef=0.0, rot_coef=1e-5, wt_n=None, nwsize=0.15, delta=1e-8)
#f2 = te.tps_fit_normals_cvx(pts1, pts2, bend_coef=0.1, rot_coef=1e-5, normal_coef=10, wt_n=None, nwsize=0.15, delta=1e-6)
f2 = te.tps_fit_normals_cvx(pts1, pts2, e1, e2, bend_coef=0.0, rot_coef=1e-5, normal_coef=1, wt_n=None, nwsize=0.15, delta=delta)
mlab.figure(1, bgcolor=(0,0,0))
mayavi_utils.plot_warping(f1, pts1, pts2, fine=False, draw_plinks=False)
_,f1e2 = te.transformed_normal_direction(pts1, e1, f1, delta=delta)#np.asarray([tu.tps_jacobian(f2, pt, 2).dot(nm) for pt,nm in zip(pts1,e1)])
test_normals_pts(np.c_[f1.transform_points(pts1),np.zeros((pts2.shape[0],1))], np.c_[f1e2,np.zeros((f1e2.shape[0],1))], wsize=0.15,delta=0.15)
test_normals_pts(np.c_[pts2,np.zeros((pts2.shape[0],1))], np.c_[e2,np.zeros((e2.shape[0],1))], wsize=0.15,delta=0.15)
#mlab.show()
mlab.figure(2,bgcolor=(0,0,0))
#mlab.clf()
mayavi_utils.plot_warping(f2, pts1, pts2, fine=False, draw_plinks=False)
_,f2e2 = te.transformed_normal_direction(pts1, e1, f2, delta=delta)
test_normals_pts(np.c_[f2.transform_points(pts1),np.zeros((pts2.shape[0],1))], np.c_[f2e2,np.zeros((f2e2.shape[0],1))], wsize=0.15,delta=0.15)
test_normals_pts(np.c_[pts2,np.zeros((pts2.shape[0],1))], np.c_[e2,np.zeros((e2.shape[0],1))], wsize=0.15,delta=0.15)
mlab.show()
def test_normals_new3 ():
#pts1 = clouds.downsample(pts1, 0.02).astype('float64')
pts1 = gen_circle_points(0.5, 30)
pts2 = gen_circle_points_pulled_in(0.5,30,6,0.4)#gen_circle_points(0.5, 30) + np.array([0.1,0.1])
wt_n = None#np.linalg.norm(pts1-pts2,axis=1)*2+1
f1 = fit_ThinPlateSpline(pts1, pts2, bend_coef=0.1, rot_coef=1e-5, wt_n=wt_n, use_cvx=True)
#f2 = fit_ThinPlateSpline(pts1, pts2, bend_coef=0.1, rot_coef=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.0001)
#f2 = te.tps_fit_normals_cvx(pts1, pts2, bend_coef=0.1, rot_coef=1e-5, normal_coef=10, wt_n=wt_n, nwsize=0.15, delta=0.0001)
#f2 = te.tps_fit_normals_cvx(pts1, pts2, bend_coef=0.1, rot_coef=1e-5, normal_coef=0.1, wt_n=None, nwsize=0.15, delta=0.0001)
f2 = te.tps_fit_normals_exact_cvx(pts1, pts2, bend_coef=0.1, rot_coef=1e-5, normal_coef = 1, wt_n=None, nwsize=0.15, delta=0.0001)
mlab.figure(1, bgcolor=(0,0,0))
mayavi_utils.plot_warping(f1, pts1, pts2, fine=False, draw_plinks=True)
test_normals_pts(np.c_[f1.transform_points(pts1),np.zeros((pts2.shape[0],1))], wsize=0.15,delta=0.15)
test_normals_pts(np.c_[pts2,np.zeros((pts2.shape[0],1))], wsize=0.15,delta=0.15)
#mlab.show()
mlab.figure(2,bgcolor=(0,0,0))
#mlab.clf()
mayavi_utils.plot_warping(f2, pts1, pts2, fine=False, draw_plinks=True)
test_normals_pts(np.c_[f2.transform_points(pts1),np.zeros((pts2.shape[0],1))], wsize=0.15,delta=0.15)
test_normals_pts(np.c_[pts2,np.zeros((pts2.shape[0],1))], wsize=0.15,delta=0.15)
mlab.show()
def test_cvx (pts1):
pts1 = clouds.downsample(pts1, 0.02).astype('float64')
pts2 = np.random.normal(0,0.004,pts1.shape) + pts1
f1 = fit_ThinPlateSpline(pts1, pts2, bend_coef=.1, rot_coef = 1e-5, wt_n=None, use_cvx = False)
f2 = fit_ThinPlateSpline(pts1, pts2, bend_coef=.1, rot_coef = 1e-5, wt_n=None, use_cvx = 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_normals_new5 ():
#pts1 = clouds.downsample(pts1, 0.02).astype('float64')
# pts1 = np.array([[0.,0.,0.], [0.,0.5,0.], [0.,1.,0.], [1.,0.,0.0], [1.,0.5,0.0], [1.,1.,0.25]])
# pts2 = np.array([[0.,0.,0.], [0.,0.5,0.], [0.,1.,0.], [1.,0.,1.], [1.,0.5,1.], [1.,1.,1.25]])
# e1 = np.array([[1.,0.,0.], [1.,0.,0.], [1.,0.,0.], [-1.,0.,0.], [-1.,0.,0.], [-1.,0.,0.]])
# e2 = np.array([[1.,0.,0.], [1.,0.,0.], [1.,0.,0.], [-1.,0.,0.], [-1.,0.,0.], [-1.,0.,0.]])
pts1, pts2, e1, e2 = create_flap_points_normals(3.0,1,dim=3)
f1 = fit_ThinPlateSpline(pts1, pts2, bend_coef=0.1, rot_coef=1e-5, wt_n=None, use_cvx=True)
#f2 = fit_ThinPlateSpline(pts1, pts2, bend_coef=0.1, rot_coef=1e-5, wt_n=None, use_cvx=True)
f2 = te.tps_eval(pts1, pts2, e1, e2, bend_coef=0.01, rot_coef=1e-5, wt_n=None, nwsize=0.15, delta=0.0001)
#f2 = te.tps_fit_normals_cvx(pts1, pts2, e1, e2, bend_coef=0.1, rot_coef=1e-5, normal_coef=0.1, wt_n=None, nwsize=0.15, delta=0.0001)
#f2 = te.tps_fit_normals_exact_cvx(pts1, pts2, e1, e2, bend_coef=0.1, rot_coef=1e-5, normal_coef = 0.1, wt_n=None, nwsize=0.15, delta=0.002)
# import IPython
# IPython.embed()
mlab.figure(1, bgcolor=(0,0,0))
mayavi_utils.plot_warping(f1, pts1, pts2, fine=False, draw_plinks=False)
_,f1e2 = te.transformed_normal_direction(pts1, e1, f1, delta=0.0001)#np.asarray([tu.tps_jacobian(f2, pt, 2).dot(nm) for pt,nm in zip(pts1,e1)])
test_normals_pts(f1.transform_points(pts1), f1e2, wsize=0.15,delta=0.15)
test_normals_pts(pts2, e2, wsize=0.15,delta=0.15)
#mlab.show()
mlab.figure(2,bgcolor=(0,0,0))
#mlab.clf()
mayavi_utils.plot_warping(f2, pts1, pts2, fine=False, draw_plinks=False)
_,f2e2 = te.transformed_normal_direction(pts1, e1, f2, delta=0.0001)#np.asarray([tu.tps_jacobian(f2, pt, 2).dot(nm) for pt,nm in zip(pts1,e1)])
test_normals_pts(f2.transform_points(pts1), f2e2, wsize=0.15,delta=0.15)
test_normals_pts(pts2, e2, wsize=0.15,delta=0.15)
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_normals_new4(n=2, l=0.5, dim=2):
pts1, pts2, e1, e2 = create_flap_points_normals(n, l, dim)
delta = 1e-2
f1 = fit_ThinPlateSpline(pts1,
pts2,
bend_coef=0.1,
rot_coef=1e-5,
wt_n=None,
use_cvx=True)
#f1 = te.tps_fit_normals_cvx(pts1, pts2, e1, e2, bend_coef=0.1, rot_coef=1e-5, normal_coef=0.1, wt_n=None, nwsize=0.15, delta=0.0001)
#f2 = fit_ThinPlateSpline(pts1, pts2, bend_coef=0.1, rot_coef=1e-5, wt_n=None, use_cvx=True)
#f2 = te.tps_eval(pts1, pts2, e1, e2, bend_coef=0.0, rot_coef=1e-5, wt_n=None, nwsize=0.15, delta=1e-8)
#f2 = te.tps_fit_normals_cvx(pts1, pts2, bend_coef=0.1, rot_coef=1e-5, normal_coef=10, wt_n=None, nwsize=0.15, delta=1e-6)
f2 = te.tps_fit_normals_cvx(pts1,
pts2,
e1,
e2,
bend_coef=0.0,
rot_coef=1e-5,
normal_coef=1,
wt_n=None,
nwsize=0.15,
delta=delta)
mlab.figure(1, bgcolor=(0, 0, 0))
mayavi_utils.plot_warping(f1, pts1, pts2, fine=False, draw_plinks=False)
_, f1e2 = te.transformed_normal_direction(
pts1, e1, f1, delta=delta
) #np.asarray([tu.tps_jacobian(f2, pt, 2).dot(nm) for pt,nm in zip(pts1,e1)])
test_normals_pts(np.c_[f1.transform_points(pts1),
np.zeros((pts2.shape[0], 1))],
np.c_[f1e2, np.zeros((f1e2.shape[0], 1))],
wsize=0.15,
delta=0.15)
test_normals_pts(np.c_[pts2, np.zeros((pts2.shape[0], 1))],
np.c_[e2, np.zeros((e2.shape[0], 1))],
wsize=0.15,
delta=0.15)
#mlab.show()
mlab.figure(2, bgcolor=(0, 0, 0))
#mlab.clf()
mayavi_utils.plot_warping(f2, pts1, pts2, fine=False, draw_plinks=False)
_, f2e2 = te.transformed_normal_direction(pts1, e1, f2, delta=delta)
test_normals_pts(np.c_[f2.transform_points(pts1),
np.zeros((pts2.shape[0], 1))],
np.c_[f2e2, np.zeros((f2e2.shape[0], 1))],
wsize=0.15,
delta=0.15)
test_normals_pts(np.c_[pts2, np.zeros((pts2.shape[0], 1))],
np.c_[e2, np.zeros((e2.shape[0], 1))],
wsize=0.15,
delta=0.15)
mlab.show()
def test_normals_new2 ():
pts1 = gen_circle_points(0.5, 30)
pts2 = gen_circle_points_pulled_in(0.5,30,4,0.2)#gen_circle_points(0.5, 30) + np.array([0.1,0.1])
#test_normals_pts(np.c_[pts2,np.zeros((pts2.shape[0],1))], wsize=0.15,delta=0.15)
# 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.1, rot_coef=1e-5, wt_n=None, use_cvx=True)
f2 = fit_ThinPlateSpline(pts1, pts2, bend_coef=0.1, rot_coef=1e-5, wt_n=None, use_cvx=False)
import IPython
IPython.embed()
mlab.figure(1)
mayavi_utils.plot_warping(f1, pts1, pts2, fine=False, draw_plinks=True)
mlab.show()
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_new3():
#pts1 = clouds.downsample(pts1, 0.02).astype('float64')
pts1 = gen_circle_points(0.5, 30)
pts2 = gen_circle_points_pulled_in(
0.5, 30, 6, 0.4) #gen_circle_points(0.5, 30) + np.array([0.1,0.1])
wt_n = None #np.linalg.norm(pts1-pts2,axis=1)*2+1
f1 = fit_ThinPlateSpline(pts1,
pts2,
bend_coef=0.1,
rot_coef=1e-5,
wt_n=wt_n,
use_cvx=True)
#f2 = fit_ThinPlateSpline(pts1, pts2, bend_coef=0.1, rot_coef=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.0001)
#f2 = te.tps_fit_normals_cvx(pts1, pts2, bend_coef=0.1, rot_coef=1e-5, normal_coef=10, wt_n=wt_n, nwsize=0.15, delta=0.0001)
#f2 = te.tps_fit_normals_cvx(pts1, pts2, bend_coef=0.1, rot_coef=1e-5, normal_coef=0.1, wt_n=None, nwsize=0.15, delta=0.0001)
f2 = te.tps_fit_normals_exact_cvx(pts1,
pts2,
bend_coef=0.1,
rot_coef=1e-5,
normal_coef=1,
wt_n=None,
nwsize=0.15,
delta=0.0001)
mlab.figure(1, bgcolor=(0, 0, 0))
mayavi_utils.plot_warping(f1, pts1, pts2, fine=False, draw_plinks=True)
test_normals_pts(np.c_[f1.transform_points(pts1),
np.zeros((pts2.shape[0], 1))],
wsize=0.15,
delta=0.15)
test_normals_pts(np.c_[pts2, np.zeros((pts2.shape[0], 1))],
wsize=0.15,
delta=0.15)
#mlab.show()
mlab.figure(2, bgcolor=(0, 0, 0))
#mlab.clf()
mayavi_utils.plot_warping(f2, pts1, pts2, fine=False, draw_plinks=True)
test_normals_pts(np.c_[f2.transform_points(pts1),
np.zeros((pts2.shape[0], 1))],
wsize=0.15,
delta=0.15)
test_normals_pts(np.c_[pts2, np.zeros((pts2.shape[0], 1))],
wsize=0.15,
delta=0.15)
mlab.show()
def test_normals_new5():
#pts1 = clouds.downsample(pts1, 0.02).astype('float64')
# pts1 = np.array([[0.,0.,0.], [0.,0.5,0.], [0.,1.,0.], [1.,0.,0.0], [1.,0.5,0.0], [1.,1.,0.25]])
# pts2 = np.array([[0.,0.,0.], [0.,0.5,0.], [0.,1.,0.], [1.,0.,1.], [1.,0.5,1.], [1.,1.,1.25]])
# e1 = np.array([[1.,0.,0.], [1.,0.,0.], [1.,0.,0.], [-1.,0.,0.], [-1.,0.,0.], [-1.,0.,0.]])
# e2 = np.array([[1.,0.,0.], [1.,0.,0.], [1.,0.,0.], [-1.,0.,0.], [-1.,0.,0.], [-1.,0.,0.]])
pts1, pts2, e1, e2 = create_flap_points_normals(3.0, 1, dim=3)
f1 = fit_ThinPlateSpline(pts1,
pts2,
bend_coef=0.1,
rot_coef=1e-5,
wt_n=None,
use_cvx=True)
#f2 = fit_ThinPlateSpline(pts1, pts2, bend_coef=0.1, rot_coef=1e-5, wt_n=None, use_cvx=True)
f2 = te.tps_eval(pts1,
pts2,
e1,
e2,
bend_coef=0.01,
rot_coef=1e-5,
wt_n=None,
nwsize=0.15,
delta=0.0001)
#f2 = te.tps_fit_normals_cvx(pts1, pts2, e1, e2, bend_coef=0.1, rot_coef=1e-5, normal_coef=0.1, wt_n=None, nwsize=0.15, delta=0.0001)
#f2 = te.tps_fit_normals_exact_cvx(pts1, pts2, e1, e2, bend_coef=0.1, rot_coef=1e-5, normal_coef = 0.1, wt_n=None, nwsize=0.15, delta=0.002)
import IPython
IPython.embed()
mlab.figure(1, bgcolor=(0, 0, 0))
mayavi_utils.plot_warping(f1, pts1, pts2, fine=False, draw_plinks=False)
_, f1e2 = te.transformed_normal_direction(
pts1, e1, f1, delta=0.0001
) #np.asarray([tu.tps_jacobian(f2, pt, 2).dot(nm) for pt,nm in zip(pts1,e1)])
test_normals_pts(f1.transform_points(pts1), f1e2, wsize=0.15, delta=0.15)
test_normals_pts(pts2, e2, wsize=0.15, delta=0.15)
#mlab.show()
mlab.figure(2, bgcolor=(0, 0, 0))
#mlab.clf()
mayavi_utils.plot_warping(f2, pts1, pts2, fine=False, draw_plinks=False)
_, f2e2 = te.transformed_normal_direction(
pts1, e1, f2, delta=0.0001
) #np.asarray([tu.tps_jacobian(f2, pt, 2).dot(nm) for pt,nm in zip(pts1,e1)])
test_normals_pts(f2.transform_points(pts1), f2e2, wsize=0.15, delta=0.15)
test_normals_pts(pts2, e2, wsize=0.15, delta=0.15)
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_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 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_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 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_rpm_bij_normals(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=0.04,
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')
fwddist_normals_nm = calculate_normal_dist2(xwarped_nd, y_md, nwsize)
invdist_nm = ssd.cdist(x_nd, ywarped_md, 'euclidean')
invdist_normals_nm = calculate_normal_dist2(x_nd, ywarped_md, nwsize)
import IPython
IPython.embed()
r = rads[i]
prob_nm = np.exp(-(fwddist_nm + invdist_nm + normal_coef *
(fwddist_normals_nm + invdist_normals_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)
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)
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 tps_eval(x_na, y_ng, e_x = None, e_y = None, bend_coef = 0.1, rot_coef = 1e-5, wt_n = None, nwsize=0.02, delta=0.0001):
"""
delta: Normal length.
"""
n,dim = x_na.shape
# Finding the evaluation matrix.
K = tu.tps_kernel_mat(x_na)
Q1 = np.c_[np.ones((n,1)),x_na]
# normal eval matrix
L = np.r_[np.c_[K,Q1],np.c_[Q1.T,np.zeros((dim+1,dim+1))]]
Linv = nlg.inv(L)
# Normals
if e_x is None:
e_x = tu.find_all_normals_naive(x_na, nwsize, flip_away=True, project_lower_dim=(dim==3))
if e_y is None:
e_y = tu.find_all_normals_naive(y_ng, nwsize, flip_away=True, project_lower_dim=(dim==3))
## First, we solve the landmark only spline.
f = registration.fit_ThinPlateSpline(x_na, y_ng, bend_coef=bend_coef, rot_coef=rot_coef, wt_n=wt_n, use_cvx=True)
# What are the slope values caused by these splines at the points?
# It can be found using the Jacobian at the point.
# Finding what the normals are being mapped to
d0 = np.empty((dim,0))
for x, nm in zip(x_na,e_x):
d0 = np.c_[d0,tu.tps_jacobian(f, x, dim).dot(nm)]
import IPython
IPython.embed()
d0 = d0.reshape((d0.shape[1]*dim,1))
# Desired slopes
d = e_y.T.reshape((e_y.shape[0]*dim,1))
## Let's find the difference of the slopes to get the edge correction.
d_diff = d - d0
M = np.zeros((n,n))
P = np.zeros((n,n))
# Get rid of these for loops at some point
for i in range(n):
p1, n1 = x_na[i,:], e_x[i,:]
for j in range(n):
if i == j:
M[i,i] = P[i,i] = 0
else:
p2, n2 = x_na[j,:], e_x[j,:]
M[i,j] = tu.deriv_U(p2,p1,n2,dim)
if i < j:
P[i,j] = P[j,i] = tu.deriv2_U(p1,p2,n2,n1,dim)
M = np.r_[M,np.zeros((1,n)),e_x.T]
T = np.r_[np.c_[np.eye(n+dim+1), np.zeros((n+dim+1,n))],np.c_[-M.T.dot(Linv), np.eye(n)]]
N = P + M.T.dot(Linv).dot(M) # + 2*log(del/delta) ---> assuming all the normals are of same length
# Evaluation matrix for just the change slopes
Q_single_dim = -2*np.log(delta)*(np.eye(n) +1.0/(2*np.log(delta))*N) # for single dimension
Q = slg.block_diag(*[Q_single_dim]*dim)
# coefficients of orthogonalized slope elements
w_diff = nlg.inv(Q).dot(d_diff) # ----> This is where the shit happens
w_diff = w_diff.reshape((n,dim), order='F')
# padding with 0's
w_diff_whole = np.r_[np.zeros((n+dim+1,dim)),w_diff]
w_whole = T.T.dot(w_diff_whole)
w_final = np.r_[f.w_ng, np.atleast_2d(f.trans_g), f.lin_ag, np.zeros((n,dim))] + w_whole
fn = registration.ThinPlateSplineNormals(dim)
fn.x_na, fn.n_na = x_na, e_x
fn.w_ng, fn.trans_g, fn.lin_ag, fn.wn_ng= w_final[:n,:], w_final[n,:], w_final[n+1:n+1+dim,:], w_final[n+1+dim:,:]
import IPython
IPython.embed()
return fn
def tps_eval(x_na,
y_ng,
e_x=None,
e_y=None,
bend_coef=0.1,
rot_coef=1e-5,
wt_n=None,
nwsize=0.02,
delta=0.0001):
"""
delta: Normal length.
"""
n, dim = x_na.shape
# Finding the evaluation matrix.
K = tu.tps_kernel_mat(x_na)
Q1 = np.c_[np.ones((n, 1)), x_na]
# normal eval matrix
L = np.r_[np.c_[K, Q1], np.c_[Q1.T, np.zeros((dim + 1, dim + 1))]]
Linv = nlg.inv(L)
# Normals
if e_x is None:
e_x = tu.find_all_normals_naive(x_na,
nwsize,
flip_away=True,
project_lower_dim=(dim == 3))
if e_y is None:
e_y = tu.find_all_normals_naive(y_ng,
nwsize,
flip_away=True,
project_lower_dim=(dim == 3))
## First, we solve the landmark only spline.
f = registration.fit_ThinPlateSpline(x_na,
y_ng,
bend_coef=bend_coef,
rot_coef=rot_coef,
wt_n=wt_n,
use_cvx=True)
# What are the slope values caused by these splines at the points?
# It can be found using the Jacobian at the point.
# Finding what the normals are being mapped to
d0 = np.empty((dim, 0))
for x, nm in zip(x_na, e_x):
d0 = np.c_[d0, tu.tps_jacobian(f, x, dim).dot(nm)]
import IPython
IPython.embed()
d0 = d0.reshape((d0.shape[1] * dim, 1))
# Desired slopes
d = e_y.T.reshape((e_y.shape[0] * dim, 1))
## Let's find the difference of the slopes to get the edge correction.
d_diff = d - d0
M = np.zeros((n, n))
P = np.zeros((n, n))
# Get rid of these for loops at some point
for i in range(n):
p1, n1 = x_na[i, :], e_x[i, :]
for j in range(n):
if i == j:
M[i, i] = P[i, i] = 0
else:
p2, n2 = x_na[j, :], e_x[j, :]
M[i, j] = tu.deriv_U(p2, p1, n2, dim)
if i < j:
P[i, j] = P[j, i] = tu.deriv2_U(p1, p2, n2, n1, dim)
M = np.r_[M, np.zeros((1, n)), e_x.T]
T = np.r_[np.c_[np.eye(n + dim + 1),
np.zeros((n + dim + 1, n))], np.c_[-M.T.dot(Linv),
np.eye(n)]]
N = P + M.T.dot(Linv).dot(
M
) # + 2*log(del/delta) ---> assuming all the normals are of same length
# Evaluation matrix for just the change slopes
Q_single_dim = -2 * np.log(delta) * (
np.eye(n) + 1.0 / (2 * np.log(delta)) * N) # for single dimension
Q = slg.block_diag(*[Q_single_dim] * dim)
# coefficients of orthogonalized slope elements
w_diff = nlg.inv(Q).dot(d_diff) # ----> This is where the shit happens
w_diff = w_diff.reshape((n, dim), order='F')
# padding with 0's
w_diff_whole = np.r_[np.zeros((n + dim + 1, dim)), w_diff]
w_whole = T.T.dot(w_diff_whole)
w_final = np.r_[f.w_ng,
np.atleast_2d(f.trans_g), f.lin_ag,
np.zeros((n, dim))] + w_whole
fn = registration.ThinPlateSplineNormals(dim)
fn.x_na, fn.n_na = x_na, e_x
fn.w_ng, fn.trans_g, fn.lin_ag, fn.wn_ng = w_final[:n, :], w_final[
n, :], w_final[n + 1:n + 1 + dim, :], w_final[n + 1 + dim:, :]
import IPython
IPython.embed()
return fn