def cb(frame):
global g_record, g_frame
g_frame = frame
global g_camera_rays, g_camera_mat
#print 'in cb'
img = freenect.sync_get_video()[0]
geom_mesh = QApp.app.getLayer('geom_mesh')
geom_mesh.setImage(img)
if 1:
depths = freenect.sync_get_depth(format=freenect.DEPTH_REGISTERED)[0]
#print 'depths',np.median(depths)
if 0: # recording
if frame not in g_record: return
img, depths = g_record[frame]['video'], g_record[frame]['depths']
g_record[frame] = {'video': img.copy(), 'depths': depths.copy()}
if frame == 99: IO.save('dump', g_record)
depths_sum = np.array(depths != 0, dtype=np.int32)
lookup = np.array([0, 1, 0.5, 1.0 / 3, 0.25], dtype=np.float32)
if 1: # average
depths_lo = np.array(depths[::2, ::2] + depths[1::2, ::2] +
depths[::2, 1::2] + depths[1::2, 1::2],
dtype=np.float32)
depths_lo = depths_lo * lookup[
(depths_sum[::2, ::2] + depths_sum[1::2, ::2] +
depths_sum[::2, 1::2] +
depths_sum[1::2, 1::2]).reshape(-1)].reshape(depths_lo.shape)
else: # fullsize
depths_lo = depths * lookup[depths_sum.reshape(-1)].reshape(
depths_lo.shape)
K, RT, P, ks, T, wh = g_camera_mat
vs = depths_to_points(g_camera_rays, T, depths_lo)
geom_mesh.setVs(vs.reshape(-1, 3))
#QApp.view().setImage(img, img.shape[0], img.shape[1], img.shape[2])
#camera = QApp.view().camera
#geom_mesh.image = camera.image
#geom_mesh.bindImage = camera.bindImage
#geom_mesh.bindId = camera.bindId
global g_predictor, reference_3d, geo_vs, geo_vts
h, w, _3 = img.shape
global g_prev_vs
try:
g_prev_vs
except:
g_prev_vs = None
use_prev_vs = True
if g_prev_vs is None:
reference_3d[:, :2] = g_predictor['ref_shape'] * [100, 100]
tmp = Face.detect_face(img,
g_predictor) if g_prev_vs is None else g_prev_vs
tmp = Face.track_face(img, g_predictor, tmp)
if use_prev_vs: g_prev_vs = tmp
if frame == 0 or Face.test_reboot(img, g_prev_vs): g_prev_vs = None
geo_vts[:len(tmp)] = tmp
geo_vts[:, 1] = img.shape[0] - geo_vts[:, 1]
current_shape = geo_vts[:len(tmp)].copy()
if 1:
ds = extract_depths(vs, current_shape * 0.5)
M, inliers = Calibrate.rigid_align_points_inliers(ds,
reference_3d,
scale=True,
threshold_ratio=5.0)
ds = np.dot(ds, M[:3, :3].T) + M[:, 3]
which = np.where(np.sum((reference_3d - ds)**2, axis=1) < 100 * 100)[0]
reference_3d[which] = reference_3d[which] * 0.99 + ds[which] * 0.01
reference_3d[
inliers] = reference_3d[inliers] * 0.95 + ds[inliers] * 0.05
ds[:] = reference_3d[:]
M[1, 3] += 1000
M[0, 3] -= 300
else:
M = np.eye(3, 4, dtype=np.float32)
M[1, 3] += 1000
geom_mesh.setPose(M.reshape(1, 3, 4))
ref_pinv = g_predictor['ref_pinv']
xform = np.dot(ref_pinv, current_shape)
ut, s, v = np.linalg.svd(xform)
s = (s[0] * s[1])**-0.5
xform_inv = np.dot(v.T, ut.T) * s
current_shape = np.dot(current_shape - np.mean(current_shape, axis=0),
xform_inv) * 100.
geo_vs[:] = 0
geo_vs[:len(current_shape), :2] = current_shape
geo_vs[:70] = reference_3d
#geo_vs[:68,:] += [0,100,5500]
#print geo_vts[:4],w,h
geo_mesh = QApp.app.getLayer('geo_mesh')
geo_mesh.setVs(geo_vs,
vts=geo_vts *
np.array([1.0 / w, 1.0 / h], dtype=np.float32))
geo_mesh.setImage(img)
#geo_mesh.transforms[0][:,:3] = [[1,0,0],[0,1,0],[0,0,1],[0,1000,0.1]]
if 1:
global g_model
w, h = 160, 160
shp = geo_vs[:68, :2]
shape_u, tex_u, A_inv, mn = Face.fit_aam(g_model, tmp, img)
Face.render_aam(g_model, A_inv * 0.5, mn * 0.5, shape_u, tex_u, img)
img_mesh = QApp.app.getLayer('img_mesh')
img_mesh.setImage(img)
QApp.view().updateGL()
x2d_threshold = 0.08 # - it * 0.04/50.
Ps = np.array([m[2] / (m[0][0, 0]) for m in mats], dtype=np.float32)
u2s, _ = Calibrate.undistort_dets(x2s, x2s_splits, mats)
x3s, x3s_labels, E, x2d_labels = Recon.solve_x3ds(
u2s, x2s_splits, x2s_labels_original, Ps, True)
clouds = ISCV.HashCloud2DList(u2s, x2s_splits, x2d_threshold)
sc, x2s_labels, _ = Label.project_assign(clouds, x3s, x3s_labels, Ps,
x2d_threshold)
print 'it', it, sc
tiara_xis = np.where(x3s_labels < len(VICON_tiara_x3ds))[0]
tiara_lis = x3s_labels[tiara_xis]
tiara_true = VICON_tiara_x3ds[tiara_lis] + [0, 1000, 0]
tiara_xs = x3s[tiara_xis]
# now solve the tiara into place by finding a rigid transform
RT, inliers = Calibrate.rigid_align_points_inliers(tiara_xs,
tiara_true,
scale=True)
x3s = np.dot(x3s, RT[:3, :3].T) + RT[:, 3]
x3s[tiara_xis] = tiara_true
singles = np.where([x in list(x2d_labels) for x in x2s_labels])[0]
x2s_labels[singles] = -1
for ci, P in enumerate(
Ps): # first pass: solve cameras from 2d-3d correspondences
x2s_which = np.where(
map(lambda x: x != -1,
x2s_labels[x2s_splits[ci]:x2s_splits[ci + 1]])
)[0] + x2s_splits[ci]
xls = x2s_labels[x2s_which]
x3s_which = [list(x3s_labels).index(xi) for xi in xls]
cv2_mat = Calibrate.cv2_solve_camera_from_3d(
x3s[x3s_which],