Ejemplo n.º 1
0
    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
Ejemplo n.º 2
0
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)
Ejemplo n.º 5
0
 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)
Ejemplo n.º 6
0
	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)
Ejemplo n.º 7
0
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
Ejemplo n.º 8
0
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')
Ejemplo n.º 9
0
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)
Ejemplo n.º 11
0
def IntersectHighlighter(highlighter):
	line = highlighter.getLineForward(viz.ABS_GLOBAL, length=100.0)
	return viz.intersect(line.begin, line.end)
Ejemplo n.º 12
0
 def _intersect(self, begin, end):
     return viz.intersect(begin, end)