def UpdateLocation(self, state, slideAngle=40):
# Reset the record of the last object run into.
self.lastObjectIntersected = None
speed, rotspeed = self.GetInput()
if speed == 0 and rotspeed == 0:
return state
newState = list(state)
if speed != 0:
startPos = [state[0], VERTICAL_POS, state[1]]
if speed > 0: colBuf = self.COLLISION_BUFFER
elif speed < 0: colBuf = -1 * self.COLLISION_BUFFER
for angleOffset in range(0, slideAngle):
driftAngle = state[2] + angleOffset
intersObj = viz.intersect(
startPos, findEndPoint(startPos, driftAngle, colBuf))
if not intersObj.intersected:
break
driftAngle = state[2] - angleOffset
intersObj = viz.intersect(
startPos, findEndPoint(startPos, driftAngle, colBuf))
if not intersObj.intersected:
break
elif not self.lastObjectIntersected:
# We've run into an object, record which object.
self.lastObjectIntersected = intersObj.object
else:
driftAngle = state[2]
if (((speed > 0 and self.forward) or (speed < 0 and self.backward))
and not intersObj.intersected):
# Compute the new position.
distance = speed * self.JOY_MOVE_FREQUENCY
if distance > 0: self.fwdDistance += distance
else: self.bwdDistance += distance
newPosition = findEndPoint(startPos, driftAngle, distance)
# Update the state.
newState[0] = newPosition[0]
newState[1] = newPosition[2]
# Move to the new position.
viz.reset(viz.HEAD_POS)
viz.translate(viz.HEAD_POS, *newPosition)
# rotation
if rotspeed != 0 and self.turn:
rotDistance = rotspeed * self.JOY_MOVE_FREQUENCY
if rotDistance > 0: self.rtDistance += rotDistance
else: self.ltDistance += rotDistance
newState[2] = (state[2] + rotDistance) % 360
viz.reset(viz.BODY_ORI)
viz.rotate(viz.BODY_ORI, newState[2], 0, 0)
return newState
def updateGaze():
"""Called every frame to update user gaze from latest sample"""
global lastLookTarget
# Get gaze matrix in local HMD coordinate system
gazeMat = gaze.getLastGazeMatrix()
# Transform gaze matrix into world coordinate system
gazeMat.postMult(viz.MainView.getMatrix())
# Intersect world gaze vector with scene
line = viz.Line(begin=gazeMat.getPosition(),
dir=gazeMat.getForward(),
length=1000)
info = viz.intersect(line.begin, line.end)
if info.valid:
# Place model at intersection point
point.setPosition(info.point)
# Update look target
target = info.object if isinstance(info.object, LookTarget) else None
if target != lastLookTarget:
if lastLookTarget:
lastLookTarget.setLooking(False)
lastLookTarget = target
if lastLookTarget:
lastLookTarget.setLooking(True)
def IntersectController(controller): """Perform intersection using controller""" line = controller.model.getLineForward(viz.ABS_GLOBAL, length=100.0) return viz.intersect(line.begin, line.end) #def HighlightPainting(name, mode): """Apply/Unapply highlight effect from specified painting"""
def focus(self):
line = viz.MainWindow.screenToWorld([0.5, 0.5])
self.intersection = viz.intersect(line.begin, line.end)
if self.intersection.valid:
newObj = self.intersection.object
# self.focusCrosshair()
if newObj is not self.selected:
self.selected = newObj
if self.selected.id in self.selectableIDs:
print "Selecting Avatar id", str(self.selected.id)
def transform(self):
if self.angl == 1:
view = viz.MainView
mat = viz.Matrix()
mat.postAxisAngle(0, 1, 0, self.theta)
mat.postTrans(self.x, self.y + 1.45, self.z + 0.15)
view.setMatrix(mat)
intersect = viz.intersect([self.x, 4000, self.z], [self.x, 0, self.z])
self.y = intersect.point[1]
mat = viz.Matrix()
mat.postAxisAngle(0, 1, 0, self.theta)
mat.postScale(2, 2, 2)
mat.postTrans(self.x, self.y, self.z)
self.avatar.setMatrix(mat)
def updateGaze(): ''' calls 'get_gaze function' and takes the average of two eyes for the normal values - they will be used to project a sphere on where subjects look at ''' # get gaze data norm_pos_x = np.mean([get_gaze(1)[0], get_gaze(0)[0]]) norm_pos_y = np.mean([get_gaze(1)[1], get_gaze(0)[1]]) # find the intersection and project sphere line = viz.MainWindow.screenToWorld([norm_pos_x, norm_pos_y]) intersection = viz.intersect(line.begin, line.end) m_gaze.setPosition(intersection.point)
def getHallLength(state, hallScale, verticalOffset=0.0):
"""Measure the distance to the end of the hall, rounded to the nearest
number of hall lengths.
hallScale is the distance from the center of one intersection
to the center of the next.
verticalOffset is the offset from the head position at which the
measurement is taken."""
# We compute the distance to the end of the hall by drawing a
# line straight in front of the camera and finding the first
# place it intersects a wall. We use a horizontal spread of
# lines and take the max to account for the user possibly not
# facing straight down the hall.
maxDist = 0
startPos = [state[0], VERTICAL_POS + verticalOffset, state[1]]
for i in range(0, NUM_BEAMS):
curAngle = state[2] + (NUM_BEAMS / 2 - i -
1 / 2) * DEGREES_BETWEEN_BEAMS
endPos = findEndPoint(startPos, curAngle, DISTANCE_BUFFER)
intersObj = viz.intersect(startPos, endPos)
# dist = sqrt(x^2 + y^2 + z^2) (y^2 isn't really needed for our case)
dist = pow(
pow(startPos[0] - intersObj.intersectPoint[0], 2) +
pow(startPos[2] - intersObj.intersectPoint[2], 2), 0.5)
if dist > maxDist:
maxDist = dist
maxEndPos = intersObj.intersectPoint
# Assume the camera is in the middle of the intersection. It measures
# to the far edge of an intersection, so compensate for half an
# intersection.
maxDist -= 0.45
# Convert to the nearest number of intersections, then round to
# the nearest integer.
maxDist = maxDist / hallScale
maxDist = int((maxDist + 0.45))
return maxDist
def validation():
'''
Show same calibration points and compare calulated gaze point to ground truth.
(Displays both + angular error)
'''
# ask for the sub port
req.send_string('SUB_PORT')
sub_port = req.recv_string()
# open a sub port to listen to pupil
sub = ctx.socket(zmq.SUB)
sub.connect("tcp://{}:{}".format(addr, sub_port))
sub.setsockopt_string(zmq.SUBSCRIBE, u'gaze')
# add gaze marker
m_gaze = vizshape.addSphere(radius=sphere_size, color=viz.GREEN)
m_gaze.disable(viz.INTERSECTION)
if not gaze_marker_visible:
m_gaze.disable(viz.RENDER) # invisible but phyisically present
def get_gaze(eye_number):
'''
checks gaze stream for confident measures of the eye (eye_number) until it finds them
Args: eye_number (int): 1=left, 0=right
Returns: [x, y] (float): normalized x and y values range [0,1]
'''
found_confident_val = False
while found_confident_val == False:
topic = sub.recv_string() # unused
msg = sub.recv()
msg = loads(msg, encoding='utf-8')
confidence = msg['confidence']
if msg['id'] == eye_number:
if confidence > confidence_level:
found_confident_val = True
t = msg['timestamp'] # unused
npx = msg['norm_pos'][0]
npy = msg['norm_pos'][1]
return [npx, npy]
def updateGaze():
'''
calls 'get_gaze function' and takes the average of two
eyes for the normal values - they will be used to project a
sphere on where subjects look at
'''
# get gaze data
norm_pos_x = np.mean([get_gaze(1)[0], get_gaze(0)[0]])
norm_pos_y = np.mean([get_gaze(1)[1], get_gaze(0)[1]])
# find the intersection and project sphere
line = viz.MainWindow.screenToWorld([norm_pos_x, norm_pos_y])
intersection = viz.intersect(line.begin, line.end)
m_gaze.setPosition(intersection.point)
# update cursor location on every sample
vizact.onupdate(viz.PRIORITY_LINKS+1, updateGaze)
dot_norm_pos=[]
gaze_norm_pos=[]
ang_error = []
yield showMessage('Zum Starten der Validierung die Leertaste drücken')
for p in norm_positions:
print('calibration point: ', p)
norm_x = p[0]
norm_y = p[1]
first_run = True
if first_run:
first_run = False
'''set up a plane 'right in front of user' on which we want to project the dots'''
# target the current center of view
p_line = viz.MainWindow.screenToWorld(.5, .5)
# let's modify the line and call it line 2. Here we let the line end at the "depth" value
p_line_2 = viz.Line(begin=p_line.begin, end=p_line.end, dir=p_line.dir, length=depth)
# Add the plane and apply the matrix of our viewpoint to the plane
plane = vizshape.addBox(size=[3.6, 2, .1])
mymat = viz.MainView.getMatrix()
plane.setMatrix(mymat)
# Reposition the plane to the end of line 2
plane.setPosition(p_line_2.end)
plane.color([.25,.25,.25])
plane.alpha(.95)
# Lock it to user
plane_link = viz.grab(viz.MainView, plane)
# interpolate a line from norm values to 3d coordinates
line = viz.MainWindow.screenToWorld([norm_x, norm_y])
# find the intersection
intersection = viz.intersect(line.begin, line.end)
# place a dot (at depth level of line)
dot = vizshape.addSphere(radius=sphere_size)
dot.setPosition(intersection.point)
# lock dot to user
view_link = viz.grab(viz.MainView, dot)
print('ready')
viz.playSound('beep500_200.wav')
yield viztask.waitKeyDown(' ')
for s in range(60):
# get the current pupil time (pupil uses CLOCK_MONOTONIC with adjustable timebase).
# You can set the pupil timebase to another clock and use that.
t = get_pupil_timestamp()
dot_norm_pos.append(p)
gaze_norm_pos.append(viz.MainWindow.worldToScreen(m_gaze.getPosition()))
yield viztask.waitTime(1/60.)
print(t)
dot.color(viz.RED)
print('waiting for next position...')
yield viztask.waitKeyDown(' ')
yield dot.remove()
time.sleep(2)
showMessage('validation done!')
for p in norm_positions:
i = norm_positions.index(p)
chunk = range(i*60, (i+1)*60)
print(i)
dmx = np.mean([gaze_norm_pos[x][0] for x in chunk])
dmy = np.mean([gaze_norm_pos[y][1] for y in chunk])
# interpolate a line from norm values to 3d coordinates
line = viz.MainWindow.screenToWorld([p[0], p[1]])
line_v = viz.MainWindow.screenToWorld([dmx, dmy])
# find the intersection
intersection = viz.intersect(line.begin, line.end)
intersection_v = viz.intersect(line_v.begin, line_v.end)
# place a dots (at depth level of line) for both ground truth and gaze point
dot = vizshape.addSphere(radius=sphere_size)
dot.setPosition(intersection.point)
dot.color(viz.BLUE)
dot_v = vizshape.addSphere(radius=sphere_size*0.75)
dot_v.setPosition(intersection_v.point)
dot_v.color(viz.YELLOW_ORANGE)
# lock dots to user
view_link = viz.grab(viz.MainView, dot)
viel_link2 = viz.grab(viz.MainView, dot_v)
# calculate angular error
error = vizmat.AngleBetweenVector(line.dir, line_v.dir)
# cosangle = np.dot(a,b) / (np.linalg.norm(a) * np.linalg.norm(b))
# angle = np.arccos(cosangle)
# error = np.degrees(angle)
ang_error.append(error)
print('angle is: ', error, 'for ', p)
showMessage('mean angular error is: {}'.format(np.mean(ang_error)))
print('mean angular error is: ', np.mean(ang_error), ' deg/ visual angle')
def calibration():
''' The heart of the calibration routine. Presents points and collects data. '''
# start calibration routine and sample data
n = {'subject':'calibration.should_start', 'hmd_video_frame_size':(2160,1200), 'outlier_threshold':35}
print(send_recv_notification(n))
ref_data = []
yield showMessage('Zum Starten die Leertaste drücken')
for p in norm_positions:
print('calibration point: ', p)
norm_x = p[0]
norm_y = p[1]
first_run = True
if first_run:
first_run = False
'''set up a plane 'right in front of user' on which we want to project the dots'''
# target the current center of view
p_line = viz.MainWindow.screenToWorld(.5, .5)
# let's modify the line and call it line 2. Here we let the line end at the "depth" value
p_line_2 = viz.Line(begin=p_line.begin, end=p_line.end, dir=p_line.dir, length=depth)
# Add the plane and apply the matrix of our viewpoint to the plane
plane = vizshape.addBox(size=[3.6, 2, .1])
mymat = viz.MainView.getMatrix()
plane.setMatrix(mymat)
# Reposition the plane to the end of line 2
plane.setPosition(p_line_2.end)
plane.color([.25,.25,.25])
plane.alpha(.95)
# Lock it to user
plane_link = viz.grab(viz.MainView, plane)
# interpolate a line from norm values to 3d coordinates
line = viz.MainWindow.screenToWorld([norm_x, norm_y])
# find the intersection
intersection = viz.intersect(line.begin, line.end)
# place a dot (at depth level of line)
dot = vizshape.addSphere(radius=sphere_size)
dot.setPosition(intersection.point)
# lock dot to user
view_link = viz.grab(viz.MainView, dot)
print('ready')
viz.playSound('beep500_200.wav')
yield viztask.waitKeyDown(' ')
for s in range(60):
# get the current pupil time (pupil uses CLOCK_MONOTONIC with adjustable timebase).
# You can set the pupil timebase to another clock and use that.
t = get_pupil_timestamp()
# here the left and right screen marker positions are identical.
datum0 = {'norm_pos':p,'timestamp':t,'id':0}
datum1 = {'norm_pos':p,'timestamp':t,'id':1}
ref_data.append(datum0)
ref_data.append(datum1)
yield viztask.waitTime(1/60.)
print(t)
dot.color(viz.RED)
print('waiting for next position...')
yield viztask.waitKeyDown(' ')
yield dot.remove()
# send ref data to Pupil Capture/Service:
# this notification can be sent once at the end or multiple times.
# during one calibraiton all new data will be appended.
n = {'subject':'calibration.add_ref_data','ref_data':ref_data}
print(send_recv_notification(n))
# stop calibration
# pupil will correlate pupil and ref data based on timestamps,
# compute the gaze mapping params, and start a new gaze mapper.
n = {'subject':'calibration.should_stop'}
print(send_recv_notification(n))
time.sleep(2)
showMessage('calibration done! - now validate!')
plane.remove()
yield viztask.waitTime(2)
def _intersect(self,begin,end): return viz.intersect(begin,end)
def IntersectHighlighter(highlighter): line = highlighter.getLineForward(viz.ABS_GLOBAL, length=100.0) return viz.intersect(line.begin, line.end)
def _intersect(self, begin, end):
return viz.intersect(begin, end)