def calib_projector(motive_filename, projector_filename, points, fps):
# Verify inputs
projector_filename = projector_filename.encode()
if not path.splitext(projector_filename)[1]:
projector_filename += '.pickle'
motive_filename = motive_filename.encode()
# Collect Data
motive.load_project(motive_filename)
hardware.motive_camera_vislight_configure()
display = pyglet.window.get_platform().get_default_display()
screen = display.get_screens()[1]
pyglet.clock.set_fps_limit(fps)
window = PointScanWindow(screen=screen, fullscreen=True, max_points=points)
pyglet.app.run()
# Run Calibration Algorithm
pos, rotmat = calibrate(window.screen_pos, window.marker_pos)
# Plot and Save Results
click.echo(pos)
click.echo(rotmat)
plot2d(window.screen_pos, window.marker_pos)
plot_estimate(obj_points=window.marker_pos, position=pos, rotation_matrix=rotmat)
with open(projector_filename, 'wb') as f:
pickle.dump({'position': pos, 'rotmat': rotmat}, f)
def latency_body_gen(motive_filename, projector_filename, port, screen):
with serial.Serial(port=port, timeout=.5) as device:
device.write('A')
motive.initialize()
motive.load_project(motive_filename.encode())
motive.update()
# Load projector's Camera object, created from the calib_projector ratcave_utils CLI tool.
projector = rc.Camera.from_pickle(projector_filename.encode())
display = pyglet.window.get_platform().get_default_display()
screen = display.get_screens()[screen]
window = LatencyDisplayApp(projector=projector, serial_device=device, fullscreen=True, screen=screen)
def update_body(dt, window, body):
motive.update()
window.dot.position.xyz = body.location
pyglet.clock.schedule(update_body, window, body)
pyglet.app.run()
def trackrotation(motive_filename, body):
motive.initialize()
motive_filename = motive_filename.encode()
motive.load_project(motive_filename)
body = motive.get_rigid_bodies()[body]
for el in range(3):
body.reset_orientation()
motive.update()
window = RotationWindow()
window.mesh.position.z = -1.5
def update_body(dt, window, body):
motive.update()
window.mesh.rotation.xyzw = body.rotation_quats
fmt_str = "loc: {:.1f}, {:.1f}, {:.1f}\t rot: {:.2f}, {:.2f}, {:.2f}, {:.2f}"
window.label.text = fmt_str.format(*(body.location +
body.rotation_quats))
pyglet.clock.schedule(update_body, window, body)
pyglet.app.run()
def trackbody(motive_filename, body):
motive_filename = motive_filename.encode()
motive.load_project(motive_filename)
body = motive.get_rigid_bodies()[body]
while True:
motive.update()
click.echo(body.location)
parser.add_argument('-p', action='store_false', dest='pca_rotate', default=True,
help='If this flag is present, there will NOT be a pca rotation of the mesh based on its markers.')
parser.add_argument('-m', action='store_false', dest='mean_center', default=True,
help='If this flag is present, the arena will be NOT offset by its mean marker position.')
parser.add_argument('-r', action='store', dest='rigid_body_name', default='',
help='Name of the Arena rigid body. If only one rigid body is present, unnecessary--that one will be used automatically.')
parser.add_argument('-i', action='store', dest='motive_projectfile', default=motive.utils.backup_project_filename,
help='Name of the motive project file to load. If not used, will load most recent Project file loaded in MotivePy.')
args = parser.parse_args()
# Select Rigid Body to track.
motive.load_project(args.motive_projectfile)
print("Loaded Motive Project: {}".format(args.motive_projectfile))
hardware.motive_camera_vislight_configure()
print("Camera Settings changed to Detect Visible light:")
print("\t"+"\n\t".join(['{}: FPS={}, Gain={}, Exp.={}, Thresh.={}'.format(cam.name, cam.frame_rate, cam.image_gain, cam.exposure, cam.threshold) for cam in motive.get_cams()]))
motive.update()
rigid_bodies = motive.get_rigid_bodies()
try:
if not args.rigid_body_name:
assert len(rigid_bodies) == 1, "Only one rigid body should be present for auto-selection. Please use the -r flag to specify a rigid body name to track for the arena."
arena_name = args.rigid_body_name if args.rigid_body_name in rigid_bodies else rigid_bodies.keys()[0]
except IndexError:
raise IndexError("No Rigid Bodies found in Optitrack tracker.")
except KeyError:
__author__ = 'nico'
import motive as m
import Tkinter, tkFileDialog
from numpy import array
import time
from pyqtgraph.Qt import QtCore, QtGui
import pyqtgraph.opengl as gl
root = Tkinter.Tk()
root.withdraw()
project_file_u=tkFileDialog.askopenfilename(title='Choose a project file to load: ', filetypes=[('motive projectfiles', '*.ttp')])
project_file = project_file_u.encode("ascii")
m.load_project(project_file)
app = QtGui.QApplication([]) #create the graphing application
w = gl.GLViewWidget() #create widget
w.opts['distance'] = 10 #start distance from where one looks at the plot
w.show() #show widget
w.setWindowTitle('markers')
last_time=time.time()
def update_position():
m.update_single_frame()
if m.get_frame_markers():
# Get Project
if args.last_project:
if args.project_filename is True:
raise Warning("Load last project file OR name a project file to load.")
project_file=utils.backup_project_filename
elif args.project_filename:
if args.last_project is True:
raise Warning("Load last project file OR name a project file to load.")
project_file=args.project_filename
else:
root = Tkinter.Tk()
root.withdraw()
project_file_u=tkFileDialog.askopenfilename(title='Choose a project file to load: ', filetypes=[('motive projectfiles', '*.ttp')])
project_file = project_file_u.encode("ascii")
# Load Motive Project
motive.load_project(project_file)
if args.love:
import matplotlib.pyplot as plt
import numpy as np
t=np.arange(0,2*np.pi, 0.1)
x, y = 16*np.sin(t)**3, 13*np.cos(t)-5*np.cos(2*t)-2*np.cos(3*t)-np.cos(4*t)
plt.plot(x,y)
plt.show()
# Display viewer
motive.show_viewer()
def view_arenafit(motive_filename, projector_filename, arena_filename, screen):
# """Displays mesh in .obj file. Useful for checking that files are rendering properly."""
reader = rc.WavefrontReader(arena_filename)
arena = reader.get_mesh('Arena', mean_center=True)
arena.rotation = arena.rotation.to_quaternion()
print('Arena Loaded. Position: {}, Rotation: {}'.format(arena.position, arena.rotation))
camera = rc.Camera.from_pickle(projector_filename)
camera.projection.fov_y = 39
light = rc.Light(position=(camera.position.xyz))
root = rc.EmptyEntity()
root.add_child(arena)
sphere = rc.WavefrontReader(rc.resources.obj_primitives).get_mesh('Sphere', scale=.05)
root.add_child(sphere)
scene = rc.Scene(meshes=root,
camera=camera,
light=light,
bgColor=(.2, .4, .2))
scene.gl_states = scene.gl_states[:-1]
display = pyglet.window.get_platform().get_default_display()
screen = display.get_screens()[screen]
window = pyglet.window.Window(fullscreen=True, screen=screen)
label = pyglet.text.Label()
fps_display = FPSDisplay(window)
shader = rc.Shader.from_file(*rc.resources.genShader)
@window.event
def on_draw():
with shader:
scene.draw()
# label.draw()
# window.clear()
fps_display.draw()
@window.event
def on_resize(width, height):
camera.projection.aspect = float(width) / height
@window.event
def on_key_release(sym, mod):
if sym == key.UP:
scene.camera.projection.fov_y += .5
elif sym == key.DOWN:
scene.camera.projection.fov_y -= .5
import motive
motive.initialize()
motive.load_project(motive_filename.encode())
motive.update()
rb = motive.get_rigid_bodies()['Arena']
# for el in range(3):
# rb.reset_orientation()
def update_arena_position(dt):
motive.update()
arena.position.xyz = rb.location
arena.rotation.xyzw = rb.rotation_quats
arena.update()
sphere.position.xyz = rb.location
label.text = "aspect={}, fov_y={}, ({:2f}, {:2f}, {:2f}), ({:2f}, {:2f}, {:2f})".format(scene.camera.projection.aspect,
scene.camera.projection.fov_y, *(arena.position.xyz + rb.location))
pyglet.clock.schedule(update_arena_position)
pyglet.app.run()
parser.add_argument('-t', action='store', dest='record_time', default=60,
help='Maximum recording time (equals actual recording time if not stopped manually).')
#make one of the two below
parser.add_argument('-s', action='store_false', dest='save_video', default=True,
help='If this flag is set, the video is shown but not saved.') #TODO: saving should not be mandatory. create save button
parser.add_argument('-f', action='store', dest='video_filename', default='video.avi',
help='Name of the file the video will be saved as.')
args = parser.parse_args()
m.load_project(args.project_file) if args.project_file else m.load_project(gui_load_project_file())
if args.camera_name:
cam=get_cam(args.camera_name)
else:
cam=gui_get_cam()
#Get Video Writer
if args.save_video:
writer=get_video_writer(cam, args.video_filename)
write_video(cam, writer, args.record_time, args.save_video) #again giving cam as argument, since writer object
#has no property to extract frame rate and frame_size from
if args.save_video:
writer.release()
#cv2.destroyAllWindows() #seems we do not need it. cvimshow disappears when the while loop ends!
__author__ = 'ratcave'
from psychopy import event
import ratcave
from ratcave.graphics import *
from ratcave.utils import rotate_to_var
import numpy as np
import argparse
from ball_simulation import Bouncer
np.set_printoptions(precision=2, suppress=True)
import motive
motive.load_project("vr_demo.ttp")
motive.update()
# Create Arena and cube
reader = WavefrontReader(ratcave.graphics.resources.obj_arena)
arena = reader.get_mesh('Arena', lighting=True, centered=False)
arena_rb = motive.get_rigid_bodies()['Arena']
player = motive.get_rigid_bodies()['CalibWand']
wand = motive.get_rigid_bodies()['CalibWand']
# Calculate Arena's orientation
for attempt in range(3):
arena_rb.reset_orientation()
motive.update()
arena_markers = np.array(arena_rb.point_cloud_markers)
additional_rotation = rotate_to_var(arena_markers)
'-s',
action='store_false',
dest='save_video',
default=True,
help='If this flag is set, the video is shown but not saved.'
) #TODO: saving should not be mandatory. create save button
parser.add_argument('-f',
action='store',
dest='video_filename',
default='video.avi',
help='Name of the file the video will be saved as.')
args = parser.parse_args()
m.load_project(args.profile_file) if args.profile_file else m.load_project(
gui_load_profile_file())
if args.camera_name:
cam = get_cam(args.camera_name)
else:
cam = gui_get_cam()
#Get Video Writer
if args.save_video:
writer = get_video_writer(cam, args.video_filename)
write_video(
cam, writer, args.record_time,
args.save_video) #again giving cam as argument, since writer object
#has no property to extract frame rate and frame_size from
if args.save_video:
if __name__ == '__main__':
import Tkinter, tkFileDialog
from os import path
import motive as m
root = Tkinter.Tk()
root.withdraw()
project_file_u = tkFileDialog.askopenfilename(
title='Choose a project file to load: ',
filetypes=[('motive projectfiles', '*.ttp')])
project_file = project_file_u.encode("ascii")
m.load_project(project_file)
for cam in m.get_cams():
cam.frame_rate = 30
if 'Prime 13' in cam.name:
cam.set_settings(
videotype=0, exposure=33000, threshold=40, intensity=0
) #check if 480 corresponds to these thousands described in motive
cam.image_gain = 8 # 8 is the maximum image gain setting
cam.set_filter_switch(False)
else:
cam.set_settings(0, cam.exposure, cam.threshold, cam.intensity)
directory, extension = path.splitext(project_file)
saved_project_pathname = ''.join([directory, '_vislight', extension])
m.save_project(saved_project_pathname)
m.shutdown()
def scan_arena(motive_filename, output_filename, body, nomeancenter, nopca,
nsides):
"""Runs Arena Scanning algorithm."""
output_filename = output_filename + '.obj' if not path.splitext(
output_filename)[1] else output_filename
assert path.splitext(output_filename)[
1] == '.obj', "Output arena filename must be a Wavefront (.obj) file"
# Load Motive Project File
motive_filename = motive_filename.encode()
motive.load_project(motive_filename)
hardware.motive_camera_vislight_configure()
motive.update()
# Get Arena's Rigid Body
rigid_bodies = motive.get_rigid_bodies()
assert body in rigid_bodies, "RigidBody {} not found in project file. Available body names: {}".format(
body, list(rigid_bodies.keys()))
assert len(
rigid_bodies[body].markers
) > 5, "At least 6 markers in the arena's rigid body is required. Only {} found".format(
len(rigid_bodies[body].markers))
# TODO: Fix bug that requires scanning be done in original orientation (doesn't affect later recreation, luckily.)
for attempt in range(
3): # Sometimes it doesn't work on the first try, for some reason.
rigid_bodies[body].reset_orientation()
motive.update()
if sum(np.abs(rigid_bodies[body].rotation)) < 1.:
break
else:
raise ValueError(
"Rigid Body Orientation not Resetting to 0,0,0 after 3 attempts. This happens sometimes (bug), please just run the script again."
)
# Scan points
display = pyglet.window.get_platform().get_default_display()
screen = display.get_screens()[1]
window = GridScanWindow(screen=screen, fullscreen=True)
pyglet.app.run()
points = np.array(window.marker_pos)
assert (
len(points) > 100
), "Only {} points detected. Tracker is not detecting enough points to model. Is the projector turned on?".format(
len(points))
assert points.ndim == 2
# Rotate all points to be mean-centered and aligned to Optitrack Markers direction or largest variance.
markers = np.array(rigid_bodies[body].point_cloud_markers)
if not nomeancenter:
points -= np.mean(markers, axis=0)
if not nopca:
points = np.dot(
points,
rotation_matrix(np.radians(orienting.rotate_to_var(markers)),
[0, 1, 0])[:3, :3])
# Plot preview of data collected
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.scatter(*points.T)
plt.show()
# Get vertex positions and normal directions from the collected data.
vertices, normals = pointcloud.meshify(points, n_surfaces=nsides)
vertices = {
wall: pointcloud.fan_triangulate(pointcloud.reorder_vertices(verts))
for wall, verts in vertices.items()
} # Triangulate
# Write wavefront .obj file to app data directory and user-specified directory for importing into Blender.
wave_str = pointcloud.to_wavefront(body, vertices, normals)
with open(output_filename, 'wb') as wavfile:
wavfile.write(wave_str)
# Show resulting plot with points and model in same place.
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.scatter(*points[::12, :].T)
for idx, verts in vertices.items():
ax.plot(*np.vstack((verts, verts[0, :])).T)
plt.show()
def scan_arena(motive_filename, output_filename, body, nomeancenter, nsides, screen):
"""Runs Arena Scanning algorithm."""
output_filename = output_filename + '.obj' if not path.splitext(output_filename)[1] else output_filename
assert path.splitext(output_filename)[1] == '.obj', "Output arena filename must be a Wavefront (.obj) file"
# Load Motive Project File
motive_filename = motive_filename.encode()
motive.initialize()
motive.load_project(motive_filename)
# Get old camera settings before changing them, to go back to them before saving later.
cam_settings = [cam.settings for cam in motive.get_cams()]
frame_rate_old = motive.get_cams()[0].frame_rate
hardware.motive_camera_vislight_configure()
motive.update()
# Get Arena's Rigid Body
rigid_bodies = motive.get_rigid_bodies()
assert body in rigid_bodies, "RigidBody {} not found in project file. Available body names: {}".format(body, list(rigid_bodies.keys()))
# assert len(rigid_bodies[body].markers) > 5, "At least 6 markers in the arena's rigid body is required. Only {} found".format(len(rigid_bodies[body].markers))
for el in range(3):
rigid_bodies[body].reset_orientation()
rigid_bodies[body].reset_pivot_offset()
motive.update()
assert np.isclose(np.array(rigid_bodies[body].rotation), 0).all(), "Orientation didn't reset."
assert np.isclose(np.array(rigid_bodies[body].location), np.mean(rigid_bodies[body].point_cloud_markers, axis=0)).all(), "Pivot didn't reset."
print("Location and Orientation Successfully reset.")
# Scan points
display = pyglet.window.get_platform().get_default_display()
screen = display.get_screens()[screen]
window = GridScanWindow(screen=screen, fullscreen=True)
pyglet.app.run()
points = np.array(window.marker_pos)
assert(len(points) > 100), "Only {} points detected. Tracker is not detecting enough points to model. Is the projector turned on?".format(len(points))
assert points.ndim == 2
# Plot preview of data collected
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.scatter(*points.T)
plt.show()
# Get vertex positions and normal directions from the collected data.
vertices, normals = pointcloud.meshify_arena(points, n_surfaces=nsides)
vertices, face_indices = pointcloud.face_index(vertices)
face_indices = pointcloud.fan_triangulate(face_indices)
# Reapply old camera settings, then save.
for setting, cam in zip(cam_settings, motive.get_cams()):
cam.settings = setting
cam.image_gain = 1
cam.frame_rate = frame_rate_old
if 'Prime 13' in cam.name:
cam.set_filter_switch(True)
motive.update()
# me
if not nomeancenter:
# import ipdb
# ipdb.set_trace()
vertmean = np.mean(vertices[face_indices.flatten(), :], axis=0)
# vertmean = np.array([np.mean(np.unique(verts)) for verts in vertices.T]) # to avoid counting the same vertices twice.
vertices -= vertmean
points -= vertmean
print('Old Location: {}'.format(rigid_bodies[body].location))
arena = rigid_bodies[body]
for attempt in range(300):
print('Trying to Set New Rigid Body location, attempt {}...'.format(attempt))
arena.reset_pivot_offset()
arena.location = vertmean
if np.isclose(arena.location, vertmean, rtol=.001).all():
break
else:
raise ValueError('Motive failed to properly shift pivot to center of mesh')
print('Vertex Mean: {}'.format(vertmean))
print('New Location: {}'.format(arena.location))
# Write wavefront .obj file to app data directory and user-specified directory for importing into Blender.
writer = WavefrontWriter.from_indexed_arrays(body, vertices, normals, face_indices)
with open(output_filename, 'w') as f:
writer.dump(output_filename)
# Show resulting plot with points and model in same place.
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.scatter(*points[::12, :].T)
ax.scatter(*vertices.T, c='r')
plt.show()
# motive.save_project(motive_filename)
motive.save_project(path.splitext(motive_filename)[0]+'_scanned.ttp')
def trackposition(motive_filename, projector_filename, body, screen):
motive.initialize()
motive_filename = motive_filename.encode()
motive.load_project(motive_filename)
body = motive.get_rigid_bodies()[body]
for el in range(3):
body.reset_orientation()
motive.update()
# Load projector's Camera object, created from the calib_projector ratcave_utils CLI tool.
display = pyglet.window.get_platform().get_default_display()
screen = display.get_screens()[screen]
window = RotationWindow(fullscreen=True, screen=screen)
keys = key.KeyStateHandler()
window.push_handlers(keys)
camera = rc.Camera.from_pickle(projector_filename.encode())
# camera.projection.fov_y = 38.5
window.scene.camera = camera
window.scene.light.position.xyz = camera.position.xyz
shader = rc.Shader.from_file(*rc.resources.genShader)
@window.event
def on_draw():
with shader:
window.scene.draw()
window.label.draw()
def update_body(dt, window, body):
motive.update()
window.mesh.rotation.xyzw = body.rotation_quats
window.mesh.position.xyz = body.location
# window.scene.camera.rotation.x += 15 * dt
fmt_str = "loc: {:.1f}, {:.1f}, {:.1f}\t rot: {:.2f}, {:.2f}, {:.2f}, {:.2f}\nfov_y: {fov_y:.2f}\taspect: {aspect:.2f}\nfps: {fps:.1f}"
window.label.text = fmt_str.format(
*(body.location + body.rotation_quats),
fov_y=window.scene.camera.projection.fov_y,
aspect=window.scene.camera.projection.aspect,
fps=1. / (dt + .00000001))
def update_fov(dt):
camera.projection.aspect = window.width / float(window.height)
camera.projection.update()
speed = 10. # How fast to change values on keyboard hold.
if keys[key.UP]:
camera.projection.fov_y += speed * dt
camera.projection.update()
if keys[key.DOWN]:
camera.projection.fov_y -= speed * dt
camera.projection.update()
if keys[key.LEFT]:
camera.projection.aspect += speed * dt
camera.projection.update()
if keys[key.RIGHT]:
camera.projection.aspect -= speed * dt
camera.projection.update()
pyglet.clock.schedule(update_fov)
pyglet.clock.schedule(update_body, window, body)
pyglet.app.run()
def vr_demo(motive_filename, projector_filename, arena_filename, body, screen):
motive.initialize()
motive_filename = motive_filename.encode()
motive.load_project(motive_filename)
body = motive.get_rigid_bodies()[body]
for el in range(3):
body.reset_orientation()
motive.update()
arena_body = motive.get_rigid_bodies()['Arena']
# Load projector's Camera object, created from the calib_projector ratcave_utils CLI tool.
display = pyglet.window.get_platform().get_default_display()
screen = display.get_screens()[screen]
window = pyglet.window.Window(fullscreen=False, screen=screen, vsync=False)
fbo = rc.FBO(rc.TextureCube(width=4096, height=4096))
shader3d = rc.Shader.from_file(*rc.resources.genShader)
reader = rc.WavefrontReader(rc.resources.obj_primitives)
mesh = reader.get_mesh('Monkey', scale=.022, position=(0, 0, .3))
mesh.position.y = .6
mesh.uniforms['ambient'] = .15, .15, .15
# mesh.rotation = mesh.rotation.to_quaternion()
arena = rc.WavefrontReader(arena_filename.encode()).get_mesh('Arena')
arena.rotation = arena.rotation.to_quaternion()
arena.texture = fbo.texture
floor = reader.get_mesh('Plane',
scale=50,
position=(0, 0, 0),
mean_center=True)
# floor.scale.x = 20
floor.rotation.x = -90
floor.texture = fbo.texture
# floor.rotation.x = 180
vr_scene = rc.Scene(meshes=[mesh], bgColor=(0., .3, 0))
vr_scene.camera.projection.fov_y = 90
vr_scene.camera.projection.aspect = 1.
vr_scene.camera.projection.z_near = .005
vr_scene.camera.projection.z_far = 2.
fbo_aa = rc.FBO(rc.Texture(width=4096, height=4096, mipmap=True))
quad = rc.gen_fullscreen_quad()
quad.texture = fbo_aa.texture
shader_deferred = rc.Shader.from_file(*rc.resources.deferredShader)
scene = rc.Scene(meshes=[arena, floor], bgColor=(0., 0., .3))
camera = rc.Camera.from_pickle(projector_filename.encode())
camera.projection.fov_y = 39
scene.camera = camera
scene.light.position.xyz = camera.position.xyz
vr_scene.light.position.xyz = camera.position.xyz
@window.event
def on_draw():
with shader3d:
with fbo:
vr_scene.camera.projection.match_aspect_to_viewport()
vr_scene.draw360_to_texture(fbo.texture)
scene.camera.projection.match_aspect_to_viewport()
with fbo_aa:
scene.draw()
with shader_deferred:
quad.draw()
def update_body(dt, body):
motive.update()
mesh.position.xyz = arena_body.location
mesh.position.y -= .07
# mesh.rotation.xyzw = arena_body.rotation_quats
# mesh.rotation.y += 10 * dt
arena.position.xyz = arena_body.location
arena.rotation.xyzw = arena_body.rotation_quats
vr_scene.camera.position.xyz = body.location
scene.camera.uniforms['playerPos'] = body.location
pyglet.clock.schedule(update_body, body)
pyglet.app.run()