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
