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