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