def main():
model_folder = sys.argv[1]
with open(model_folder + '/classic.poly') as f:
model = pickle.load(f)
mosaic = TagMosaic(0.0254)
for imfile in sys.argv[2:]:
im = imread(imfile)
im = rgb2gray(im)
detections = [ TagDetectionEx(d) for d in get_tag_detections(im) ]
# Rectify tag detections and store in `c1` attribute
det_i = np.array([ d.c0 for d in detections ])
undist = np.array([ model.undistort(p) for p in det_i ])
for d, r in zip(detections, undist):
d.c1 = np.add(d.c0, r)
def line_fit_sqerr(points):
""" smallest eigen value of covariance """
cov = np.cov(points)
return np.linalg.eig(cov)[0].min()
# Row-wise average residual
tag_row = lambda d: mosaic.get_row(d.id)
detections.sort(key=tag_row)
sq_errs = []
for _, group in groupby(detections, key=tag_row):
rectified = np.array([ d.c1 for d in group ])
if len(rectified) < 3: continue
sq_errs.append( line_fit_sqerr(rectified.T) )
sq_errs = np.array(sq_errs)
row_rmse, row_rmaxse = np.sqrt(sq_errs.mean()), np.sqrt(sq_errs.max())
# Col-wise average residual
tag_col = lambda d: mosaic.get_col(d.id)
detections.sort(key=tag_col)
sq_errs = []
for _, group in groupby(detections, key=tag_col):
rectified = np.array([ d.c1 for d in group ])
if len(rectified) < 3: continue
sq_errs.append( line_fit_sqerr(rectified.T) )
sq_errs = np.array(sq_errs)
col_rmse, col_rmaxse = np.sqrt(sq_errs.mean()), np.sqrt(sq_errs.max())
print '[ %s, %.2f, %.2f ],' % (
imfile.split('/')[-2],
np.sqrt(row_rmse**2 + col_rmse**2),
max(row_rmaxse, col_rmaxse))
def main():
model_folder = sys.argv[1]
with open(model_folder + '/pose0.gp') as f:
gpmodel = pickle.load(f)
mosaic = TagMosaic(0.0254)
for imfile in sys.argv[2:]:
im = imread(imfile)
im = rgb2gray(im)
detections = [ TagDetectionEx(d) for d in get_tag_detections(im) ]
# Rectify tag detections and store in `c1` attribute
det_i = np.array([ d.c0 for d in detections ])
undist = gpmodel.predict(det_i)
for d, r in zip(detections, undist):
d.c1 = np.add(d.c0, r)
def line_fit_sqerr(points):
""" smallest eigen value of covariance """
cov = np.cov(points)
return np.linalg.eig(cov)[0].min()
# Row-wise average residual
tag_row = lambda d: mosaic.get_row(d.id)
detections.sort(key=tag_row)
sq_errs = []
for _, group in groupby(detections, key=tag_row):
rectified = np.array([ d.c1 for d in group ])
if len(rectified) < 3: continue
sq_errs.append( line_fit_sqerr(rectified.T) )
sq_errs = np.array(sq_errs)
row_rmse, row_rmaxse = np.sqrt(sq_errs.mean()), np.sqrt(sq_errs.max())
# Col-wise average residual
tag_col = lambda d: mosaic.get_col(d.id)
detections.sort(key=tag_col)
sq_errs = []
for _, group in groupby(detections, key=tag_col):
rectified = np.array([ d.c1 for d in group ])
if len(rectified) < 3: continue
sq_errs.append( line_fit_sqerr(rectified.T) )
sq_errs = np.array(sq_errs)
col_rmse, col_rmaxse = np.sqrt(sq_errs.mean()), np.sqrt(sq_errs.max())
print '[ %d, %.2f, %.2f ],' % (
int(imfile.split('/')[-2]),
np.sqrt(row_rmse**2 + col_rmse**2),
max(row_rmaxse, col_rmaxse))
def main():
model_folder = sys.argv[1]
test_image = sys.argv[2]
zoom_levels = [
float(f.split('/')[-2]) for f in iglob(model_folder + '/*/')
]
zoom_levels = np.array(sorted(zoom_levels))
test_zoom = float(test_image.split('/')[-2])
test_zoom_pos = np.searchsorted(zoom_levels, test_zoom)
zoom0 = zoom_levels[test_zoom_pos - 1]
zoom1 = zoom_levels[test_zoom_pos]
with open('%s/%03d/classic.poly' % (model_folder, zoom0)) as f:
model0 = pickle.load(f)
with open('%s/%03d/classic.poly' % (model_folder, zoom1)) as f:
model1 = pickle.load(f)
def interpolate_predict(query_points):
predict0 = np.array([model0.undistort(q) for q in query_points])
predict1 = np.array([model1.undistort(q) for q in query_points])
t = (zoom1 - test_zoom) / (zoom1 - zoom0)
return t * predict0 + (1. - t) * predict1
im = imread(test_image)
im = rgb2gray(im)
detections = [TagDetectionEx(d) for d in get_tag_detections(im)]
# Rectify tag detections and store in `c1` attribute
det_i = np.array([d.c0 for d in detections])
undist = interpolate_predict(det_i)
for d, r in zip(detections, undist):
d.c1 = np.add(d.c0, r)
def line_fit_sqerr(points):
""" smallest eigen value of covariance """
cov = np.cov(points)
return np.linalg.eig(cov)[0].min()
mosaic = TagMosaic(0.0254)
# Row-wise average residual
tag_row = lambda d: mosaic.get_row(d.id)
detections.sort(key=tag_row)
sq_errs = []
for _, group in groupby(detections, key=tag_row):
rectified = np.array([d.c1 for d in group])
if len(rectified) < 3: continue
sq_errs.append(line_fit_sqerr(rectified.T))
sq_errs = np.array(sq_errs)
row_rmse, row_rmaxse = np.sqrt(sq_errs.mean()), np.sqrt(sq_errs.max())
# Col-wise average residual
tag_col = lambda d: mosaic.get_col(d.id)
detections.sort(key=tag_col)
sq_errs = []
for _, group in groupby(detections, key=tag_col):
rectified = np.array([d.c1 for d in group])
if len(rectified) < 3: continue
sq_errs.append(line_fit_sqerr(rectified.T))
sq_errs = np.array(sq_errs)
col_rmse, col_rmaxse = np.sqrt(sq_errs.mean()), np.sqrt(sq_errs.max())
print '( %d, %.2f, %.2f ),' % (int(
test_image.split('/')[-2]), np.sqrt(row_rmse**2 + col_rmse**2),
max(row_rmaxse, col_rmaxse))
def process(filename):
#
# Conventions:
# a_i, b_i
# are variables in image space, units are pixels
# a_w, b_w
# are variables in world space, units are meters
#
print '\n========================================'
print ' File: ' + filename
print '========================================\n'
im = imread(filename)
im = rgb2gray(im)
im = (im * 255.).astype(np.uint8)
tag_mosaic = TagMosaic(0.0254)
detections = AprilTagDetector().detect(im)
print ' %d tags detected.' % len(detections)
#
# Sort detections by distance to center
#
c_i = np.array([im.shape[1], im.shape[0]]) / 2.
dist = lambda p_i: np.linalg.norm(p_i - c_i)
closer_to_center = lambda d1, d2: int(dist(d1.c) - dist(d2.c))
detections.sort(cmp=closer_to_center)
mosaic_pos = lambda det: tag_mosaic.get_position_meters(det.id)
det_i = np.array([ d.c for d in detections ])
det_w = np.array([ mosaic_pos(d) for d in detections ])
#
# To learn a weighted local homography, we find the weighting
# function parameters that minimize reprojection error across
# leave-one-out validation folds of the data. Since the
# homography is local at the center, we only use 9 nearest
# detections to the center
#
det_i9 = det_i[:9]
det_w9 = det_w[:9]
def local_homography_loocv_error(theta, args):
src, tgt = args
errs = [ local_homography_error(theta, src[t_ix], tgt[t_ix], src[v_ix], tgt[v_ix])
for t_ix, v_ix in LeaveOneOut(len(src)) ]
return np.mean(errs)
def learn_homography_i2w():
result = minimize( local_homography_loocv_error,
x0=[ 50, 1, 1e-3 ],
args=[ det_i9, det_w9 ],
method='Powell',
options={'ftol': 1e-3} )
print '\nHomography: i->w'
print '------------------'
print ' params:', result.x
print ' rmse: %.6f' % sqrt(result.fun)
print '\n Optimization detail:'
print ' ' + str(result).replace('\n', '\n ')
H = create_local_homography_object(*result.x)
for i, w in zip(det_i9, det_w9):
H.add_correspondence(i, w)
return H
def learn_homography_w2i():
result = minimize( local_homography_loocv_error,
x0=[ 0.0254, 1, 1e-3 ],
method='Powell',
args=[ det_w9, det_i9 ],
options={'ftol': 1e-3} )
print '\nHomography: w->i'
print '------------------'
print ' params:', result.x
print ' rmse: %.6f' % sqrt(result.fun)
print '\n Optimization detail:'
print ' ' + str(result).replace('\n', '\n ')
H = create_local_homography_object(*result.x)
for w, i in zip(det_w9, det_i9):
H.add_correspondence(w, i)
return H
#
# We assume that the distortion is zero at the center of
# the image and we are interesting in the word to image
# homography at the center of the image. However, we don't
# know the center of the image in world coordinates.
# So we follow a procedure as explained below:
#
# First, we learn a homography from image to world
# Next, we find where the image center `c_i` maps to in
# world coordinates (`c_w`). Finally, we find the local
# homography `LH0` from world to image at `c_w`
#
H_iw = learn_homography_i2w()
c_i = np.array([im.shape[1], im.shape[0]]) / 2.
c_w = H_iw.map(c_i)[:2]
H_wi = learn_homography_w2i()
LH0 = H_wi.get_homography_at(c_w)
print '\nHomography at center'
print '----------------------'
print ' c_w =', c_w
print ' c_i =', c_i
print 'LH0 * c_w =', H_wi.map(c_w)
#
# Obtain distortion estimate
# detected + undistortion = mapped
# (or) undistortion = mapped - detected
#
def homogeneous_coords(arr):
return np.hstack([ arr, np.ones((len(arr), 1)) ])
mapped_i = LH0.dot(homogeneous_coords(det_w).T).T
mapped_i = np.array([ p / p[2] for p in mapped_i ])
mapped_i = mapped_i[:,:2]
undistortion = mapped_i - det_i # image + undistortion = mapped
max_distortion = np.max([np.linalg.norm(u) for u in undistortion])
print '\nMaximum distortion is %.2f pixels' % max_distortion
#
# Fit non-parametric model to the observations
#
model = GPModel(det_i, undistortion)
print '\nGP Hyper-parameters'
print '---------------------'
print ' x: ', model._gp_x._covf.theta
print ' log-likelihood: %.4f' % model._gp_x.model_evidence()
print ' y: ', model._gp_y._covf.theta
print ' log-likelihood: %.4f' % model._gp_y.model_evidence()
print ''
print ' Optimization detail:'
print ' [ x ]'
print ' ' + str(model._gp_x.fit_result).replace('\n', '\n ')
print ' [ y ]'
print ' ' + str(model._gp_y.fit_result).replace('\n', '\n ')
#
# Visualization
#
from skimage.filters import scharr
from matplotlib import pyplot as plt
plt.style.use('ggplot')
plt.figure(figsize=(16,10))
#__1__
plt.subplot(221)
plt.title(filename.split('/')[-1])
plt.imshow(im, cmap='gray')
plt.plot(det_i[:,0], det_i[:,1], 'o')
for i, d in enumerate(detections):
plt.text(d.c[0], d.c[1], str(i),
fontsize=8, color='white', bbox=dict(facecolor='maroon', alpha=0.75))
plt.grid()
plt.axis('equal')
#__2__
plt.subplot(222)
plt.title('Non-linear Homography')
if False:
plt.imshow(1.-scharr(im), cmap='bone', interpolation='gaussian')
from collections import defaultdict
row_groups = defaultdict(list)
col_groups = defaultdict(list)
for d in detections:
row, col = tag_mosaic.get_row(d.id), tag_mosaic.get_col(d.id)
row_groups[row] += [ d.id ]
col_groups[col] += [ d.id ]
for k, v in row_groups.iteritems():
a = tag_mosaic.get_position_meters(np.min(v))
b = tag_mosaic.get_position_meters(np.max(v))
x_coords = np.linspace(a[0], b[0], 100)
points = np.array([ H_wi.map([x, a[1]]) for x in x_coords ])
plt.plot(points[:,0], points[:,1], '-',color='#CF4457', linewidth=2)
for k, v in col_groups.iteritems():
a = tag_mosaic.get_position_meters(np.min(v))
b = tag_mosaic.get_position_meters(np.max(v))
y_coords = np.linspace(a[1], b[1], 100)
points = np.array([ H_wi.map([a[0], y]) for y in y_coords ])
plt.plot(points[:,0], points[:,1], '-',color='#CF4457', linewidth=2)
plt.plot(det_i[:,0], det_i[:,1], 'kx')
plt.grid()
plt.axis('equal')
#__3__
plt.subplot(223)
plt.title('Qualitative Estimates')
for i, d in enumerate(detections):
plt.text(d.c[0], d.c[1], str(i), fontsize=8, color='#999999')
X, Y = det_i[:,0], det_i[:,1]
U, V = undistortion[:,0], undistortion[:,1]
plt.quiver(X, Y, U, -V, units='dots')
# plt.quiver(X, Y, U, -V, angles='xy', scale_units='xy', scale=1) # plot exact
plt.text( 0.5, 0, 'max observed distortion: %.2f px' % max_distortion, color='#CF4457', fontsize=10,
horizontalalignment='center', verticalalignment='bottom', transform=plt.gca().transAxes)
plt.gca().invert_yaxis()
plt.axis('equal')
#__4__
plt.subplot(224)
plt.title('Qualitative Undistortion')
H, W = im.shape
grid = np.array([[x, y] for y in xrange(0, H, 80) for x in xrange(0, W, 80)])
predicted = model.predict(grid)
X, Y = grid[:,0], grid[:,1]
U, V = predicted[:,0], predicted[:,1]
plt.quiver(X, Y, U, -V, units='dots')
#plt.quiver(X, Y, U, -V, angles='xy', scale_units='xy', scale=1)) # plot exact
plt.gca().invert_yaxis()
plt.axis('equal')
plt.tight_layout()
plt.gcf().patch.set_facecolor('#dddddd')
#plt.show()
plt.savefig(filename + '.svg', bbox_inches='tight')
def main():
model_folder = sys.argv[1]
test_image = sys.argv[2]
zoom_levels = [ float(f.split('/')[-2]) for f in iglob(model_folder + '/*/') ]
zoom_levels = np.array(sorted(zoom_levels))
test_zoom = float(test_image.split('/')[-2])
test_zoom_pos = np.searchsorted(zoom_levels, test_zoom)
zoom0 = zoom_levels[test_zoom_pos-1]
zoom1 = zoom_levels[test_zoom_pos]
with open('%s/%03d/pose0.gp' % (model_folder,zoom0)) as f:
gpmodel0 = pickle.load(f)
with open('%s/%03d/pose0.gp' % (model_folder,zoom1)) as f:
gpmodel1 = pickle.load(f)
def interpolate_predict(query_points):
predict0 = gpmodel0.predict(query_points)
predict1 = gpmodel1.predict(query_points)
t = (zoom1-test_zoom) / (zoom1-zoom0)
return t*predict0 + (1.-t)*predict1
im = imread(test_image)
im = rgb2gray(im)
detections = [ TagDetectionEx(d) for d in get_tag_detections(im) ]
# Rectify tag detections and store in `c1` attribute
det_i = np.array([ d.c0 for d in detections ])
undist = interpolate_predict(det_i)
for d, r in zip(detections, undist):
d.c1 = np.add(d.c0, r)
def line_fit_sqerr(points):
""" smallest eigen value of covariance """
cov = np.cov(points)
return np.linalg.eig(cov)[0].min()
mosaic = TagMosaic(0.0254)
# Row-wise average residual
tag_row = lambda d: mosaic.get_row(d.id)
detections.sort(key=tag_row)
sq_errs = []
for _, group in groupby(detections, key=tag_row):
rectified = np.array([ d.c1 for d in group ])
if len(rectified) < 3: continue
sq_errs.append( line_fit_sqerr(rectified.T) )
sq_errs = np.array(sq_errs)
row_rmse, row_rmaxse = np.sqrt(sq_errs.mean()), np.sqrt(sq_errs.max())
# Col-wise average residual
tag_col = lambda d: mosaic.get_col(d.id)
detections.sort(key=tag_col)
sq_errs = []
for _, group in groupby(detections, key=tag_col):
rectified = np.array([ d.c1 for d in group ])
if len(rectified) < 3: continue
sq_errs.append( line_fit_sqerr(rectified.T) )
sq_errs = np.array(sq_errs)
col_rmse, col_rmaxse = np.sqrt(sq_errs.mean()), np.sqrt(sq_errs.max())
print '[ %d, %.2f, %.2f ],' % (
int(test_image.split('/')[-2]),
np.sqrt(row_rmse**2 + col_rmse**2),
max(row_rmaxse, col_rmaxse))
def main():
matplotlib.rcParams.update({'font.size': 8})
model_folder = sys.argv[1]
with open(model_folder + '/pose0.gp') as f:
gpmodel = pickle.load(f)
mosaic = TagMosaic(0.0254)
for num, imfile in enumerate(sys.argv[2:14]):
im = imread(imfile)
im = rgb2gray(im)
detections = [ TagDetectionEx(d) for d in get_tag_detections(im) ]
print imfile
# Rectify tag detections and store in `c1` attribute
det_i = np.array([ d.c0 for d in detections ])
undist = gpmodel.predict(det_i)
rectified = det_i + undist
for d, r in zip(detections, undist):
d.c1 = np.add(d.c0, r)
plt.subplot(3, 4, num+1)
plt.axis('off')
plt.plot(det_i[:,0], det_i[:,1], 'o', markersize=3, markeredgecolor='b', markerfacecolor='c')
plt.plot(rectified[:,0], rectified[:,1], 'ko', markersize=3)
def line_fit_sqerr(points):
""" smallest eigen value of covariance """
cov = np.cov(points)
return np.linalg.eig(cov)[0].min()
# Plot rows
tag_row = lambda d: mosaic.get_row(d.id)
detections.sort(key=tag_row)
sq_errs = []
colors = cycle(['#922428', '#CC2529'])
for _, group in groupby(detections, key=tag_row):
rectified = np.array([d.c1 for d in group])
plt.plot(rectified[:,0], rectified[:,1], '-', color=colors.next())
if len(rectified) < 3: continue
sq_errs.append( line_fit_sqerr(rectified.T) )
sq_errs = np.array(sq_errs)
row_rmse = np.sqrt(sq_errs.max())
# Plot cols
tag_col = lambda d: mosaic.get_col(d.id)
detections.sort(key=tag_col)
sq_errs = []
colors = cycle(['#6B4C9A', '#396AB1'])
for _, group in groupby(detections, key=tag_col):
rectified = np.array([d.c1 for d in group])
plt.plot(rectified[:,0], rectified[:,1], '-', color=colors.next())
if len(rectified) < 3: continue
sq_errs.append( line_fit_sqerr(rectified.T) )
sq_errs = np.array(sq_errs)
col_rmse = np.sqrt(sq_errs.max())
plt.title( '%s (%.1f, %.1f)' % (imfile.split('/')[-1], row_rmse, col_rmse) )
plt.show()