def tps_rpm_bootstrap(x_nd, y_md, z_kd, xy_corr, n_init_iter = 10, n_iter = 20, reg_init = .1, reg_final = .001, rad_init = .1, rad_final = .005, rot_reg = 1e-3,
plotting = False, plot_cb = None, old_xyz=None, new_xyz=None):
"""
modification for tps_rpm in order to use bootstrapping info
return a warp taking x to z, proceeds by warping y->z, then intializes tps_rpm with the correspondences from that warping
"""
_, _, yz_corr = tps_rpm_bij(y_md, z_kd, n_iter = n_iter, reg_init = reg_init, reg_final = reg_final,
rad_init = rad_init, rad_final = rad_final, rot_reg = rot_reg, plotting=plotting,
plot_cb = plot_cb, old_xyz = old_xyz, new_xyz = new_xyz, return_corr = True)
xz_corr = xy_corr.dot(yz_corr)
# corr_nk, r_N, _ = balance_matrix3(xz_corr, 10, 1e-1, 1e-2)
# corr_nk += 1e-9
corr_nk = xz_corr
wt_n = corr_nk.sum(axis=1)
wt_k = corr_nk.sum(axis=0)
xtarg_nd = (corr_nk/wt_n[:,None]).dot(z_kd)
ztarg_kd = (corr_nk/wt_k[None,:]).T.dot(x_nd)
f = fit_ThinPlateSpline(x_nd, xtarg_nd, bend_coef = reg_final, wt_n=wt_n, rot_coef = rot_reg)
g = fit_ThinPlateSpline(z_kd, ztarg_kd, bend_coef = reg_final, wt_n=wt_k, rot_coef = rot_reg)
f._cost = tps.tps_cost(f.lin_ag, f.trans_g, f.w_ng, f.x_na, xtarg_nd, reg_final, wt_n=wt_n)/wt_n.mean()
g._cost = tps.tps_cost(g.lin_ag, g.trans_g, g.w_ng, g.x_na, ztarg_kd, reg_final, wt_n=wt_k)/wt_k.mean()
print 'cost:\t', f._cost + g._cost
return (f, g, corr_nk)
def tps_rpm_bij(x_nd, y_md, n_iter = 20, reg_init = .1, reg_final = .001, rad_init = .1, rad_final = .005, rot_reg = 1e-3,
plotting = False, plot_cb = None, old_xyz=None, new_xyz=None, f_init = None, g_init = None, return_corr = False):
"""
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)
if not f_init:
f = ThinPlateSpline(d)
f.trans_g = np.median(y_md,axis=0) - np.median(x_nd,axis=0)
else:
f = f_init
if not f_init:
g = ThinPlateSpline(d)
g.trans_g = -f.trans_g
else:
g = g_init
# 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, 1e-2)
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 == n_iter -1) or (plotting and i%plotting==0):
plot_cb(x_nd, y_md, xtarg_nd, corr_nm, wt_n, f, old_xyz, new_xyz, last_one=(i == n_iter -1))
f = fit_ThinPlateSpline(x_nd, xtarg_nd, bend_coef = regs[i], wt_n=wt_n, rot_coef = rot_reg)
g = fit_ThinPlateSpline(y_md, ytarg_md, 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()
if return_corr:
return f, g, corr_nm
return f,g
def tps_rpm(
x_nd,
y_md,
n_iter=20,
reg_init=0.1,
reg_final=0.001,
rad_init=0.05,
rad_final=0.001,
plotting=False,
verbose=True,
f_init=None,
return_full=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)
if f_init is not None:
f = f_init
else:
f = ThinPlateSpline(d)
f.trans_g = np.median(y_md, axis=0) - np.median(x_nd, axis=0)
for i in xrange(n_iter):
xwarped_nd = f.transform_points(x_nd)
# targ_nd = find_targets(x_nd, y_md, corr_opts = dict(r = rads[i], p = .1))
corr_nm = calc_correspondence_matrix(xwarped_nd, y_md, r=rads[i], p=0.2)
wt_n = corr_nm.sum(axis=1)
goodn = wt_n > 0.1
targ_Nd = np.dot(corr_nm[goodn, :] / wt_n[goodn][:, None], y_md)
if plotting and i % plotting == 0:
plot_cb(x_nd, y_md, targ_Nd, corr_nm, wt_n, f)
x_Nd = x_nd[goodn]
f = fit_ThinPlateSpline(x_Nd, targ_Nd, bend_coef=regs[i], wt_n=wt_n[goodn], rot_coef=10 * regs[i])
if return_full:
info = {}
info["corr_nm"] = corr_nm
info["goodn"] = goodn
info["x_Nd"] = x_nd[goodn, :]
info["targ_Nd"] = targ_Nd
info["wt_N"] = wt_n[goodn]
info["cost"] = tps.tps_cost(f.lin_ag, f.trans_g, f.w_ng, x_Nd, targ_Nd, regs[-1])
return f, info
else:
return f
def tps_fit_fixedrot_is_minimizer():
"""
check that tps_fit_fixedrot minimizes tps_cost subject to constraint
"""
x_na = np.random.randn(100,3)
y_ng = np.random.randn(100,3)
bend_coef = .1
lin_ag = np.random.randn(3,3)
trans_g, w_ng = tps.tps_fit_fixedrot(x_na, y_ng, bend_coef, lin_ag)
hopefully_min_cost = tps.tps_cost(lin_ag, trans_g, w_ng, x_na, y_ng, bend_coef)
n_tries = 50
other_costs = np.empty(n_tries)
for i in xrange(n_tries):
N = len(x_na)
_u,_s,_vh = np.linalg.svd(np.c_[x_na, np.ones((N,1))], full_matrices=True)
pert = .01*_u[:,4:].dot(np.random.randn(N-4,3))
assert np.allclose(x_na.T.dot(pert),np.zeros((3,3)))
other_costs[i] = tps.tps_cost(lin_ag, trans_g, w_ng+pert, x_na, y_ng, bend_coef)
assert (other_costs > hopefully_min_cost).all()
def tps_fit_is_minimizer():
"""
check that tps_fit_fixedrot minimizes tps_cost subject to constraint
"""
x_na = np.random.randn(100, 3)
y_ng = np.random.randn(100, 3)
bend_coef = .1
lin_ag = np.random.randn(3, 3)
trans_g, w_ng = tps.tps_fit_fixedrot(x_na, y_ng, bend_coef, lin_ag)
hopefully_min_cost = tps.tps_cost(lin_ag, trans_g, w_ng, x_na, y_ng,
bend_coef)
n_tries = 50
other_costs = np.empty(n_tries)
for i in xrange(n_tries):
N = len(x_na)
_u, _s, _vh = np.linalg.svd(np.c_[x_na, np.ones((N, 1))],
full_matrices=True)
pert = .01 * _u[:, 4:].dot(np.random.randn(N - 4, 3))
assert np.allclose(x_na.T.dot(pert), np.zeros((3, 3)))
other_costs[i] = tps.tps_cost(lin_ag, trans_g, w_ng + pert, x_na, y_ng,
bend_coef)
assert (other_costs > hopefully_min_cost).all()
def tps_rpm(x_nd, y_md, n_iter = 20, reg_init = .1, reg_final = .001, rad_init = .05, rad_final = .001,
plotting = False, verbose=True, f_init = None, return_full = 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)
if f_init is not None:
f = f_init
else:
f = ThinPlateSpline(d)
f.trans_g = np.median(y_md,axis=0) - np.median(x_nd,axis=0)
for i in xrange(n_iter):
xwarped_nd = f.transform_points(x_nd)
# targ_nd = find_targets(x_nd, y_md, corr_opts = dict(r = rads[i], p = .1))
corr_nm = calc_correspondence_matrix(xwarped_nd, y_md, r=rads[i], p=.2)
wt_n = corr_nm.sum(axis=1)
goodn = wt_n > .1
targ_Nd = np.dot(corr_nm[goodn, :]/wt_n[goodn][:,None], y_md)
if plotting and i%plotting==0:
plot_cb(x_nd, y_md, targ_Nd, corr_nm, wt_n, f)
x_Nd = x_nd[goodn]
f = fit_ThinPlateSpline(x_Nd, targ_Nd, bend_coef = regs[i], wt_n = wt_n[goodn], rot_coef = 10*regs[i])
if return_full:
info = {}
info["corr_nm"] = corr_nm
info["goodn"] = goodn
info["x_Nd"] = x_nd[goodn,:]
info["targ_Nd"] = targ_Nd
info["wt_N"] = wt_n[goodn]
info["cost"] = tps.tps_cost(f.lin_ag, f.trans_g, f.w_ng, x_Nd, targ_Nd, regs[-1])
return f, info
else:
return f
def tps_multi_mix(x_clouds, y_clouds,
n_iter = 100,
bend_init = 0.05, bend_final = .0001,
rot_init = (0.1,0.1,0.025), rot_final=(0.001,0.001,0.00025),
scale_init=1, scale_final=0.001,
rad_init = .5, rad_final = .0005,
x_aug=None, y_aug=None,
verbose=False, f_init = None, return_full = False,
plotting_cb=None, plotter=None):
"""
x_aug : dict of matrices of extra coordinates for x_clouds. The key is the index of the cloud.
y_aug : similar to x_aug for y_clouds
Similar to tps_rpm_regrot except that it accepts a
LIST of source and target point clouds and registers
a cloud in the source to the corresponding one in the target.
For details on the various parameters check the doc of tps_rpm_regrot.
"""
assert len(x_clouds)==len(y_clouds), "Different number of point-clouds in source and target."
if x_aug==None or y_aug==None:
x_aug = y_aug = {}
#flatten the list of point clouds into one big point cloud
combined_x = np.concatenate(x_clouds)
combined_y = np.concatenate(y_clouds)
# concatenate the clouds into one big cloud
_,d = combined_x.shape
regs = registration.loglinspace(bend_init, bend_final, n_iter)
rads = registration.loglinspace(rad_init, rad_final, n_iter)
scales = registration.loglinspace(scale_init, scale_final, n_iter)
rots = registration.loglinspace_arr(rot_init, rot_final, n_iter)
# initialize the function f.
if f_init is not None:
f = f_init
else:
f = registration.ThinPlateSpline(d)
f.trans_g = np.median(combined_y,axis=0) - np.median(combined_x,axis=0)
# iterate b/w calculating correspondences and fitting the transformation.
for i in xrange(n_iter):
target_pts = []
good_inds = []
wt = []
for j in xrange(len(x_clouds)): #process a pair of point-clouds
x_nd = x_clouds[j]
y_md = y_clouds[j]
assert x_nd.ndim==y_md.ndim==2, "tps_rpm_reg_rot_multi : Point clouds are not two dimensional arrays"
xwarped_nd = f.transform_points(x_nd)
# use augmented coordinates.
if x_aug.has_key(j) and y_aug.has_key(j):
corr_nm = registration.calc_correspondence_matrix(np.c_[xwarped_nd, x_aug[j]], np.c_[y_md, y_aug[j]], r=rads[i], p=.2)
else:
corr_nm = registration.calc_correspondence_matrix(xwarped_nd, y_md, r=rads[i], p=.2)
wt_n = corr_nm.sum(axis=1) # gives the row-wise sum of the corr_nm matrix
goodn = wt_n > 0.1
targ_Nd = np.dot(corr_nm[goodn, :]/wt_n[goodn][:,None], y_md) # calculate the average points based on softmatching
target_pts.append(targ_Nd)
good_inds.append(goodn)
wt.append(wt_n[goodn])
target_pts = np.concatenate(target_pts)
good_inds = np.concatenate(good_inds)
source_pts = combined_x[good_inds]
wt = np.concatenate(wt)
assert len(target_pts)==len(source_pts)==len(wt), "Lengths are not equal. Error!"
## USE SQP BASED FITTING:
f = registration.fit_ThinPlateSpline(source_pts, target_pts, bend_coef = regs[i], wt_n = wt_n[good_inds], rot_coef = 10*regs[i])
# A, B, c = sqpregpy.fit_sqp(source_pts, target_pts, wt_n[good_inds], rots[i], scales[i], regs[i], True, False)
# c = c.flatten()
# f = registration.ThinPlateSpline()
# f.x_na = source_pts
# f.w_ng = A
# f.lin_ag = B
# f.trans_g = c
mscore = registration.match_score(source_pts, target_pts)
tscore = tps.tps_cost(f.lin_ag, f.trans_g, f.w_ng, source_pts, target_pts, regs[-1])
print colorize("\ttps-mix : iter : %d | fit distance : "%i, "red") , colorize("%g"%mscore, "green"), colorize(" | tps score: %g"%tscore, "blue")
if False and plotting_cb and i%5==0:
plotting_cb(f)
# just plots the "target_pts" : the matched up points found by the correspondences.
if False and plotter:
plotter.request(gen_mlab_request(mlab.points3d, target_pts[:,0], target_pts[:,1], target_pts[:,2], color=(1,1,0), scale_factor=0.001))
# return if source and target match up well
if tscore < 1e-6:
break
if return_full:
info = {}
info["cost"] = tps.tps_cost(f.lin_ag, f.trans_g, f.w_ng, source_pts, target_pts, regs[-1])
return f, info
else:
return f
def tps_rpm_bij(x_nd, y_md, n_iter = 20, reg_init = .1, reg_final = .001, rad_init = .1, rad_final = .005, rot_reg = 1e-3,
plotting = False, plot_cb = None, x_interest_pts = None, y_interest_pts = None, outlierprior = .1, outlierfrac = 2e-1):
"""
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
interest_pts are points in either scene where we want a lower prior of outliers
"""
_,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) # align the medians
# do a coarse search through rotations
# fit_rotation(f, x_nd, y_md) removed b/c it is very slow and doesn't realy help
g = ThinPlateSpline(d)
g.trans_g = -f.trans_g
# set up outlier priors for source and target scenes
n, _ = x_nd.shape
m, _ = y_md.shape
if x_interest_pts:
x_interest_dists = outlierprior*np.exp( -1*ssd.cdist(x_interest_pts, x_nd, 'euclidean')/(rad_init))
raise NotImplementedError
else:
x_priors = np.ones(n)*outlierfrac
if y_interest_pts:
y_interest_dists = np.exp( -1*ssd.cdist(y_interest_pts, y_md, 'euclidean')/(rad_init))
raise NotImplementedError
else:
y_priors = np.ones(m)*outlierfrac
# 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, x_priors, y_priors, outlierfrac) # edit final value to change outlier percentage
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:
plot_cb(x_nd, y_md, xtarg_nd, corr_nm, wt_n, f)
f = fit_ThinPlateSpline(x_nd, xtarg_nd, bend_coef = regs[i], wt_n=wt_n, rot_coef = rot_reg)
g = fit_ThinPlateSpline(y_md, ytarg_md, 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_multi_mix(x_clouds,
y_clouds,
n_iter=100,
bend_init=0.05,
bend_final=.0001,
rot_init=(0.1, 0.1, 0.025),
rot_final=(0.001, 0.001, 0.00025),
scale_init=1,
scale_final=0.001,
rad_init=.5,
rad_final=.0005,
x_aug=None,
y_aug=None,
verbose=False,
f_init=None,
return_full=False,
plotting_cb=None,
plotter=None):
"""
x_aug : dict of matrices of extra coordinates for x_clouds. The key is the index of the cloud.
y_aug : similar to x_aug for y_clouds
Similar to tps_rpm_regrot except that it accepts a
LIST of source and target point clouds and registers
a cloud in the source to the corresponding one in the target.
For details on the various parameters check the doc of tps_rpm_regrot.
"""
assert len(x_clouds) == len(
y_clouds), "Different number of point-clouds in source and target."
if x_aug == None or y_aug == None:
x_aug = y_aug = {}
#flatten the list of point clouds into one big point cloud
combined_x = np.concatenate(x_clouds)
combined_y = np.concatenate(y_clouds)
# concatenate the clouds into one big cloud
_, d = combined_x.shape
regs = registration.loglinspace(bend_init, bend_final, n_iter)
rads = registration.loglinspace(rad_init, rad_final, n_iter)
scales = registration.loglinspace(scale_init, scale_final, n_iter)
rots = registration.loglinspace_arr(rot_init, rot_final, n_iter)
# initialize the function f.
if f_init is not None:
f = f_init
else:
f = registration.ThinPlateSpline(d)
f.trans_g = np.median(combined_y, axis=0) - np.median(combined_x,
axis=0)
# iterate b/w calculating correspondences and fitting the transformation.
for i in xrange(n_iter):
target_pts = []
good_inds = []
wt = []
for j in xrange(len(x_clouds)): #process a pair of point-clouds
x_nd = x_clouds[j]
y_md = y_clouds[j]
assert x_nd.ndim == y_md.ndim == 2, "tps_rpm_reg_rot_multi : Point clouds are not two dimensional arrays"
xwarped_nd = f.transform_points(x_nd)
# use augmented coordinates.
if x_aug.has_key(j) and y_aug.has_key(j):
corr_nm = registration.calc_correspondence_matrix(
np.c_[xwarped_nd, x_aug[j]],
np.c_[y_md, y_aug[j]],
r=rads[i],
p=.2)
else:
corr_nm = registration.calc_correspondence_matrix(xwarped_nd,
y_md,
r=rads[i],
p=.2)
wt_n = corr_nm.sum(
axis=1) # gives the row-wise sum of the corr_nm matrix
goodn = wt_n > 0.1
targ_Nd = np.dot(
corr_nm[goodn, :] / wt_n[goodn][:, None],
y_md) # calculate the average points based on softmatching
target_pts.append(targ_Nd)
good_inds.append(goodn)
wt.append(wt_n[goodn])
target_pts = np.concatenate(target_pts)
good_inds = np.concatenate(good_inds)
source_pts = combined_x[good_inds]
wt = np.concatenate(wt)
assert len(target_pts) == len(source_pts) == len(
wt), "Lengths are not equal. Error!"
## USE SQP BASED FITTING:
f = registration.fit_ThinPlateSpline(source_pts,
target_pts,
bend_coef=regs[i],
wt_n=wt_n[good_inds],
rot_coef=10 * regs[i])
# A, B, c = sqpregpy.fit_sqp(source_pts, target_pts, wt_n[good_inds], rots[i], scales[i], regs[i], True, False)
# c = c.flatten()
# f = registration.ThinPlateSpline()
# f.x_na = source_pts
# f.w_ng = A
# f.lin_ag = B
# f.trans_g = c
mscore = registration.match_score(source_pts, target_pts)
tscore = tps.tps_cost(f.lin_ag, f.trans_g, f.w_ng, source_pts,
target_pts, regs[-1])
print colorize("\ttps-mix : iter : %d | fit distance : " % i,
"red"), colorize("%g" % mscore, "green"), colorize(
" | tps score: %g" % tscore, "blue")
if False and plotting_cb and i % 5 == 0:
plotting_cb(f)
# just plots the "target_pts" : the matched up points found by the correspondences.
if False and plotter:
plotter.request(
gen_mlab_request(mlab.points3d,
target_pts[:, 0],
target_pts[:, 1],
target_pts[:, 2],
color=(1, 1, 0),
scale_factor=0.001))
# return if source and target match up well
if tscore < 1e-6:
break
if return_full:
info = {}
info["cost"] = tps.tps_cost(f.lin_ag, f.trans_g, f.w_ng, source_pts,
target_pts, regs[-1])
return f, info
else:
return f
def tps_rpm_bij(x_nd,
y_md,
n_iter=20,
reg_init=.1,
reg_final=.001,
rad_init=.1,
rad_final=.005,
rot_reg=1e-3,
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)
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:
plot_cb(x_nd, y_md, xtarg_nd, corr_nm, wt_n, f)
f = fit_ThinPlateSpline(x_nd,
xtarg_nd,
bend_coef=regs[i],
wt_n=wt_n,
rot_coef=rot_reg)
g = fit_ThinPlateSpline(y_md,
ytarg_md,
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_sc_multi(x_clouds, y_clouds,
n_iter = 100,
bend_init = 0.05, bend_final = .0001,
rot_init = (0.1,0.1,0.025), rot_final=(0.001,0.001,0.00025),
scale_init=1, scale_final=0.001,
rad_init = .5, rad_final = .0005,
verbose=False, f_init = None, return_full = False,
plotting_cb=None, plotter=None):
"""
Combines the shape-context distances with tps-rpm.
"""
assert len(x_clouds)==len(y_clouds), "Different number of point-clouds in source and target."
#flatten the list of point clouds into one big point cloud
combined_x = np.concatenate(x_clouds)
combined_y = np.concatenate(y_clouds)
# concatenate the clouds into one big cloud
_,d = combined_x.shape
regs = registration.loglinspace(bend_init, bend_final, n_iter)
rads = registration.loglinspace(rad_init, rad_final, n_iter)
scales = registration.loglinspace(scale_init, scale_final, n_iter)
rots = registration.loglinspace_arr(rot_init, rot_final, n_iter)
# initialize the function f.
if f_init is not None:
f = f_init
else:
f = registration.ThinPlateSpline(d)
f.trans_g = np.median(combined_y,axis=0) - np.median(combined_x,axis=0)
# iterate b/w calculating correspondences and fitting the transformation.
for i in xrange(n_iter):
target_pts = []
good_inds = []
wt = []
for j in xrange(len(x_clouds)): #process a pair of point-clouds
x_nd = x_clouds[j]
y_md = y_clouds[j]
assert x_nd.ndim==y_md.ndim==2, "tps_rpm_reg_rot_multi : Point clouds are not two dimensional arrays"
xwarped_nd = f.transform_points(x_nd)
corr_nm = calc_dist_matrix(xwarped_nd, y_md, r=rads[i], p=.2)
wt_n = corr_nm.sum(axis=1) # gives the row-wise sum of the corr_nm matrix
goodn = wt_n > 0.
targ_Nd = np.dot(corr_nm[goodn, :]/wt_n[goodn][:,None], y_md) # calculate the average points based on softmatching
target_pts.append(targ_Nd)
good_inds.append(goodn)
wt.append(wt_n[goodn])
target_pts = np.concatenate(target_pts)
good_inds = np.concatenate(good_inds)
source_pts = combined_x[good_inds]
wt = np.concatenate(wt)
assert len(target_pts)==len(source_pts)==len(wt), "Lengths are not equal. Error!"
f = registration.fit_ThinPlateSpline(source_pts, target_pts, bend_coef = regs[i], wt_n = wt_n[good_inds], rot_coef = 10*regs[i])
mscore = registration.match_score(source_pts, target_pts)
tscore = tps.tps_cost(f.lin_ag, f.trans_g, f.w_ng, source_pts, target_pts, regs[-1])
print colorize("\ttps-mix : iter : %d | fit distance : "%i, "red") , colorize("%g"%mscore, "green"), colorize(" | tps score: %g"%tscore, "blue")
if plotting_cb and i%5==0:
plotting_cb(f)
# just plots the "target_pts" : the matched up points found by the correspondences.
if plotter:
plotter.request(gen_mlab_request(mlab.points3d, target_pts[:,0], target_pts[:,1], target_pts[:,2], color=(1,1,0), scale_factor=0.001))
# return if source and target match up well
if tscore < 1e-6:
break
if return_full:
info = {}
info["cost"] = tps.tps_cost(f.lin_ag, f.trans_g, f.w_ng, source_pts, target_pts, regs[-1])
return f, info
else:
return f
def tps_sc_multi(x_clouds,
y_clouds,
n_iter=100,
bend_init=0.05,
bend_final=.0001,
rot_init=(0.1, 0.1, 0.025),
rot_final=(0.001, 0.001, 0.00025),
scale_init=1,
scale_final=0.001,
rad_init=.5,
rad_final=.0005,
verbose=False,
f_init=None,
return_full=False,
plotting_cb=None,
plotter=None):
"""
Combines the shape-context distances with tps-rpm.
"""
assert len(x_clouds) == len(
y_clouds), "Different number of point-clouds in source and target."
#flatten the list of point clouds into one big point cloud
combined_x = np.concatenate(x_clouds)
combined_y = np.concatenate(y_clouds)
# concatenate the clouds into one big cloud
_, d = combined_x.shape
regs = registration.loglinspace(bend_init, bend_final, n_iter)
rads = registration.loglinspace(rad_init, rad_final, n_iter)
scales = registration.loglinspace(scale_init, scale_final, n_iter)
rots = registration.loglinspace_arr(rot_init, rot_final, n_iter)
# initialize the function f.
if f_init is not None:
f = f_init
else:
f = registration.ThinPlateSpline(d)
f.trans_g = np.median(combined_y, axis=0) - np.median(combined_x,
axis=0)
# iterate b/w calculating correspondences and fitting the transformation.
for i in xrange(n_iter):
target_pts = []
good_inds = []
wt = []
for j in xrange(len(x_clouds)): #process a pair of point-clouds
x_nd = x_clouds[j]
y_md = y_clouds[j]
assert x_nd.ndim == y_md.ndim == 2, "tps_rpm_reg_rot_multi : Point clouds are not two dimensional arrays"
xwarped_nd = f.transform_points(x_nd)
corr_nm = calc_dist_matrix(xwarped_nd, y_md, r=rads[i], p=.2)
wt_n = corr_nm.sum(
axis=1) # gives the row-wise sum of the corr_nm matrix
goodn = wt_n > 0.
targ_Nd = np.dot(
corr_nm[goodn, :] / wt_n[goodn][:, None],
y_md) # calculate the average points based on softmatching
target_pts.append(targ_Nd)
good_inds.append(goodn)
wt.append(wt_n[goodn])
target_pts = np.concatenate(target_pts)
good_inds = np.concatenate(good_inds)
source_pts = combined_x[good_inds]
wt = np.concatenate(wt)
assert len(target_pts) == len(source_pts) == len(
wt), "Lengths are not equal. Error!"
f = registration.fit_ThinPlateSpline(source_pts,
target_pts,
bend_coef=regs[i],
wt_n=wt_n[good_inds],
rot_coef=10 * regs[i])
mscore = registration.match_score(source_pts, target_pts)
tscore = tps.tps_cost(f.lin_ag, f.trans_g, f.w_ng, source_pts,
target_pts, regs[-1])
print colorize("\ttps-mix : iter : %d | fit distance : " % i,
"red"), colorize("%g" % mscore, "green"), colorize(
" | tps score: %g" % tscore, "blue")
if plotting_cb and i % 5 == 0:
plotting_cb(f)
# just plots the "target_pts" : the matched up points found by the correspondences.
if plotter:
plotter.request(
gen_mlab_request(mlab.points3d,
target_pts[:, 0],
target_pts[:, 1],
target_pts[:, 2],
color=(1, 1, 0),
scale_factor=0.001))
# return if source and target match up well
if tscore < 1e-6:
break
if return_full:
info = {}
info["cost"] = tps.tps_cost(f.lin_ag, f.trans_g, f.w_ng, source_pts,
target_pts, regs[-1])
return f, info
else:
return f
def tps_rpm_regrot(x_nd, y_md, n_iter = 100, bend_init = 0.05, bend_final = .0001,
rot_init = (0.1,0.1,0.025), rot_final=(0.001,0.001,0.00025), scale_init=1, scale_final=0.001, rad_init = .5, rad_final = .0005,
verbose=True, f_init = None, return_full = False):
"""
tps-rpm which uses regularization on the rotation and scaling separately.
Based on John's [Chui & Rangarajan] tps-rpm.
Various parameters are:
======================================
1. bend_init/final : Regularization on the non-linear part of the transform.
This value should go to zero to get exact fit.
2. rot_init/final (x,y,z) : Regularization on the rotation along the x,y,z axes.
Note that the default cost for rotation along z is much lower than
the cost for rotation along the x and y axes.
3. scale_init/final : Regularization on the scaling part of the affine transform.
This should in general be high because normally the scene is not scaled.
4. rad_init/final : This defines the unit of distance in each iteration.
The correspondences are calculated as exp(- dist/ r).
This parameter defines this r.
[see Chui and Rangarajan page 5, equation 6. This r is the 'T' there.]
Possible scope for improvement/ exploration:
Currently, in every iteration, all the parameters are updated,
may be we need to add more loops, iterating over the parameters individually.
"""
_,d = x_nd.shape
regs = loglinspace(bend_init, bend_final, n_iter)
rads = loglinspace(rad_init, rad_final, n_iter)
scales = loglinspace(scale_init, scale_final, n_iter)
rots = loglinspace_arr(rot_init, rot_final, n_iter)
# initialize the function f.
if f_init is not None:
f = f_init
else:
f = ThinPlateSpline(d)
f.trans_g = np.median(y_md,axis=0) - np.median(x_nd,axis=0)
# iterate b/w calculating correspondences and fitting the transformation.
for i in xrange(n_iter):
print 'iter : ', i
xwarped_nd = f.transform_points(x_nd)
corr_nm = calc_correspondence_matrix(xwarped_nd, y_md, r=rads[i], p=.2)
wt_n = corr_nm.sum(axis=1)
goodn = wt_n > .1
targ_Nd = np.dot(corr_nm[goodn, :]/wt_n[goodn][:,None], y_md)
x_Nd = x_nd[goodn]
f = fit_ThinPlateSpline_RotReg(x_Nd, targ_Nd, wt_n=wt_n[goodn], bend_coef = regs[i], rot_coefs = rots[i], scale_coef=scales[i])
if return_full:
info = {}
info["corr_nm"] = corr_nm
info["goodn"] = goodn
info["x_Nd"] = x_nd[goodn,:]
info["targ_Nd"] = targ_Nd
info["wt_N"] = wt_n[goodn]
info["cost"] = tps.tps_cost(f.lin_ag, f.trans_g, f.w_ng, x_Nd, targ_Nd, regs[-1])
return f, info
else:
return f
