def optimize(rgb, debug=False):
im = rgb.mean(2).astype('u1')
size = im.shape[::-1]
mask = table_calibration.make_mask(config.bg['boundpts'])
d = DividingLine(im, mask)
def error(x):
theta, dist = x
line = middle_offset(theta, dist, size)
s = 1./(d.score(line, debug) + 1e-5)
if debug:
clf()
imshow(d.debug * d.image)
pylab.waitforbuttonpress(0.01)
return s
for iteration in xrange(5):
initial = (np.random.rand()*2*pi, (2*np.random.rand()-1)*100)
r, score, _, _, _ = scipy.optimize.fmin(error, initial,
full_output=True, disp=False)
# This threshold is based on empirical observations. Might change.
if score < 0.02: break
else:
print 'Failed %d times' % iteration
raise ValueError
line = middle_offset(r[0], r[1], size)
line = np.array(line) / line[2]
res = d.traverse(line)
# Scale the line so that a positive dot product indicates the point is
# on the 'bright' side of the line
if res['p1'] / res['p0'] < res['n1'] / res['n0']:
line *= -1
return line
def finish_calibration(calib):
mask = table_calibration.make_mask(config.bg['boundpts'])
# Construct the homography from the camera to the XZ plane
Hi = np.dot(config.bg['Ktable'], config.bg['KK'])
Hi = np.linalg.inv(Hi)
Hi = Hi[(0,1,3),:][:,(0,2,3)]
L_table = []
L_image = []
for i, (L, rgb, depth) in enumerate(calib):
Li = optimize(rgb)
L_table.append(L)
L_image.append(Li)
L_table, L_image = map(np.array, (L_table, L_image))
# We need to solve for M.
# P_image = Hi * M * P_table
# L_table * P_table = 0 when P_table is on L.
# L_image * P_image = 0 when P_image is on L.
# So,
# L_image = L_table * np.linalg.inv(Hi * M)
# L_table = L_image * Hi * M
# At this point, we could solve by constructing an Ax = 0 and
# computing SVD (also known as the DLT method)
# But since we know we're only looking for rigid transformations,
# we can solve for rotation and translation separately.
# To solve for rotation, lets look at the vanishing points.
normalize2 = lambda x: x / np.sqrt((x[:2]**2).sum())
V_table = np.array([normalize2(x[:2]) for x in L_table])
V_image = np.array([normalize2(x[:2]) for x in np.dot(L_image, Hi)])
V = np.dot(V_table.T, V_image)
assert V.shape == (2,2)
u,s,v = np.linalg.svd(V)
R = np.dot(v,u.T)
within_eps = lambda a, b: np.all(np.abs(a-b) < 1e-2)
assert within_eps(np.dot(R, V_table.T), V_image.T)
R_ = np.eye(3)
R_[:2,:2] = R
# Now we need to solve for the two translation parameters.
# First lets normalize the lines so that the C's are the same.
L_i = np.array(map(normalize2, np.dot(L_image, np.dot(Hi, R_))))
L_t = np.array(map(normalize2, np.dot(L_table, np.eye(3))))
A = L_i[:,:2]
b = L_t[:,2] - L_i[:,2]
x, _, _, _ = np.linalg.lstsq(A, b)
R_ = R_.T
R_[:2,2] = -x.T
R_ = np.linalg.inv(R_)
L_final = np.array(map(normalize2, np.dot(L_image, np.dot(Hi, R_))))
L_gold = np.array(map(normalize2, L_table))
assert within_eps(L_final, L_gold)
M = np.eye(4)
M[0,(0,2,3)] = R_[0,:]
M[2,(0,2,3)] = R_[1,:]
# If we've made it this far, then we can patch up the config
print M
config.bg['Ktable'] = np.dot(np.linalg.inv(M), config.bg['Ktable']).astype('f')
config.save('data/newest_calibration')
return M
