def update_tilt(dev, ctx):
global state, state_wall_time
if not keep_running:
raise freenect.Kill
freenect.update_tilt_state(dev)
state = freenect.get_tilt_state(dev)
state_wall_time = time.time()
def body(dev, ctx):
"""
Execution body for the runloop, manages execution init for Kinect-related parameters.
Terminates all running processes to safely shutdown device services.
"""
global tilt_angle, state_file, tr_counter, file_manager
#Initializes tilt_angle variable for motor control
if tilt_angle == None:
freenect.update_tilt_state(dev)
tilt_angle = freenect.get_tilt_degs(freenect.get_tilt_state(dev))
#If the movement variable is updated, moves the motor
if not freenect.get_tilt_degs(freenect.get_tilt_state(dev)) == tilt_angle:
freenect.set_tilt_degs(dev, tilt_angle)
freenect.update_tilt_state(dev)
#Exit condition for finishing execution
if not keep_running:
del file_manager
cv.destroyAllWindows() # Kills all windows
raise freenect.Kill() # Shutdowns all Kinect's services
def body(dev,ctx):
"""
Execution body for the runloop, manages execution init for Kinect-related parameters.
Terminates all running processes to safely shutdown device services.
"""
global tilt_angle,state_file,tr_counter,file_manager
#Initializes tilt_angle variable for motor control
if tilt_angle==None:
freenect.update_tilt_state(dev)
tilt_angle=freenect.get_tilt_degs(freenect.get_tilt_state(dev))
#If the movement variable is updated, moves the motor
if not freenect.get_tilt_degs(freenect.get_tilt_state(dev))==tilt_angle:
freenect.set_tilt_degs(dev, tilt_angle)
freenect.update_tilt_state(dev)
#Exit condition for finishing execution
if not keep_running:
del file_manager
cv.destroyAllWindows() # Kills all windows
raise freenect.Kill() # Shutdowns all Kinect's services
def main():
grip_dir = os.environ['GRIP_DATA']
global g_model
g_model = IO.load(os.path.join(grip_dir, 'aam.new.io'))[1]
global g_predictor, reference_3d, geo_vs, geo_vts, rect
rect = None
pred_fn = os.path.join(grip_dir, 'pred.new.io')
g_predictor = Face.load_predictor(pred_fn) #, cutOff=15)
reference_shape = g_predictor['ref_shape']
size = reference_shape.shape[0]
geo_vs = np.zeros((size, 3), dtype=np.float32)
geo_vs[:size, :2] = reference_shape
geo_vts = np.zeros((size, 2), dtype=np.float32)
geo_vts[:size] = reference_shape + 0.5
geo_ts = np.array([[1, 0, 0, 0], [0, 1, 0, 1000], [0, 0, 1, 0]],
dtype=np.float32)
geo_fs = Face.triangulate_2D(reference_shape)
geo_bs = []
for p0, p1, p2 in geo_fs:
geo_bs.append((p0, p1))
geo_bs.append((p1, p2))
geo_bs.append((p2, p0))
reference_3d = np.zeros((reference_shape.shape[0], 3), dtype=np.float32)
reference_3d[:, :2] = reference_shape * [100, 100]
img_vs = np.array([[0, 0, 0], [640, 0, 0], [640, 480, 0], [0, 480, 0]],
dtype=np.float32)
img_vts = np.array([[0, 1], [1, 1], [1, 0], [0, 0]], dtype=np.float32)
img_fs = np.array([[0, 1, 2, 3]], dtype=np.int32)
img_ts = np.array([[1, 0, 0, 0], [0, 1, 0, 1000], [0, 0, 1, 0]],
dtype=np.float32)
geo_mesh = GLMeshes(names=['geo_mesh'],
verts=[geo_vs],
faces=[geo_fs],
transforms=[geo_ts],
bones=[geo_bs],
vts=[geo_vts])
img_mesh = GLMeshes(names=['img_mesh'],
verts=[img_vs],
faces=[img_fs],
transforms=[img_ts],
bones=[None],
vts=[img_vts])
kinect = freenect.init()
tilt, roll = 0, 0
if 1:
kdev = freenect.open_device(kinect, 0)
freenect.set_led(kdev, 0) # turn off LED
freenect.set_tilt_degs(kdev, 25)
kstate = freenect.get_tilt_state(kdev)
freenect.update_tilt_state(kdev)
tilt_angle, tilt_status = kstate.tilt_angle, kstate.tilt_status
ax, ay, az = kstate.accelerometer_x, kstate.accelerometer_y, kstate.accelerometer_z
#bottom facing down: (85, 743, 369, 52, 0)
#right side down: (916, 71, 96, 112, 0)
#front side down: (52, 63, -863, -128, 0)
freenect.close_device(kdev)
y_axis = np.array((ax, ay, az), dtype=np.float32)
y_axis = y_axis / np.linalg.norm(y_axis)
roll = np.degrees(np.arctan2(ax, ay))
tilt = -np.degrees(np.arctan2(az, (ax**2 + ay**2)**0.5))
fovX = 62.0
pan_tilt_roll = (0, tilt, roll)
tx_ty_tz = (0, 1000, 6000)
P = Calibrate.composeP_fromData((fovX, ), (pan_tilt_roll), (tx_ty_tz), 0)
global g_camera_rays, g_camera_mat
h, w = 480 // 2, 640 // 2
coord, pix_coord = make_coords(h, w)
#P = np.eye(3,4,dtype=np.float32)
#P[0,0] = P[1,1] = 2.0
k1, k2 = 0, 0
g_camera_mat = Calibrate.makeMat(P, (k1, k2), [w, h])
K, RT, P, ks, T, wh = g_camera_mat
coord_undist = coord.copy()
Calibrate.undistort_points_mat(coord.reshape(-1, 2), g_camera_mat,
coord_undist.reshape(-1, 2))
g_camera_rays = np.dot(coord_undist,
RT[:2, :3]) # ray directions (unnormalized)
g_camera_rays -= np.dot([-K[0, 2], -K[1, 2], K[0, 0]], RT[:3, :3])
g_camera_rays /= (np.sum(g_camera_rays**2, axis=-1)**0.5).reshape(
h, w, 1) # normalized ray directions
names = ['kinect']
vs = [np.zeros((h * w, 3), dtype=np.float32)]
ts = [np.eye(3, 4, dtype=np.float32)]
vts = [pix_coord * (1.0 / w, 1.0 / h)]
faces = [make_faces(h, w)]
mats = None
geom_mesh = GLMeshes(names=names,
verts=vs,
faces=faces,
transforms=ts,
vts=vts)
layers = {
'geom_mesh': geom_mesh,
'geo_mesh': geo_mesh,
'img_mesh': img_mesh
}
QGLViewer.makeViewer(layers=layers,
mats=mats,
callback=cb,
timeRange=(0, 10000))
def update_tilt(dev, ctx):
global state, state_wall_time
freenect.update_tilt_state(dev)
state = freenect.get_tilt_state(dev)
state_wall_time = time.time()