def test_sqpregrot(src, target, bend_coeff=0.001, rot_coeff=np.array((0.0001, 0.0001, 0.000025)), scale_coeff=0.0001, corres_coeff=10): start = time.time() A, B, c = fit_reg_sqp(src, target, rot_coeff, scale_coeff, bend_coeff, corres_coeff, True, False) c = c.flatten() #c *= 0.1 f = registration.ThinPlateSpline() f.x_na = src f.w_ng = A f.lin_ag = B f.trans_g = c end = time.time() print colorize("SQP : took : %f seconds." % (end - start), "red", True) plotter = PlotterInit() plot_requests = plot_warping(f.transform_points, src, target, True) for req in plot_requests: plotter.request(req)
def test_tps_mix(src_clouds, target_clouds, fine=False, augment_coords=False, scale_down=500.0): """ FINE: set to TRUE if you want to plot a very fine grid. """ print colorize("Fitting tps-rpm ...", 'green', True) plotter = PlotterInit() def plot_cb(f): plot_requests = plot_warping(f.transform_points, np.concatenate(src_clouds), np.concatenate(target_clouds), fine) for req in plot_requests: plotter.request(req) start = time.time() f, info = tps_sc_multi(src_clouds, target_clouds, n_iter=20, rad_init=0.3, rad_final=0.0001, # if testing for box-holes points, rad_final=0.00001 bend_init=10, bend_final=0.000005, rot_init = (0.01,0.01,0.0025), rot_final=(0.00001,0.00001,0.0000025), scale_init=50, scale_final=0.00001, return_full=True, plotting_cb=plot_cb, plotter=plotter) print "(src, w, aff, trans) : ", f.x_na.shape, f.w_ng.shape, f.lin_ag.shape, f.trans_g.shape end = time.time() print colorize("Iterative : took : %f seconds."%(end - start), "red", True) plot_requests = plot_warping(f.transform_points,np.concatenate(src_clouds), np.concatenate(target_clouds), fine) for req in plot_requests: plotter.request(req) return f
def test_sqp_just_fit (src, target, bend_coeff=0.00001, rot_coeff=np.array((0.005,0.005,0.000025)), scale_coeff=0.0001): start = time.time() # A, B, c = fit_sqp(10*src, 10*target, np.ones((src.shape[0],1)), rot_coeff, scale_coeff, bend_coeff, False, False) # c = c.flatten() # c *= 0.1 # # f = registration.ThinPlateSpline() # f.x_na = src # f.w_ng = A # f.lin_ag = B # f.trans_g = c f = registration.fit_ThinPlateSpline(src, target, bend_coef=bend_coeff, rot_coef = 10*bend_coeff) end = time.time() print colorize("JUST FIT SQP : took : %f seconds."%(end - start), "red", True) plotter = PlotterInit() plot_requests = plot_warping(f.transform_points,src, target, True) for req in plot_requests: plotter.request(req)
def test_sqp_just_fit(src, target, bend_coeff=0.00001, rot_coeff=np.array((0.005, 0.005, 0.000025)), scale_coeff=0.0001): start = time.time() # A, B, c = fit_sqp(10*src, 10*target, np.ones((src.shape[0],1)), rot_coeff, scale_coeff, bend_coeff, False, False) # c = c.flatten() # c *= 0.1 # # f = registration.ThinPlateSpline() # f.x_na = src # f.w_ng = A # f.lin_ag = B # f.trans_g = c f = registration.fit_ThinPlateSpline(src, target, bend_coef=bend_coeff, rot_coef=10 * bend_coeff) end = time.time() print colorize("JUST FIT SQP : took : %f seconds." % (end - start), "red", True) plotter = PlotterInit() plot_requests = plot_warping(f.transform_points, src, target, True) for req in plot_requests: plotter.request(req)
def test_tps_mix(src_clouds, target_clouds, fine=False, augment_coords=False, scale_down=500.0): """ FINE: set to TRUE if you want to plot a very fine grid. """ print colorize("Fitting tps-rpm ...", 'green', True) plotter = PlotterInit() def plot_cb(f): plot_requests = plot_warping(f.transform_points, np.concatenate(src_clouds), np.concatenate(target_clouds), fine) for req in plot_requests: plotter.request(req) x_aug = {} y_aug = {} if augment_coords: for i, c in enumerate(src_clouds): x_aug[i] = np.abs(np.arange(len(c)) - len(c) / 2) / scale_down for i, c in enumerate(target_clouds): y_aug[i] = np.abs(np.arange(len(c)) - len(c) / 2) / scale_down start = time.time() f, info = tps_multi_mix( src_clouds, target_clouds, x_aug=x_aug, y_aug=y_aug, n_iter=20, rad_init=0.3, rad_final=0.0001, # if testing for box-holes points, rad_final=0.00001 bend_init=10, bend_final=0.00001, rot_init=(0.01, 0.01, 0.0025), rot_final=(0.00001, 0.00001, 0.0000025), scale_init=50, scale_final=0.00001, return_full=True, plotting_cb=plot_cb, plotter=plotter) print "(src, w, aff, trans) : ", f.x_na.shape, f.w_ng.shape, f.lin_ag.shape, f.trans_g.shape end = time.time() print colorize("Iterative : took : %f seconds." % (end - start), "red", True) plot_requests = plot_warping(f.transform_points, np.concatenate(src_clouds), np.concatenate(target_clouds), fine) for req in plot_requests: plotter.request(req) return f
def rot_reg(src, target): f = registration.fit_ThinPlateSpline_RotReg(src, target, bend_coef=.1, rot_coefs=[.1, .1, 0], scale_coef=1) print colorize("Linear part of the warping function is:\n", "blue"), f.lin_ag
def fit_and_plot_n(n=100, regrot=True, draw_plinks=True, fine=False, augment_coords=False): """ function for timing. """ sc = [np.random.randn(n,3)/100.] tc = [np.random.randn(n,3)/100.] start = time.time() if regrot: test_tps_rpm_regrot_multi(sc, tc, fine=fine, augment_coords=augment_coords) else: registration.tps_rpm(sc[0], tc[0]) end = time.time() print colorize("It took : %f seconds."%(end - start), "red", True)
def test_tps_rpm_regrot_multi(src_clouds, target_clouds, fine=False, augment_coords=False, scale_down=500.0): """ FINE: set to TRUE if you want to plot a very fine grid. """ print colorize("Fitting tps-rpm ...", 'green', True) #f = registration.fit_ThinPlateSpline_RotReg(src_cloud, target_cloud, bend_coef = 0.05, rot_coefs = [.1,.1,0], scale_coef=1) #f = registration.tps_rpm(src_cloud, target_cloud, f_init=None, n_iter=1000, rad_init=.05, rad_final=0.0001, reg_init=10, reg_final=0.01) plotter = PlotterInit() def plot_cb(f): plot_requests = plot_warping(f.transform_points, np.concatenate(src_clouds), np.concatenate(target_clouds), fine) for req in plot_requests: plotter.request(req) x_aug = {} y_aug = {} if augment_coords: for i,c in enumerate(src_clouds): x_aug[i] = np.abs(np.arange(len(c)) - len(c)/2)/scale_down for i,c in enumerate(target_clouds): y_aug[i] = np.abs(np.arange(len(c)) - len(c)/2)/scale_down # for c in src_clouds: # c[:,2] = np.abs(np.arange(len(c)) - len(c)/2)/scale_down # for c in target_clouds: # c[:,2] = np.abs(np.arange(len(c)) - len(c)/2)/scale_down start = time.time() f = registration.tps_rpm_regrot_multi(src_clouds, target_clouds, x_aug=x_aug, y_aug=y_aug, n_iter=15, n_iter_powell_init=50, n_iter_powell_final=50, rad_init=0.3, rad_final=0.000001, # if testing for box-holes points, rad_final=0.00001 bend_init=100, bend_final=0.0000001, rot_init = (0.001,0.001,0.00025), rot_final=(0.00001,0.00001,0.0000025), scale_init=10, scale_final=0.0000001, return_full=False, plotting_cb=plot_cb, plotter=plotter) print "(src, w, aff, trans) : ", f.x_na.shape, f.w_ng.shape, f.lin_ag.shape, f.trans_g.shape end = time.time() print colorize("Iterative : took : %f seconds."%(end - start), "red", True) #plot_requests = plot_warping(f.transform_points,np.concatenate(src_clouds), np.concatenate(target_clouds), fine) #for req in plot_requests: # plotter.request(req) return f
def process_request(self, req): if req['type'] == 'custom': try: f = self.plotting_funcs[req['func']] except KeyError: print colorize("No custom plotting function with name : %s. Ignoring plot request."%req['func'], "red") elif req['type'] == 'mlab': try: f = getattr(mlab, req['func']) except KeyError: print colorize("No mlab plotting function with name : %s. Ignoring plot request."%req['func'], "red") args, kwargs = req['data'] f(*args, **kwargs)
def test_tps_dtw(file_num, fine=False): (sc, tc) = load_clouds(file_num) x_nd = sc[0] y_md = tc[0] print colorize("Fitting tps-DTW ...", 'green', True) plotter = PlotterInit() def plot_cb(f): plot_requests = plot_warping(f.transform_points, x_nd, y_md, fine) for req in plot_requests: plotter.request(req) start = time.time() f, info = tps_dtw(x_nd, y_md, plot_cb=plot_cb, plotter=plotter) end = time.time() print colorize("TPS-DTW : took : %f seconds."%(end - start), "red", True) return f
def test_tps_dtw(file_num, fine=False): (sc, tc) = load_clouds(file_num) x_nd = sc[0] y_md = tc[0] print colorize("Fitting tps-DTW ...", 'green', True) plotter = PlotterInit() def plot_cb(f): plot_requests = plot_warping(f.transform_points, x_nd, y_md, fine) for req in plot_requests: plotter.request(req) start = time.time() f, info = tps_dtw(x_nd, y_md, plot_cb=plot_cb, plotter=plotter) end = time.time() print colorize("TPS-DTW : took : %f seconds." % (end - start), "red", True) return f
def process_request(self, req): if req['type'] == 'custom': try: f = self.plotting_funcs[req['func']] except KeyError: print colorize( "No custom plotting function with name : %s. Ignoring plot request." % req['func'], "red") elif req['type'] == 'mlab': try: f = getattr(mlab, req['func']) except KeyError: print colorize( "No mlab plotting function with name : %s. Ignoring plot request." % req['func'], "red") args, kwargs = req['data'] f(*args, **kwargs)
def fit_and_plot_n(n=100, regrot=True, draw_plinks=True, fine=False, augment_coords=False): """ function for timing. """ sc = [np.random.randn(n, 3) / 100.] tc = [np.random.randn(n, 3) / 100.] start = time.time() if regrot: test_tps_rpm_regrot_multi(sc, tc, fine=fine, augment_coords=augment_coords) else: registration.tps_rpm(sc[0], tc[0]) end = time.time() print colorize("It took : %f seconds." % (end - start), "red", True)
def plot_warping(f, src, target, fine=True, draw_plinks=True): """ function to plot the warping as defined by the function f. src : nx3 array target : nx3 array fine : if fine grid else coarse grid. """ print colorize("Plotting grid ...", 'blue', True) mean = np.mean(src, axis=0) print '\tmean : ', mean print '\tmins : ', np.min(src, axis=0) print '\tmaxes : ', np.max(src, axis=0) mins = mean + [-0.1, -0.1, -0.01] maxes = mean + [0.1, 0.1, 0.01] grid_lines = [] if fine: grid_lines = gen_grid2(f, mins=mins, maxes=maxes, xres=0.005, yres=0.005, zres=0.002) else: grid_lines = gen_grid(f, mins=mins, maxes=maxes) plotter_requests = [] plotter_requests.append(gen_mlab_request(mlab.clf)) plotter_requests.append(gen_custom_request('lines', lines=grid_lines, color=(0,0.5,0.3))) warped = f(src) plotter_requests.append(gen_mlab_request(mlab.points3d, src[:,0], src[:,1], src[:,2], color=(1,0,0), scale_factor=0.001)) plotter_requests.append(gen_mlab_request(mlab.points3d, target[:,0], target[:,1], target[:,2], color=(0,0,1), scale_factor=0.001)) plotter_requests.append(gen_mlab_request(mlab.points3d, warped[:,0], warped[:,1], warped[:,2], color=(0,1,0), scale_factor=0.001)) if draw_plinks: plinks = [np.c_[ps, pw].T for ps,pw in zip(src, warped)] plotter_requests.append(gen_custom_request('lines', lines=plinks, color=(0.5,0,0), line_width=2, opacity=1)) return plotter_requests
def test_sqpregrot (src, target, bend_coeff=0.001, rot_coeff=np.array((0.0001,0.0001,0.000025)), scale_coeff=0.0001, corres_coeff=10): start = time.time() A, B, c = fit_reg_sqp(src, target, rot_coeff, scale_coeff, bend_coeff, corres_coeff, True, False) c = c.flatten() #c *= 0.1 f = registration.ThinPlateSpline() f.x_na = src f.w_ng = A f.lin_ag = B f.trans_g = c end = time.time() print colorize("SQP : took : %f seconds."%(end - start), "red", True) plotter = PlotterInit() plot_requests = plot_warping(f.transform_points,src, target, True) for req in plot_requests: plotter.request(req)
def parsescene(fname): sfile = open(fname, 'r') match = re.search('run(\d*)\.txt', fname) if match: runnum = match.group(1) else: print colorize('[ERROR : Scene parser] : Not a valid scene file path.', 'red', True) # fast-forward to the first look: try: while('look' not in sfile.next()): pass except StopIteration: print colorize("[ERROR : Scene parser] : First look not found", "red", True) raise RuntimeError segments = [] currseg = getnewseg() for line in sfile: splitline = line.split() if splitline[1] != ":" : print colorize("[ERROR : Scene parser] : Error in format -- prompt missing.", "red", True) raise RuntimeError cmd = splitline[2] timestamp = float(splitline[0]) if cmd == 'points': currseg['ptimes'].append(timestamp) points, ptypes, secCounts = readpoints(sfile) if not currseg.has_key('ptypes'): currseg['ptypes'] = ptypes # this 'point_secs' is a big hack if not currseg.has_key('point_secs'): currseg['point_secs'] = secCounts currseg['points'].append(points) elif cmd == 'joints': joints = [float(j) for j in splitline[4:]] currseg['jtimes'].append(timestamp) currseg['joints'].append(joints) elif cmd == 'grab': grab = Grips.GRAB_R if splitline[3]=='r' else Grips.GRAB_L currseg['gtimes'].append(timestamp) currseg['grips'].append(grab) elif cmd == 'release': release = Grips.RELEASE_R if splitline[3]=='r' else Grips.RELEASE_L currseg['gtimes'].append(timestamp) currseg['grips'].append(release) elif cmd == 'look': segments.append(currseg) currseg = getnewseg() segments.append(currseg) # post-process each segment after reading them: for i,seg in enumerate(segments): # name each segment seg['name'] = 'run%s-seg%d'%(runnum, i) # numpy-ze the data for each segment seg['joints'] = np.array(seg['joints']) seg['points'] = np.array(seg['points']) # correct for relative transformation in bulletsim seg['points'] -= np.array((0,0, 0.05)) seg['jtimes'] = np.array(seg['jtimes']) seg['gtimes'] = np.array(seg['gtimes']) seg['ptimes'] = np.array(seg['ptimes']) # associate point-clouds with each joint-set: seg['j2ptimes'] = np.searchsorted(seg['ptimes'], seg['jtimes'], side='left') - 1 return segments
def rot_reg(src, target): f = registration.fit_ThinPlateSpline_RotReg(src, target, bend_coef = .1, rot_coefs = [.1,.1,0], scale_coef=1) print colorize("Linear part of the warping function is:\n", "blue"), f.lin_ag
import cPickle from os import path as osp from rapprentice.colorize import * fformat = "/home/ankush/sandbox444/pnpApp/run3_results/run-%d2-results.cpickle" merge_fname = "/home/ankush/sandbox444/pnpApp/run3_results/merged-results.cpickle" mergedict = {} for runnum in range(10): runinfo = cPickle.load(open(fformat % (runnum+1), 'r')) mergedict.update(runinfo) cPickle.dump(mergedict, open(merge_fname,'wb')) print colorize("Saved merged results to : " + merge_fname, "red", True)
nnN = 5 res_fname = "/home/ankush/sandbox444/pnpApp/run2_results/merged-results.cpickle" pert_scale = np.array([-0.0025, 0.005, 0.005, 5, 5, 5]) results = cPickle.load(open(res_fname, 'rb')) # separate the passed and failed runs: pass_runs = [] fail_runs = [] for item in results.iteritems(): if item[1]['result']: pass_runs.append(item[0]) else: fail_runs.append(item[0]) print colorize("Found %d pass runs, %d failed runs" %(len(pass_runs), len(fail_runs)), "green", True) fail_runs = np.sort(np.array(fail_runs, dtype='int')) pass_runs = np.sort(np.array(pass_runs, dtype='int')) # this matrix stores the top nnN nearest neighbors in the pass-runs for each failed run topN = np.empty((len(fail_runs), nnN), dtype='int') for i,frun in enumerate(fail_runs): fpert = results[frun]['perturb'] norms = np.array([np.linalg.norm((fpert - results[srun]['perturb'])/ pert_scale) for srun in pass_runs]) nearestN = pass_runs[np.argsort(norms)][:nnN] topN[i,:] = nearestN bestWarp = np.empty(len(fail_runs), dtype='int')
import cPickle from os import path as osp from rapprentice.colorize import * fformat = "/home/ankush/sandbox444/pnpApp/run3_results/run-%d2-results.cpickle" merge_fname = "/home/ankush/sandbox444/pnpApp/run3_results/merged-results.cpickle" mergedict = {} for runnum in range(10): runinfo = cPickle.load(open(fformat % (runnum + 1), 'r')) mergedict.update(runinfo) cPickle.dump(mergedict, open(merge_fname, 'wb')) print colorize("Saved merged results to : " + merge_fname, "red", True)
def parsescene(fname): sfile = open(fname, 'r') match = re.search('run(\d*)\.txt', fname) if match: runnum = match.group(1) else: print colorize('[ERROR : Scene parser] : Not a valid scene file path.', 'red', True) # fast-forward to the first look: try: while ('look' not in sfile.next()): pass except StopIteration: print colorize("[ERROR : Scene parser] : First look not found", "red", True) raise RuntimeError segments = [] currseg = getnewseg() for line in sfile: splitline = line.split() if splitline[1] != ":": print colorize( "[ERROR : Scene parser] : Error in format -- prompt missing.", "red", True) raise RuntimeError cmd = splitline[2] timestamp = float(splitline[0]) if cmd == 'points': currseg['ptimes'].append(timestamp) points, ptypes, secCounts = readpoints(sfile) if not currseg.has_key('ptypes'): currseg['ptypes'] = ptypes # this 'point_secs' is a big hack if not currseg.has_key('point_secs'): currseg['point_secs'] = secCounts currseg['points'].append(points) elif cmd == 'joints': joints = [float(j) for j in splitline[4:]] currseg['jtimes'].append(timestamp) currseg['joints'].append(joints) elif cmd == 'grab': grab = Grips.GRAB_R if splitline[3] == 'r' else Grips.GRAB_L currseg['gtimes'].append(timestamp) currseg['grips'].append(grab) elif cmd == 'release': release = Grips.RELEASE_R if splitline[ 3] == 'r' else Grips.RELEASE_L currseg['gtimes'].append(timestamp) currseg['grips'].append(release) elif cmd == 'look': segments.append(currseg) currseg = getnewseg() segments.append(currseg) # post-process each segment after reading them: for i, seg in enumerate(segments): # name each segment seg['name'] = 'run%s-seg%d' % (runnum, i) # numpy-ze the data for each segment seg['joints'] = np.array(seg['joints']) seg['points'] = np.array(seg['points']) # correct for relative transformation in bulletsim seg['points'] -= np.array((0, 0, 0.05)) seg['jtimes'] = np.array(seg['jtimes']) seg['gtimes'] = np.array(seg['gtimes']) seg['ptimes'] = np.array(seg['ptimes']) # associate point-clouds with each joint-set: seg['j2ptimes'] = np.searchsorted( seg['ptimes'], seg['jtimes'], side='left') - 1 return segments
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_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 test_tps_rpm_regrot_multi(src_clouds, target_clouds, fine=False, augment_coords=False, scale_down=500.0): """ FINE: set to TRUE if you want to plot a very fine grid. """ print colorize("Fitting tps-rpm ...", 'green', True) #f = registration.fit_ThinPlateSpline_RotReg(src_cloud, target_cloud, bend_coef = 0.05, rot_coefs = [.1,.1,0], scale_coef=1) #f = registration.tps_rpm(src_cloud, target_cloud, f_init=None, n_iter=1000, rad_init=.05, rad_final=0.0001, reg_init=10, reg_final=0.01) plotter = PlotterInit() def plot_cb(f): plot_requests = plot_warping(f.transform_points, np.concatenate(src_clouds), np.concatenate(target_clouds), fine) for req in plot_requests: plotter.request(req) x_aug = {} y_aug = {} if augment_coords: for i, c in enumerate(src_clouds): x_aug[i] = np.abs(np.arange(len(c)) - len(c) / 2) / scale_down for i, c in enumerate(target_clouds): y_aug[i] = np.abs(np.arange(len(c)) - len(c) / 2) / scale_down # for c in src_clouds: # c[:,2] = np.abs(np.arange(len(c)) - len(c)/2)/scale_down # for c in target_clouds: # c[:,2] = np.abs(np.arange(len(c)) - len(c)/2)/scale_down start = time.time() f = registration.tps_rpm_regrot_multi( src_clouds, target_clouds, x_aug=x_aug, y_aug=y_aug, n_iter=15, n_iter_powell_init=50, n_iter_powell_final=50, rad_init=0.3, rad_final=0.000001, # if testing for box-holes points, rad_final=0.00001 bend_init=100, bend_final=0.0000001, rot_init=(0.001, 0.001, 0.00025), rot_final=(0.00001, 0.00001, 0.0000025), scale_init=10, scale_final=0.0000001, return_full=False, plotting_cb=plot_cb, plotter=plotter) print "(src, w, aff, trans) : ", f.x_na.shape, f.w_ng.shape, f.lin_ag.shape, f.trans_g.shape end = time.time() print colorize("Iterative : took : %f seconds." % (end - start), "red", True) #plot_requests = plot_warping(f.transform_points,np.concatenate(src_clouds), np.concatenate(target_clouds), fine) #for req in plot_requests: # plotter.request(req) 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 plot_warping(f, src, target, fine=True, draw_plinks=True): """ function to plot the warping as defined by the function f. src : nx3 array target : nx3 array fine : if fine grid else coarse grid. """ print colorize("Plotting grid ...", 'blue', True) mean = np.mean(src, axis=0) print '\tmean : ', mean print '\tmins : ', np.min(src, axis=0) print '\tmaxes : ', np.max(src, axis=0) mins = mean + [-0.1, -0.1, -0.01] maxes = mean + [0.1, 0.1, 0.01] grid_lines = [] if fine: grid_lines = gen_grid2(f, mins=mins, maxes=maxes, xres=0.005, yres=0.005, zres=0.002) else: grid_lines = gen_grid(f, mins=mins, maxes=maxes) plotter_requests = [] plotter_requests.append(gen_mlab_request(mlab.clf)) plotter_requests.append( gen_custom_request('lines', lines=grid_lines, color=(0, 0.5, 0.3))) warped = f(src) plotter_requests.append( gen_mlab_request(mlab.points3d, src[:, 0], src[:, 1], src[:, 2], color=(1, 0, 0), scale_factor=0.001)) plotter_requests.append( gen_mlab_request(mlab.points3d, target[:, 0], target[:, 1], target[:, 2], color=(0, 0, 1), scale_factor=0.001)) plotter_requests.append( gen_mlab_request(mlab.points3d, warped[:, 0], warped[:, 1], warped[:, 2], color=(0, 1, 0), scale_factor=0.001)) if draw_plinks: plinks = [np.c_[ps, pw].T for ps, pw in zip(src, warped)] plotter_requests.append( gen_custom_request('lines', lines=plinks, color=(0.5, 0, 0), line_width=2, opacity=1)) return plotter_requests