class Calibration_Visualizer(Visualizer):
	def __init__(self, g_pool, world_camera_intrinsics , cal_ref_points_3d, cal_observed_points_3d, eye_camera_to_world_matrix0 , cal_gaze_points0_3d, eye_camera_to_world_matrix1 = np.eye(4) , cal_gaze_points1_3d = [],  run_independently = False , name = "Calibration Visualizer" ):
		super(Calibration_Visualizer, self).__init__( g_pool,name,  run_independently)

		self.image_width = 640 # right values are assigned in update
		self.focal_length = 620
		self.image_height = 480

		self.eye_camera_to_world_matrix0 = eye_camera_to_world_matrix0
		self.eye_camera_to_world_matrix1 = eye_camera_to_world_matrix1

		self.cal_ref_points_3d = cal_ref_points_3d
		self.cal_observed_points_3d = cal_observed_points_3d
		self.cal_gaze_points0_3d = cal_gaze_points0_3d
		self.cal_gaze_points1_3d = cal_gaze_points1_3d

		if world_camera_intrinsics:
			self.world_camera_width = world_camera_intrinsics['resolution'][0]
			self.world_camera_height = world_camera_intrinsics['resolution'][1]
			self.world_camera_focal = (world_camera_intrinsics['camera_matrix'][0][0] +  world_camera_intrinsics['camera_matrix'][1][1] ) / 2.0
		else:
			self.world_camera_width = 0
			self.world_camera_height = 0
			self.world_camera_focal = 0

		camera_fov = math.degrees(2.0 * math.atan( self.window_size[0] / (2.0 * self.focal_length)))
		self.trackball = Trackball(camera_fov)
		self.trackball.distance = [0,0,-80.]
		self.trackball.pitch = 210
		self.trackball.roll = 0


	########### Open, update, close #####################

	def update_window(self, g_pool , gaze_points0 , sphere0 , gaze_points1 = [] , sphere1 = None, intersection_points = []  ):

		if not self.window:
			return

		self.begin_update_window() #sets context

		self.clear_gl_screen()
		self.trackball.push()

		glMatrixMode( GL_MODELVIEW )

		# draw things in world camera coordinate system
		glPushMatrix()
		glLoadIdentity()

		calibration_points_line_color = RGBA(0.5,0.5,0.5,0.1);
		error_line_color = RGBA(1.0,0.0,0.0,0.5)

		self.draw_coordinate_system(200)
		if self.world_camera_width != 0:
			self.draw_frustum( self.world_camera_width/ 10.0 , self.world_camera_height/ 10.0 , self.world_camera_focal / 10.0)

		for p in self.cal_observed_points_3d:
			glutils.draw_polyline( [ (0,0,0), p]  , 1 , calibration_points_line_color, line_type = GL_LINES)
			#draw error lines form eye gaze points to  ref points
		for(cal_point,ref_point) in zip(self.cal_ref_points_3d, self.cal_observed_points_3d):
				glutils.draw_polyline( [ cal_point, ref_point]  , 1 , error_line_color, line_type = GL_LINES)

		#calibration points
		glutils.draw_points( self.cal_ref_points_3d , 4 , RGBA( 0, 1, 1, 1 ) )


		glPopMatrix()

		if sphere0:
			# eye camera
			glPushMatrix()
			glLoadMatrixf( self.eye_camera_to_world_matrix0.T )

			self.draw_coordinate_system(60)
			self.draw_frustum( self.image_width / 10.0, self.image_height / 10.0, self.focal_length /10.)
			glPopMatrix()

			#everything else is in world coordinates

			#eye
			sphere_center0 = list(sphere0['center'])
			sphere_radius0 = sphere0['radius']
			self.draw_sphere(sphere_center0,sphere_radius0,  color = RGBA(1,1,0,1))

			#gazelines
			for p in self.cal_gaze_points0_3d:
				glutils.draw_polyline( [ sphere_center0, p]  , 1 , calibration_points_line_color, line_type = GL_LINES)

			#calibration points
			# glutils.draw_points( self.cal_gaze_points0_3d , 4 , RGBA( 1, 0, 1, 1 ) )

			#current gaze points
			glutils.draw_points( gaze_points0 , 2 , RGBA( 1, 0, 0, 1 ) )
			for p in gaze_points0:
				glutils.draw_polyline( [sphere_center0, p]  , 1 , RGBA(0,0,0,1), line_type = GL_LINES)

			#draw error lines form eye gaze points to  ref points
			for(cal_gaze_point,ref_point) in zip(self.cal_gaze_points0_3d, self.cal_ref_points_3d):
				glutils.draw_polyline( [ cal_gaze_point, ref_point]  , 1 , error_line_color, line_type = GL_LINES)

		#second eye
		if sphere1:
			# eye camera
			glPushMatrix()
			glLoadMatrixf( self.eye_camera_to_world_matrix1.T )

			self.draw_coordinate_system(60)
			self.draw_frustum( self.image_width / 10.0, self.image_height / 10.0, self.focal_length /10.)
			glPopMatrix()

			#everything else is in world coordinates

			#eye
			sphere_center1 = list(sphere1['center'])
			sphere_radius1 = sphere1['radius']
			self.draw_sphere(sphere_center1,sphere_radius1,  color = RGBA(1,1,0,1))

			#gazelines
			for p in self.cal_gaze_points1_3d:
				glutils.draw_polyline( [ sphere_center1, p]  , 4 , calibration_points_line_color, line_type = GL_LINES)

			#calibration points
			glutils.draw_points( self.cal_gaze_points1_3d , 4 , RGBA( 1, 0, 1, 1 ) )

			#current gaze points
			glutils.draw_points( gaze_points1 , 2 , RGBA( 1, 0, 0, 1 ) )
			for p in gaze_points1:
				glutils.draw_polyline( [sphere_center1, p]  , 1 , RGBA(0,0,0,1), line_type = GL_LINES)

			#draw error lines form eye gaze points to  ref points
			for(cal_gaze_point,ref_point) in zip(self.cal_gaze_points1_3d, self.cal_ref_points_3d):
				glutils.draw_polyline( [ cal_gaze_point, ref_point]  , 1 , error_line_color, line_type = GL_LINES)

		self.trackball.pop()

		self.end_update_window() #swap buffers, handle context



	############ window callbacks #################
	def on_resize(self,window,w, h):
		Visualizer.on_resize(self,window,w, h)
		self.trackball.set_window_size(w,h)


	def on_char(self,window,char):
		if char == ord('r'):
			self.trackball.distance = [0,0,-0.1]
			self.trackball.pitch = 0
			self.trackball.roll = 180

	def on_scroll(self,window,x,y):
		self.trackball.zoom_to(y)
Beispiel #2
0
class Calibration_Visualizer(Visualizer):
    def __init__(self,
                 g_pool,
                 world_camera_intrinsics,
                 cal_ref_points_3d,
                 cal_observed_points_3d,
                 eye_camera_to_world_matrix0,
                 cal_gaze_points0_3d,
                 eye_camera_to_world_matrix1=np.eye(4),
                 cal_gaze_points1_3d=[],
                 run_independently=False,
                 name="Calibration Visualizer"):
        super().__init__(g_pool, name, run_independently)

        self.image_width = 640  # right values are assigned in update
        self.focal_length = 620
        self.image_height = 480

        self.eye_camera_to_world_matrix0 = np.asarray(
            eye_camera_to_world_matrix0)
        self.eye_camera_to_world_matrix1 = np.asarray(
            eye_camera_to_world_matrix1)

        self.cal_ref_points_3d = cal_ref_points_3d
        self.cal_observed_points_3d = cal_observed_points_3d
        self.cal_gaze_points0_3d = cal_gaze_points0_3d
        self.cal_gaze_points1_3d = cal_gaze_points1_3d

        if world_camera_intrinsics:
            self.world_camera_width = world_camera_intrinsics['resolution'][0]
            self.world_camera_height = world_camera_intrinsics['resolution'][1]
            self.world_camera_focal = (
                world_camera_intrinsics['camera_matrix'][0][0] +
                world_camera_intrinsics['camera_matrix'][1][1]) / 2.0
        else:
            self.world_camera_width = 0
            self.world_camera_height = 0
            self.world_camera_focal = 0

        camera_fov = math.degrees(2.0 * math.atan(self.window_size[0] /
                                                  (2.0 * self.focal_length)))
        self.trackball = Trackball(camera_fov)
        self.trackball.distance = [0, 0, -80.]
        self.trackball.pitch = 210
        self.trackball.roll = 0

    ########### Open, update, close #####################

    def update_window(self,
                      g_pool,
                      gaze_points0,
                      sphere0,
                      gaze_points1=[],
                      sphere1=None,
                      intersection_points=[]):

        if not self.window:
            return

        self.begin_update_window()  #sets context

        self.clear_gl_screen()
        self.trackball.push()

        glMatrixMode(GL_MODELVIEW)

        # draw things in world camera coordinate system
        glPushMatrix()
        glLoadIdentity()

        calibration_points_line_color = RGBA(0.5, 0.5, 0.5, 0.1)
        error_line_color = RGBA(1.0, 0.0, 0.0, 0.5)

        self.draw_coordinate_system(200)
        if self.world_camera_width != 0:
            self.draw_frustum(self.world_camera_width / 10.0,
                              self.world_camera_height / 10.0,
                              self.world_camera_focal / 10.0)

        for p in self.cal_observed_points_3d:
            glutils.draw_polyline([(0, 0, 0), p],
                                  1,
                                  calibration_points_line_color,
                                  line_type=GL_LINES)
            #draw error lines form eye gaze points to  ref points
        for (cal_point, ref_point) in zip(self.cal_ref_points_3d,
                                          self.cal_observed_points_3d):
            glutils.draw_polyline([cal_point, ref_point],
                                  1,
                                  error_line_color,
                                  line_type=GL_LINES)

        #calibration points
        glutils.draw_points(self.cal_ref_points_3d, 4, RGBA(0, 1, 1, 1))

        glPopMatrix()

        if sphere0:
            # eye camera
            glPushMatrix()
            glLoadMatrixf(self.eye_camera_to_world_matrix0.T)

            self.draw_coordinate_system(60)
            self.draw_frustum(self.image_width / 10.0,
                              self.image_height / 10.0,
                              self.focal_length / 10.)
            glPopMatrix()

            #everything else is in world coordinates

            #eye
            sphere_center0 = list(sphere0['center'])
            sphere_radius0 = sphere0['radius']
            self.draw_sphere(sphere_center0,
                             sphere_radius0,
                             color=RGBA(1, 1, 0, 1))

            #gazelines
            for p in self.cal_gaze_points0_3d:
                glutils.draw_polyline([sphere_center0, p],
                                      1,
                                      calibration_points_line_color,
                                      line_type=GL_LINES)

            #calibration points
            # glutils.draw_points( self.cal_gaze_points0_3d , 4 , RGBA( 1, 0, 1, 1 ) )

            #current gaze points
            glutils.draw_points(gaze_points0, 2, RGBA(1, 0, 0, 1))
            for p in gaze_points0:
                glutils.draw_polyline([sphere_center0, p],
                                      1,
                                      RGBA(0, 0, 0, 1),
                                      line_type=GL_LINES)

            #draw error lines form eye gaze points to  ref points
            for (cal_gaze_point, ref_point) in zip(self.cal_gaze_points0_3d,
                                                   self.cal_ref_points_3d):
                glutils.draw_polyline([cal_gaze_point, ref_point],
                                      1,
                                      error_line_color,
                                      line_type=GL_LINES)

        #second eye
        if sphere1:
            # eye camera
            glPushMatrix()
            glLoadMatrixf(self.eye_camera_to_world_matrix1.T)

            self.draw_coordinate_system(60)
            self.draw_frustum(self.image_width / 10.0,
                              self.image_height / 10.0,
                              self.focal_length / 10.)
            glPopMatrix()

            #everything else is in world coordinates

            #eye
            sphere_center1 = list(sphere1['center'])
            sphere_radius1 = sphere1['radius']
            self.draw_sphere(sphere_center1,
                             sphere_radius1,
                             color=RGBA(1, 1, 0, 1))

            #gazelines
            for p in self.cal_gaze_points1_3d:
                glutils.draw_polyline([sphere_center1, p],
                                      4,
                                      calibration_points_line_color,
                                      line_type=GL_LINES)

            #calibration points
            glutils.draw_points(self.cal_gaze_points1_3d, 4, RGBA(1, 0, 1, 1))

            #current gaze points
            glutils.draw_points(gaze_points1, 2, RGBA(1, 0, 0, 1))
            for p in gaze_points1:
                glutils.draw_polyline([sphere_center1, p],
                                      1,
                                      RGBA(0, 0, 0, 1),
                                      line_type=GL_LINES)

            #draw error lines form eye gaze points to  ref points
            for (cal_gaze_point, ref_point) in zip(self.cal_gaze_points1_3d,
                                                   self.cal_ref_points_3d):
                glutils.draw_polyline([cal_gaze_point, ref_point],
                                      1,
                                      error_line_color,
                                      line_type=GL_LINES)

        self.trackball.pop()

        self.end_update_window()  #swap buffers, handle context

    ############ window callbacks #################
    def on_resize(self, window, w, h):
        Visualizer.on_resize(self, window, w, h)
        self.trackball.set_window_size(w, h)

    def on_window_char(self, window, char):
        if char == ord('r'):
            self.trackball.distance = [0, 0, -0.1]
            self.trackball.pitch = 0
            self.trackball.roll = 180

    def on_scroll(self, window, x, y):
        self.trackball.zoom_to(y)
class Visualizer(object):
	def __init__(self,focal_length, name = "Debug Visualizer", run_independently = False):
       # super(Visualizer, self).__init__()
		self.focal_length = focal_length
		self.image_width = 640 # right values are assigned in update
		self.image_height = 480
		# transformation matrices
		self.anthromorphic_matrix = self.get_anthropomorphic_matrix()

		self.name = name
		self.window_size = (640,480)
		self._window = None
		self.input = None
		self.run_independently = run_independently

		camera_fov = math.degrees(2.0 * math.atan( self.image_height / (2.0 * self.focal_length)))
		self.trackball = Trackball(camera_fov)

	############## MATRIX FUNCTIONS ##############################

	def get_anthropomorphic_matrix(self):
		temp =  np.identity(4)
		temp[2,2] *= -1
		return temp


	def get_adjusted_pixel_space_matrix(self,scale):
		# returns a homoegenous matrix
		temp = self.get_anthropomorphic_matrix()
		temp[3,3] *= scale
		return temp

	def get_image_space_matrix(self,scale=1.):
		temp = self.get_adjusted_pixel_space_matrix(scale)
		temp[1,1] *=-1 #image origin is top left
		temp[0,3] = -self.image_width/2.0
		temp[1,3] = self.image_height/2.0
		temp[2,3] = -self.focal_length
		return temp.T

	def get_pupil_transformation_matrix(self,circle_normal,circle_center, circle_scale = 1.0):
		"""
			OpenGL matrix convention for typical GL software
			with positive Y=up and positive Z=rearward direction
			RT = right
			UP = up
			BK = back
			POS = position/translation
			US = uniform scale

			float transform[16];

			[0] [4] [8 ] [12]
			[1] [5] [9 ] [13]
			[2] [6] [10] [14]
			[3] [7] [11] [15]

			[RT.x] [UP.x] [BK.x] [POS.x]
			[RT.y] [UP.y] [BK.y] [POS.y]
			[RT.z] [UP.z] [BK.z] [POS.Z]
			[    ] [    ] [    ] [US   ]
		"""
		temp = self.get_anthropomorphic_matrix()
		right = temp[:3,0]
		up = temp[:3,1]
		back = temp[:3,2]
		translation = temp[:3,3]
		back[:] = np.array(circle_normal)
		back[2] *=-1 #our z axis is inverted

		if np.linalg.norm(back) != 0:
			back[:] /= np.linalg.norm(back)
			right[:] = get_perpendicular_vector(back)/np.linalg.norm(get_perpendicular_vector(back))
			up[:] = np.cross(right,back)/np.linalg.norm(np.cross(right,back))
			right[:] *= circle_scale
			back[:] *=circle_scale
			up[:] *=circle_scale
			translation[:] = np.array(circle_center)
			translation[2] *= -1
		return   temp.T

	############## DRAWING FUNCTIONS ##############################

	def draw_frustum(self ):

		W = self.image_width/2.0
		H = self.image_height/2.0
		Z = self.focal_length
		glPushMatrix()
		# draw it
		glLineWidth(1)
		glColor4f( 1, 0.5, 0, 0.5 )
		glBegin( GL_LINE_LOOP )
		glVertex3f( 0, 0, 0 )
		glVertex3f( -W, H, Z )
		glVertex3f( W, H, Z )
		glVertex3f( 0, 0, 0 )
		glVertex3f( W, H, Z )
		glVertex3f( W, -H, Z )
		glVertex3f( 0, 0, 0 )
		glVertex3f( W, -H, Z )
		glVertex3f( -W, -H, Z )
		glVertex3f( 0, 0, 0 )
		glVertex3f( -W, -H, Z )
		glVertex3f( -W, H, Z )
		glEnd( )
		glPopMatrix()

	def draw_coordinate_system(self,l=1):
		# Draw x-axis line. RED
		glLineWidth(2)
		glColor3f( 1, 0, 0 )
		glBegin( GL_LINES )
		glVertex3f( 0, 0, 0 )
		glVertex3f( l, 0, 0 )
		glEnd( )

		# Draw y-axis line. GREEN.
		glColor3f( 0, 1, 0 )
		glBegin( GL_LINES )
		glVertex3f( 0, 0, 0 )
		glVertex3f( 0, l, 0 )
		glEnd( )

		# Draw z-axis line. BLUE
		glColor3f( 0, 0, 1 )
		glBegin( GL_LINES )
		glVertex3f( 0, 0, 0 )
		glVertex3f( 0, 0, l )
		glEnd( )

	def draw_sphere(self,sphere_position, sphere_radius,contours = 45, color =RGBA(.2,.5,0.5,.5) ):
		# this function draws the location of the eye sphere
		glPushMatrix()

		glTranslatef(sphere_position[0],sphere_position[1],sphere_position[2]) #sphere[0] contains center coordinates (x,y,z)
		glTranslatef(0,0,sphere_radius) #sphere[1] contains radius
		for i in xrange(1,contours+1):

			glTranslatef(0,0, -sphere_radius/contours*2)
			position = sphere_radius - i*sphere_radius*2/contours
			draw_radius = np.sqrt(sphere_radius**2 - position**2)
			glPushMatrix()
			glScalef(draw_radius,draw_radius,1)
			draw_polyline((circle_xy),2,color)
			glPopMatrix()

		glPopMatrix()


	def draw_circle(self, circle_center, circle_normal, circle_radius, color=RGBA(1.1,0.2,.8), num_segments = 20):
		vertices = []
		vertices.append( (0,0,0) )  # circle center

		#create circle vertices in the xy plane
		for i in np.linspace(0.0, 2.0*math.pi , num_segments ):
			x = math.sin(i)
			y = math.cos(i)
			z = 0
			vertices.append((x,y,z))

		glPushMatrix()
		glMatrixMode(GL_MODELVIEW )
		glLoadMatrixf(self.get_pupil_transformation_matrix(circle_normal,circle_center, circle_radius))
		draw_polyline((vertices),color=color, line_type = GL_TRIANGLE_FAN) # circle
		draw_polyline( [ (0,0,0), (0,0, 4) ] ,color=RGBA(0,0,0), line_type = GL_LINES) #normal
		glPopMatrix()




	def draw_contours(self, contours, thickness = 1, color = RGBA(0.,0.,0.,0.5) ):
		glPushMatrix()
		glLoadMatrixf(self.get_anthropomorphic_matrix())
		for contour in contours:
			draw_polyline(contour, thickness, color = color )
		glPopMatrix()

	def draw_contour(self, contour, thickness = 1, color = RGBA(0.,0.,0.,0.5) ):
		glPushMatrix()
		glLoadMatrixf(self.get_anthropomorphic_matrix())
		draw_polyline(contour,thickness, color)
		glPopMatrix()


	def draw_debug_info(self, result  ):
		models = result['models']
		eye = models[0]['sphere'];
		direction = result['circle'][1];
		pupil_radius = result['circle'][2];

		status = ' Eyeball center : X: %.2fmm Y: %.2fmm Z: %.2fmm\n Pupil direction:  X: %.2f Y: %.2f Z: %.2f\n Pupil Diameter: %.2fmm\n  ' \
		%(eye[0][0], eye[0][1],eye[0][2],
		direction[0], direction[1],direction[2], pupil_radius*2)

		self.glfont.push_state()
		self.glfont.set_color_float( (0,0,0,1) )

		self.glfont.draw_multi_line_text(5,20,status)


		#draw model info for each model
		delta_y = 20
		for model in models:
			modelStatus =	('Model: %d \n ' %  model['modelID'] ,
							'    maturity: %.3f\n' % model['maturity'] ,
							'    fit: %.6f\n' % model['fit'] ,
							'    performance: %.6f\n' % model['performance'] ,
							'    perf.Grad.: %.3e\n' % model['performanceGradient'] ,
							)
			modeltext = ''.join( modelStatus )
			self.glfont.draw_multi_line_text(self.window_size[0] - 200 ,delta_y, modeltext)

			delta_y += 100

		self.glfont.pop_state()


	########## Setup functions I don't really understand ############

	def basic_gl_setup(self):
		glEnable(GL_POINT_SPRITE )
		glEnable(GL_VERTEX_PROGRAM_POINT_SIZE) # overwrite pointsize
		glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
		glEnable(GL_BLEND)
		glClearColor(.8,.8,.8,1.)
		glEnable(GL_LINE_SMOOTH)
		# glEnable(GL_POINT_SMOOTH)
		glHint(GL_LINE_SMOOTH_HINT, GL_NICEST)
		glEnable(GL_LINE_SMOOTH)
		glEnable(GL_POLYGON_SMOOTH)
		glHint(GL_POLYGON_SMOOTH_HINT, GL_NICEST)

	def adjust_gl_view(self,w,h):
		"""
		adjust view onto our scene.
		"""
		glViewport(0, 0, w, h)
		glMatrixMode(GL_PROJECTION)
		glLoadIdentity()
		glOrtho(0, w, h, 0, -1, 1)
		glMatrixMode(GL_MODELVIEW)
		glLoadIdentity()

	def clear_gl_screen(self):
		glClearColor(.9,.9,0.9,1.)
		glClear(GL_COLOR_BUFFER_BIT)

	########### Open, update, close #####################

	def open_window(self):
		if not self._window:
			self.input = {'button':None, 'mouse':(0,0)}

			# get glfw started
			if self.run_independently:
				glfwInit()
			window = glfwGetCurrentContext()
			self._window = glfwCreateWindow(self.window_size[0], self.window_size[1], self.name, None, window)
			glfwMakeContextCurrent(self._window)

			if not self._window:
				exit()

			glfwSetWindowPos(self._window,0,0)
			# Register callbacks window
			glfwSetFramebufferSizeCallback(self._window,self.on_resize)
			glfwSetWindowIconifyCallback(self._window,self.on_iconify)
			glfwSetKeyCallback(self._window,self.on_key)
			glfwSetCharCallback(self._window,self.on_char)
			glfwSetMouseButtonCallback(self._window,self.on_button)
			glfwSetCursorPosCallback(self._window,self.on_pos)
			glfwSetScrollCallback(self._window,self.on_scroll)

			# get glfw started
			if self.run_independently:
				init()
			self.basic_gl_setup()

			self.glfont = fs.Context()
			self.glfont.add_font('opensans',get_opensans_font_path())
			self.glfont.set_size(22)
			self.glfont.set_color_float((0.2,0.5,0.9,1.0))
			self.on_resize(self._window,*glfwGetFramebufferSize(self._window))
			glfwMakeContextCurrent(window)

			# self.gui = ui.UI()

	def update_window(self, g_pool, result  ):

		if not result:
			return

		if glfwWindowShouldClose(self._window):
			self.close_window()
			return

		if self._window != None:
			glfwMakeContextCurrent(self._window)
		else:
			return

		self.image_width , self.image_height = g_pool.capture.frame_size

		latest_circle = result['circle']
		predicted_circle = result['predictedCircle']
		edges =  result['edges']
		sphere_models = result['models']

		self.clear_gl_screen()
		self.trackball.push()

		# 2. in pixel space draw video frame
		glLoadMatrixf(self.get_image_space_matrix(15))
		g_pool.image_tex.draw( quad=((0,self.image_height),(self.image_width,self.image_height),(self.image_width,0),(0,0)) ,alpha=0.5)

		glLoadMatrixf(self.get_adjusted_pixel_space_matrix(15))
		self.draw_frustum()

		glLoadMatrixf(self.get_anthropomorphic_matrix())
		model_count = 0;
		sphere_color = RGBA( 0,147/255.,147/255.,0.2)
		initial_sphere_color = RGBA( 0,147/255.,147/255.,0.2)

		alternative_sphere_color = RGBA( 1,0.5,0.5,0.05)
		alternative_initial_sphere_color = RGBA( 1,0.5,0.5,0.05)

		for model in sphere_models:
			bin_positions = model['binPositions']
			sphere = model['sphere']
			initial_sphere = model['initialSphere']

			if model_count == 0:
				# self.draw_sphere(initial_sphere[0],initial_sphere[1], color = sphere_color )
				self.draw_sphere(sphere[0],sphere[1],  color = initial_sphere_color )
				draw_points(bin_positions, 3 , RGBA(0.6,0.0,0.6,0.5) )

			else:
				#self.draw_sphere(initial_sphere[0],initial_sphere[1], color = alternative_sphere_color )
				self.draw_sphere(sphere[0],sphere[1],  color = alternative_initial_sphere_color )

			model_count += 1


		self.draw_circle( latest_circle[0], latest_circle[1], latest_circle[2], RGBA(0.0,1.0,1.0,0.4))
		# self.draw_circle( predicted_circle[0], predicted_circle[1], predicted_circle[2], RGBA(1.0,0.0,0.0,0.4))

		draw_points(edges, 2 , RGBA(1.0,0.0,0.6,0.5) )

		glLoadMatrixf(self.get_anthropomorphic_matrix())
		self.draw_coordinate_system(4)

		self.trackball.pop()

		self.draw_debug_info(result)

		glfwSwapBuffers(self._window)
		glfwPollEvents()
		return True

	def close_window(self):
		if self._window:
			glfwDestroyWindow(self._window)
			self._window = None

	############ window callbacks #################
	def on_resize(self,window,w, h):
		h = max(h,1)
		w = max(w,1)
		self.trackball.set_window_size(w,h)

		self.window_size = (w,h)
		active_window = glfwGetCurrentContext()
		glfwMakeContextCurrent(window)
		self.adjust_gl_view(w,h)
		glfwMakeContextCurrent(active_window)

	def on_char(self,window,char):
		if char == ord('r'):
			self.trackball.distance = [0,0,-0.1]
			self.trackball.pitch = 0
			self.trackball.roll = 0

	def on_button(self,window,button, action, mods):
		# self.gui.update_button(button,action,mods)
		if action == GLFW_PRESS:
			self.input['button'] = button
			self.input['mouse'] = glfwGetCursorPos(window)
		if action == GLFW_RELEASE:
			self.input['button'] = None

	def on_pos(self,window,x, y):
		hdpi_factor = float(glfwGetFramebufferSize(window)[0]/glfwGetWindowSize(window)[0])
		x,y = x*hdpi_factor,y*hdpi_factor
		# self.gui.update_mouse(x,y)
		if self.input['button']==GLFW_MOUSE_BUTTON_RIGHT:
			old_x,old_y = self.input['mouse']
			self.trackball.drag_to(x-old_x,y-old_y)
			self.input['mouse'] = x,y
		if self.input['button']==GLFW_MOUSE_BUTTON_LEFT:
			old_x,old_y = self.input['mouse']
			self.trackball.pan_to(x-old_x,y-old_y)
			self.input['mouse'] = x,y

	def on_scroll(self,window,x,y):
		self.trackball.zoom_to(y)

	def on_iconify(self,window,iconified): pass
	def on_key(self,window, key, scancode, action, mods): pass
Beispiel #4
0
class Eye_Visualizer(Visualizer):
    def __init__(self, g_pool, focal_length):
        super().__init__(g_pool, "Debug Visualizer", False)

        self.focal_length = focal_length
        self.image_width = 640  # right values are assigned in update
        self.image_height = 480

        camera_fov = math.degrees(2.0 * math.atan(self.image_height /
                                                  (2.0 * self.focal_length)))
        self.trackball = Trackball(camera_fov)

    ############## MATRIX FUNCTIONS ##############################

    def get_anthropomorphic_matrix(self):
        temp = np.identity(4)
        temp[2, 2] *= -1
        return temp

    def get_adjusted_pixel_space_matrix(self, scale):
        # returns a homoegenous matrix
        temp = self.get_anthropomorphic_matrix()
        temp[3, 3] *= scale
        return temp

    def get_image_space_matrix(self, scale=1.):
        temp = self.get_adjusted_pixel_space_matrix(scale)
        temp[1, 1] *= -1  #image origin is top left
        temp[0, 3] = -self.image_width / 2.0
        temp[1, 3] = self.image_height / 2.0
        temp[2, 3] = -self.focal_length
        return temp.T

    def get_pupil_transformation_matrix(self,
                                        circle_normal,
                                        circle_center,
                                        circle_scale=1.0):
        """
            OpenGL matrix convention for typical GL software
            with positive Y=up and positive Z=rearward direction
            RT = right
            UP = up
            BK = back
            POS = position/translation
            US = uniform scale

            float transform[16];

            [0] [4] [8 ] [12]
            [1] [5] [9 ] [13]
            [2] [6] [10] [14]
            [3] [7] [11] [15]

            [RT.x] [UP.x] [BK.x] [POS.x]
            [RT.y] [UP.y] [BK.y] [POS.y]
            [RT.z] [UP.z] [BK.z] [POS.Z]
            [    ] [    ] [    ] [US   ]
        """
        temp = self.get_anthropomorphic_matrix()
        right = temp[:3, 0]
        up = temp[:3, 1]
        back = temp[:3, 2]
        translation = temp[:3, 3]
        back[:] = np.array(circle_normal)
        back[2] *= -1  #our z axis is inverted

        if np.linalg.norm(back) != 0:
            back[:] /= np.linalg.norm(back)
            right[:] = get_perpendicular_vector(back) / np.linalg.norm(
                get_perpendicular_vector(back))
            up[:] = np.cross(right, back) / np.linalg.norm(
                np.cross(right, back))
            right[:] *= circle_scale
            back[:] *= circle_scale
            up[:] *= circle_scale
            translation[:] = np.array(circle_center)
            translation[2] *= -1
        return temp.T

    ############## DRAWING FUNCTIONS ##############################

    def draw_debug_info(self, result):
        models = result['models']
        eye = models[0]['sphere']
        direction = result['circle'][1]
        pupil_radius = result['circle'][2]

        status = ' Eyeball center : X: %.2fmm Y: %.2fmm Z: %.2fmm\n Pupil direction:  X: %.2f Y: %.2f Z: %.2f\n Pupil Diameter: %.2fmm\n  ' \
        %(eye[0][0], eye[0][1],eye[0][2],
        direction[0], direction[1],direction[2], pupil_radius*2)

        self.glfont.push_state()
        self.glfont.set_color_float((0, 0, 0, 1))

        self.glfont.draw_multi_line_text(5, 20, status)

        #draw model info for each model
        delta_y = 20
        for model in models:
            modelStatus = (
                'Model: %d \n' % model['model_id'],
                '    age: %.1fs\n' %
                (self.g_pool.get_timestamp() - model['birth_timestamp']),
                '    maturity: %.3f\n' % model['maturity'],
                '    solver fit: %.6f\n' % model['solver_fit'],
                '    confidence: %.6f\n' % model['confidence'],
                '    performance: %.6f\n' % model['performance'],
                '    perf.Grad.: %.3e\n' % model['performance_gradient'],
            )
            modeltext = ''.join(modelStatus)
            self.glfont.draw_multi_line_text(self.window_size[0] - 200,
                                             delta_y, modeltext)

            delta_y += 160

        self.glfont.pop_state()

    def draw_circle(self,
                    circle_center,
                    circle_normal,
                    circle_radius,
                    color=RGBA(1.1, 0.2, .8),
                    num_segments=20):
        vertices = []
        vertices.append((0, 0, 0))  # circle center

        #create circle vertices in the xy plane
        for i in np.linspace(0.0, 2.0 * math.pi, num_segments):
            x = math.sin(i)
            y = math.cos(i)
            z = 0
            vertices.append((x, y, z))

        glPushMatrix()
        glMatrixMode(GL_MODELVIEW)
        glLoadMatrixf(
            self.get_pupil_transformation_matrix(circle_normal, circle_center,
                                                 circle_radius))
        glutils.draw_polyline((vertices),
                              color=color,
                              line_type=GL_TRIANGLE_FAN)  # circle
        glutils.draw_polyline([(0, 0, 0), (0, 0, 4)],
                              color=RGBA(0, 0, 0),
                              line_type=GL_LINES)  #normal
        glPopMatrix()

    def update_window(self, g_pool, result):

        if not result:
            return

        if not self.window:
            return

        self.begin_update_window()

        self.image_width, self.image_height = g_pool.capture.frame_size

        latest_circle = result['circle']
        predicted_circle = result['predicted_circle']
        edges = result['edges']
        sphere_models = result['models']

        self.clear_gl_screen()
        self.trackball.push()

        # 2. in pixel space draw video frame
        glLoadMatrixf(self.get_image_space_matrix(15))
        g_pool.image_tex.draw(quad=((0, self.image_height),
                                    (self.image_width, self.image_height),
                                    (self.image_width, 0), (0, 0)),
                              alpha=0.5)

        glLoadMatrixf(self.get_adjusted_pixel_space_matrix(15))
        self.draw_frustum(self.image_width, self.image_height,
                          self.focal_length)

        glLoadMatrixf(self.get_anthropomorphic_matrix())
        model_count = 0
        sphere_color = RGBA(0, 147 / 255., 147 / 255., 0.2)
        initial_sphere_color = RGBA(0, 147 / 255., 147 / 255., 0.2)

        alternative_sphere_color = RGBA(1, 0.5, 0.5, 0.05)
        alternative_initial_sphere_color = RGBA(1, 0.5, 0.5, 0.05)

        for model in sphere_models:
            bin_positions = model['bin_positions']
            sphere = model['sphere']
            initial_sphere = model['initial_sphere']

            if model_count == 0:
                # self.draw_sphere(initial_sphere[0],initial_sphere[1], color = sphere_color )
                self.draw_sphere(sphere[0],
                                 sphere[1],
                                 color=initial_sphere_color)
                glutils.draw_points(bin_positions, 3, RGBA(0.6, 0.0, 0.6, 0.5))

            else:
                #self.draw_sphere(initial_sphere[0],initial_sphere[1], color = alternative_sphere_color )
                self.draw_sphere(sphere[0],
                                 sphere[1],
                                 color=alternative_initial_sphere_color)

            model_count += 1

        self.draw_circle(latest_circle[0], latest_circle[1], latest_circle[2],
                         RGBA(0.0, 1.0, 1.0, 0.4))
        # self.draw_circle( predicted_circle[0], predicted_circle[1], predicted_circle[2], RGBA(1.0,0.0,0.0,0.4))

        glutils.draw_points(edges, 2, RGBA(1.0, 0.0, 0.6, 0.5))

        glLoadMatrixf(self.get_anthropomorphic_matrix())
        self.draw_coordinate_system(4)

        self.trackball.pop()

        self.draw_debug_info(result)

        self.end_update_window()

        return True

    ############ window callbacks #################
    def on_resize(self, window, w, h):
        Visualizer.on_resize(self, window, w, h)
        self.trackball.set_window_size(w, h)

    def on_window_char(self, window, char):
        if char == ord('r'):
            self.trackball.distance = [0, 0, -0.1]
            self.trackball.pitch = 0
            self.trackball.roll = 0

    def on_scroll(self, window, x, y):
        self.trackball.zoom_to(y)
Beispiel #5
0
class Eye_Visualizer(Visualizer):
	def __init__(self,g_pool , focal_length ):
		super(Eye_Visualizer, self).__init__(g_pool , "Debug Visualizer", False)

		self.focal_length = focal_length
		self.image_width = 640 # right values are assigned in update
		self.image_height = 480

		camera_fov = math.degrees(2.0 * math.atan( self.image_height / (2.0 * self.focal_length)))
		self.trackball = Trackball(camera_fov)

	############## MATRIX FUNCTIONS ##############################

	def get_anthropomorphic_matrix(self):
		temp =  np.identity(4)
		temp[2,2] *= -1
		return temp


	def get_adjusted_pixel_space_matrix(self,scale):
		# returns a homoegenous matrix
		temp = self.get_anthropomorphic_matrix()
		temp[3,3] *= scale
		return temp

	def get_image_space_matrix(self,scale=1.):
		temp = self.get_adjusted_pixel_space_matrix(scale)
		temp[1,1] *=-1 #image origin is top left
		temp[0,3] = -self.image_width/2.0
		temp[1,3] = self.image_height/2.0
		temp[2,3] = -self.focal_length
		return temp.T

	def get_pupil_transformation_matrix(self,circle_normal,circle_center, circle_scale = 1.0):
		"""
			OpenGL matrix convention for typical GL software
			with positive Y=up and positive Z=rearward direction
			RT = right
			UP = up
			BK = back
			POS = position/translation
			US = uniform scale

			float transform[16];

			[0] [4] [8 ] [12]
			[1] [5] [9 ] [13]
			[2] [6] [10] [14]
			[3] [7] [11] [15]

			[RT.x] [UP.x] [BK.x] [POS.x]
			[RT.y] [UP.y] [BK.y] [POS.y]
			[RT.z] [UP.z] [BK.z] [POS.Z]
			[    ] [    ] [    ] [US   ]
		"""
		temp = self.get_anthropomorphic_matrix()
		right = temp[:3,0]
		up = temp[:3,1]
		back = temp[:3,2]
		translation = temp[:3,3]
		back[:] = np.array(circle_normal)
		back[2] *=-1 #our z axis is inverted

		if np.linalg.norm(back) != 0:
			back[:] /= np.linalg.norm(back)
			right[:] = get_perpendicular_vector(back)/np.linalg.norm(get_perpendicular_vector(back))
			up[:] = np.cross(right,back)/np.linalg.norm(np.cross(right,back))
			right[:] *= circle_scale
			back[:] *=circle_scale
			up[:] *=circle_scale
			translation[:] = np.array(circle_center)
			translation[2] *= -1
		return   temp.T

	############## DRAWING FUNCTIONS ##############################

	def draw_debug_info(self, result  ):
		models = result['models']
		eye = models[0]['sphere'];
		direction = result['circle'][1];
		pupil_radius = result['circle'][2];

		status = ' Eyeball center : X: %.2fmm Y: %.2fmm Z: %.2fmm\n Pupil direction:  X: %.2f Y: %.2f Z: %.2f\n Pupil Diameter: %.2fmm\n  ' \
		%(eye[0][0], eye[0][1],eye[0][2],
		direction[0], direction[1],direction[2], pupil_radius*2)

		self.glfont.push_state()
		self.glfont.set_color_float( (0,0,0,1) )

		self.glfont.draw_multi_line_text(5,20,status)


		#draw model info for each model
		delta_y = 20
		for model in models:
			modelStatus =	('Model: %d \n' %  model['model_id'] ,
							'    age: %.1fs\n' %(self.g_pool.get_timestamp()-model['birth_timestamp']) ,
							'    maturity: %.3f\n' % model['maturity'] ,
							'    solver fit: %.6f\n' % model['solver_fit'] ,
							'    confidence: %.6f\n' % model['confidence'] ,
							'    performance: %.6f\n' % model['performance'] ,
							'    perf.Grad.: %.3e\n' % model['performance_gradient'] ,
							)
			modeltext = ''.join( modelStatus )
			self.glfont.draw_multi_line_text(self.window_size[0] - 200 ,delta_y, modeltext)

			delta_y += 160

		self.glfont.pop_state()

	def draw_circle(self, circle_center, circle_normal, circle_radius, color=RGBA(1.1,0.2,.8), num_segments = 20):
		vertices = []
		vertices.append( (0,0,0) )  # circle center

		#create circle vertices in the xy plane
		for i in np.linspace(0.0, 2.0*math.pi , num_segments ):
		    x = math.sin(i)
		    y = math.cos(i)
		    z = 0
		    vertices.append((x,y,z))

		glPushMatrix()
		glMatrixMode(GL_MODELVIEW )
		glLoadMatrixf(self.get_pupil_transformation_matrix(circle_normal,circle_center, circle_radius))
		glutils.draw_polyline((vertices),color=color, line_type = GL_TRIANGLE_FAN) # circle
		glutils.draw_polyline( [ (0,0,0), (0,0, 4) ] ,color=RGBA(0,0,0), line_type = GL_LINES) #normal
		glPopMatrix()

	def update_window(self, g_pool, result ):

		if not result:
			return

		if not self.window:
			return

		self.begin_update_window()

		self.image_width , self.image_height = g_pool.capture.frame_size

		latest_circle = result['circle']
		predicted_circle = result['predicted_circle']
		edges =  result['edges']
		sphere_models = result['models']

		self.clear_gl_screen()
		self.trackball.push()

		# 2. in pixel space draw video frame
		glLoadMatrixf(self.get_image_space_matrix(15))
		g_pool.image_tex.draw( quad=((0,self.image_height),(self.image_width,self.image_height),(self.image_width,0),(0,0)) ,alpha=0.5)

		glLoadMatrixf(self.get_adjusted_pixel_space_matrix(15))
		self.draw_frustum( self.image_width, self.image_height, self.focal_length )

		glLoadMatrixf(self.get_anthropomorphic_matrix())
		model_count = 0;
		sphere_color = RGBA( 0,147/255.,147/255.,0.2)
		initial_sphere_color = RGBA( 0,147/255.,147/255.,0.2)

		alternative_sphere_color = RGBA( 1,0.5,0.5,0.05)
		alternative_initial_sphere_color = RGBA( 1,0.5,0.5,0.05)

		for model in sphere_models:
			bin_positions = model['bin_positions']
			sphere = model['sphere']
			initial_sphere = model['initial_sphere']

			if model_count == 0:
				# self.draw_sphere(initial_sphere[0],initial_sphere[1], color = sphere_color )
				self.draw_sphere(sphere[0],sphere[1],  color = initial_sphere_color )
				glutils.draw_points(bin_positions, 3 , RGBA(0.6,0.0,0.6,0.5) )

			else:
				#self.draw_sphere(initial_sphere[0],initial_sphere[1], color = alternative_sphere_color )
				self.draw_sphere(sphere[0],sphere[1],  color = alternative_initial_sphere_color )

			model_count += 1


		self.draw_circle( latest_circle[0], latest_circle[1], latest_circle[2], RGBA(0.0,1.0,1.0,0.4))
		# self.draw_circle( predicted_circle[0], predicted_circle[1], predicted_circle[2], RGBA(1.0,0.0,0.0,0.4))

		glutils.draw_points(edges, 2 , RGBA(1.0,0.0,0.6,0.5) )

		glLoadMatrixf(self.get_anthropomorphic_matrix())
		self.draw_coordinate_system(4)

		self.trackball.pop()

		self.draw_debug_info(result)

		self.end_update_window()

		return True


	############ window callbacks #################
	def on_resize(self,window,w, h):
		Visualizer.on_resize(self,window,w, h)
		self.trackball.set_window_size(w,h)

	def on_char(self,window,char):
		if char == ord('r'):
			self.trackball.distance = [0,0,-0.1]
			self.trackball.pitch = 0
			self.trackball.roll = 0


	def on_scroll(self,window,x,y):
		self.trackball.zoom_to(y)
class Calibration_Visualizer(object):
	def __init__(self, g_pool, world_camera_intrinsics , cal_ref_points_3d, eye_to_world_matrix0 , cal_gaze_points0_3d, eye_to_world_matrix1 = np.eye(4) , cal_gaze_points1_3d = [],   name = "Debug Calibration Visualizer", run_independently = False):
       # super(Visualizer, self).__init__()

		self.g_pool = g_pool
		self.image_width = 640 # right values are assigned in update
		self.focal_length = 620
		self.image_height = 480

		self.eye_to_world_matrix0 = eye_to_world_matrix0
		self.eye_to_world_matrix1 = eye_to_world_matrix1

		self.cal_ref_points_3d = cal_ref_points_3d
		self.cal_gaze_points0_3d = cal_gaze_points0_3d
		self.cal_gaze_points1_3d = cal_gaze_points1_3d

		self.world_camera_width = world_camera_intrinsics['resolution'][0]
		self.world_camera_height = world_camera_intrinsics['resolution'][1]
		self.world_camera_focal = (world_camera_intrinsics['camera_matrix'][0][0] +  world_camera_intrinsics['camera_matrix'][1][1] ) / 2.0

		# transformation matrices
		self.anthromorphic_matrix = self.get_anthropomorphic_matrix()
		self.adjusted_pixel_space_matrix = self.get_adjusted_pixel_space_matrix(1)

		self.name = name
		self.window_size = (640,480)
		self.window = None
		self.input = None
		self.run_independently = run_independently

		camera_fov = math.degrees(2.0 * math.atan( self.window_size[0] / (2.0 * self.focal_length)))
		self.trackball = Trackball(camera_fov)
		self.trackball.distance = [0,0,-0.1]
		self.trackball.pitch = 0
		self.trackball.roll = 180

	############## MATRIX FUNCTIONS ##############################

	def get_anthropomorphic_matrix(self):
		temp =  np.identity(4)
		return temp

	def get_adjusted_pixel_space_matrix(self,scale  = 1.0):
		# returns a homoegenous matrix
		temp = self.get_anthropomorphic_matrix()
		temp[3,3] *= scale
		return temp

	def get_image_space_matrix(self,scale=1.):
		temp = self.get_adjusted_pixel_space_matrix(scale)
		#temp[1,1] *=-1 #image origin is top left
		#temp[2,2] *=-1 #image origin is top left
		temp[0,3] = -self.world_camera_width/2.0
		temp[1,3] = -self.world_camera_height/2.0
		temp[2,3] = self.world_camera_focal
		return temp.T

	def get_pupil_transformation_matrix(self,circle_normal,circle_center, circle_scale = 1.0):
		"""
			OpenGL matrix convention for typical GL software
			with positive Y=up and positive Z=rearward direction
			RT = right
			UP = up
			BK = back
			POS = position/translation
			US = uniform scale

			float transform[16];

			[0] [4] [8 ] [12]
			[1] [5] [9 ] [13]
			[2] [6] [10] [14]
			[3] [7] [11] [15]

			[RT.x] [UP.x] [BK.x] [POS.x]
			[RT.y] [UP.y] [BK.y] [POS.y]
			[RT.z] [UP.z] [BK.z] [POS.Z]
			[    ] [    ] [    ] [US   ]
		"""
		temp = self.get_anthropomorphic_matrix()
		right = temp[:3,0]
		up = temp[:3,1]
		back = temp[:3,2]
		translation = temp[:3,3]
		back[:] = np.array(circle_normal)

		# if np.linalg.norm(back) != 0:
		back[:] /= np.linalg.norm(back)
		right[:] = get_perpendicular_vector(back)/np.linalg.norm(get_perpendicular_vector(back))
		up[:] = np.cross(right,back)/np.linalg.norm(np.cross(right,back))
		right[:] *= circle_scale
		back[:] *=circle_scale
		up[:] *=circle_scale
		translation[:] = np.array(circle_center)
		return   temp.T

	############## DRAWING FUNCTIONS ##############################

	def draw_frustum(self, width, height , length):

		W = width/2.0
		H = height/2.0
		Z = length
		# draw it
		glLineWidth(1)
		glColor4f( 1, 0.5, 0, 0.5 )
		glBegin( GL_LINE_LOOP )
		glVertex3f( 0, 0, 0 )
		glVertex3f( -W, H, Z )
		glVertex3f( W, H, Z )
		glVertex3f( 0, 0, 0 )
		glVertex3f( W, H, Z )
		glVertex3f( W, -H, Z )
		glVertex3f( 0, 0, 0 )
		glVertex3f( W, -H, Z )
		glVertex3f( -W, -H, Z )
		glVertex3f( 0, 0, 0 )
		glVertex3f( -W, -H, Z )
		glVertex3f( -W, H, Z )
		glEnd( )

	def draw_coordinate_system(self,l=1):
		# Draw x-axis line. RED
		glLineWidth(2)
		glColor3f( 1, 0, 0 )
		glBegin( GL_LINES )
		glVertex3f( 0, 0, 0 )
		glVertex3f( l, 0, 0 )
		glEnd( )

		# Draw y-axis line. GREEN.
		glColor3f( 0, 1, 0 )
		glBegin( GL_LINES )
		glVertex3f( 0, 0, 0 )
		glVertex3f( 0, l, 0 )
		glEnd( )

		# Draw z-axis line. BLUE
		glColor3f( 0, 0, 1 )
		glBegin( GL_LINES )
		glVertex3f( 0, 0, 0 )
		glVertex3f( 0, 0, l )
		glEnd( )

	def draw_sphere(self,sphere_position, sphere_radius,contours = 45, color =RGBA(.2,.5,0.5,.5) ):
		# this function draws the location of the eye sphere
		glPushMatrix()

		glTranslatef(sphere_position[0],sphere_position[1],sphere_position[2]) #sphere[0] contains center coordinates (x,y,z)
		glTranslatef(0,0,sphere_radius) #sphere[1] contains radius
		for i in xrange(1,contours+1):

			glTranslatef(0,0, -sphere_radius/contours*2)
			position = sphere_radius - i*sphere_radius*2/contours
			draw_radius = np.sqrt(sphere_radius**2 - position**2)
			glPushMatrix()
			glScalef(draw_radius,draw_radius,1)
			draw_polyline((circle_xy),2,color)
			glPopMatrix()

		glPopMatrix()


	def draw_circle(self, circle_center, circle_normal, circle_radius, color=RGBA(1.1,0.2,.8), num_segments = 20):
		vertices = []
		vertices.append( (0,0,0) )  # circle center

		#create circle vertices in the xy plane
		for i in np.linspace(0.0, 2.0*math.pi , num_segments ):
			x = math.sin(i)
			y = math.cos(i)
			z = 0
			vertices.append((x,y,z))

		glPushMatrix()
		glMatrixMode(GL_MODELVIEW )
		glLoadMatrixf(self.get_pupil_transformation_matrix(circle_normal,circle_center, circle_radius))
		draw_polyline((vertices),color=color, line_type = GL_TRIANGLE_FAN) # circle
		draw_polyline( [ (0,0,0), (0,0, 4) ] ,color=RGBA(0,0,0), line_type = GL_LINES) #normal
		glPopMatrix()



	def basic_gl_setup(self):
		glEnable(GL_POINT_SPRITE )
		glEnable(GL_VERTEX_PROGRAM_POINT_SIZE) # overwrite pointsize
		glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
		glEnable(GL_BLEND)
		glClearColor(.8,.8,.8,1.)
		glEnable(GL_LINE_SMOOTH)
		# glEnable(GL_POINT_SMOOTH)
		glHint(GL_LINE_SMOOTH_HINT, GL_NICEST)
		glEnable(GL_LINE_SMOOTH)
		glEnable(GL_POLYGON_SMOOTH)
		glHint(GL_POLYGON_SMOOTH_HINT, GL_NICEST)

	def adjust_gl_view(self,w,h):
		"""
		adjust view onto our scene.
		"""
		glViewport(0, 0, w, h)
		glMatrixMode(GL_PROJECTION)
		glLoadIdentity()
		glOrtho(0, w, h, 0, -1, 1)
		glMatrixMode(GL_MODELVIEW)
		glLoadIdentity()

	def clear_gl_screen(self):
		glClearColor(.9,.9,0.9,1.)
		glClear(GL_COLOR_BUFFER_BIT)

	########### Open, update, close #####################


	def open_window(self):
		if not self.window:
			self.input = {'button':None, 'mouse':(0,0)}

			# get glfw started
			if self.run_independently:
				glfwInit()
			self.window = glfwCreateWindow(self.window_size[0], self.window_size[1], self.name, None, share=self.g_pool.main_window )
			active_window = glfwGetCurrentContext();
			glfwMakeContextCurrent(self.window)

			glfwSetWindowPos(self.window,0,0)
			# Register callbacks window
			glfwSetFramebufferSizeCallback(self.window,self.on_resize)
			glfwSetWindowIconifyCallback(self.window,self.on_iconify)
			glfwSetKeyCallback(self.window,self.on_key)
			glfwSetCharCallback(self.window,self.on_char)
			glfwSetMouseButtonCallback(self.window,self.on_button)
			glfwSetCursorPosCallback(self.window,self.on_pos)
			glfwSetScrollCallback(self.window,self.on_scroll)

			# get glfw started
			if self.run_independently:
				init()
			self.basic_gl_setup()

			self.glfont = fs.Context()
			self.glfont.add_font('opensans',get_opensans_font_path())
			self.glfont.set_size(22)
			self.glfont.set_color_float((0.2,0.5,0.9,1.0))
			self.on_resize(self.window,*glfwGetFramebufferSize(self.window))
			glfwMakeContextCurrent(active_window)

			# self.gui = ui.UI()

	def update_window(self, g_pool , gaze_points0 , sphere0 , gaze_points1 = [] , sphere1 = None, intersection_points = []  ):
		if self.window:
			if glfwWindowShouldClose(self.window):
				self.close_window()
				return

			active_window = glfwGetCurrentContext()
			glfwMakeContextCurrent(self.window)

			self.clear_gl_screen()
			self.trackball.push()

			# use opencv coordinate system
			#glMatrixMode( GL_PROJECTION )
			#glScalef( 1. ,-1. , -1. )
			glMatrixMode( GL_MODELVIEW )

			# draw things in world camera coordinate system
			glPushMatrix()
			glLoadIdentity()

			calibration_points_line_color = RGBA(0.5,0.5,0.5,0.05);
			error_line_color = RGBA(1.0,0.0,0.0,0.5)

			self.draw_coordinate_system(200)
			self.draw_frustum( self.world_camera_width/ 10.0 , self.world_camera_height/ 10.0 , self.world_camera_focal / 10.0)

			for p in self.cal_ref_points_3d:
				draw_polyline( [ (0,0,0), p]  , 1 , calibration_points_line_color, line_type = GL_LINES)
			#calibration points
			draw_points( self.cal_ref_points_3d , 4 , RGBA( 0, 1, 1, 1 ) )


			glPopMatrix()

			if sphere0:

				# draw things in first eye oordinate system
				glPushMatrix()
				glLoadMatrixf( self.eye_to_world_matrix0.T )

				sphere_center0 = list(sphere0['center'])
				sphere_radius0 = sphere0['radius']

				self.draw_sphere(sphere_center0,sphere_radius0,  color = RGBA(1,1,0,1))

				for p in self.cal_gaze_points0_3d:
					draw_polyline( [ sphere_center0, p]  , 1 , calibration_points_line_color, line_type = GL_LINES)
				#calibration points
				draw_points( self.cal_gaze_points0_3d , 4 , RGBA( 1, 0, 1, 1 ) )

				# eye camera
				self.draw_coordinate_system(60)
				self.draw_frustum( self.image_width / 10.0, self.image_height / 10.0, self.focal_length /10.)

				draw_points( gaze_points0 , 2 , RGBA( 1, 0, 0, 1 ) )
				for p in gaze_points0:
					draw_polyline( [sphere_center0, p]  , 1 , RGBA(0,0,0,1), line_type = GL_LINES)

				glPopMatrix()

				#draw error lines form eye gaze points to world camera ref points
				for(cal_gaze_point,ref_point) in zip(self.cal_gaze_points0_3d, self.cal_ref_points_3d):
					point = np.zeros(4)
					point[:3] = cal_gaze_point
					point[3] = 1.0
					point =  self.eye_to_world_matrix0.dot( point )
					point = np.squeeze(np.asarray(point))
					draw_polyline( [ point[:3], ref_point]  , 1 , error_line_color, line_type = GL_LINES)


			# if we have a second eye
			if sphere1:
				# draw things in second eye oordinate system
				glPushMatrix()
				glLoadMatrixf( self.eye_to_world_matrix1.T )

				sphere_center1 = list(sphere1['center'])
				sphere_radius1 = sphere1['radius']

				self.draw_sphere(sphere_center1,sphere_radius1,  color = RGBA(1,1,0,1))

				for p in self.cal_gaze_points1_3d:
					draw_polyline( [ sphere_center1, p]  , 1 , calibration_points_line_color, line_type = GL_LINES)
				#calibration points
				draw_points( self.cal_gaze_points1_3d , 4 , RGBA( 1, 0, 1, 1 ) )

				# eye camera
				self.draw_coordinate_system(60)
				self.draw_frustum( self.image_width / 10.0, self.image_height / 10.0, self.focal_length /10.)

				draw_points( gaze_points1 , 2 , RGBA( 1, 0, 0, 1 ) )
				for p in gaze_points1:
					draw_polyline( [sphere_center1, p]  , 1 , RGBA(0,0,0,1), line_type = GL_LINES)

				glPopMatrix()


				#draw error lines form eye gaze points to world camera ref points
				for(cal_gaze_point,ref_point) in zip(self.cal_gaze_points1_3d, self.cal_ref_points_3d):
					point = np.zeros(4)
					point[:3] = cal_gaze_point
					point[3] = 1.0
					point =  self.eye_to_world_matrix1.dot( point )
					point = np.squeeze(np.asarray(point))
					draw_polyline( [ point[:3], ref_point]  , 1 , error_line_color, line_type = GL_LINES)


			#intersection points in world coordinate system
			if len(intersection_points) > 0:
				draw_points( intersection_points , 2 , RGBA( 1, 0.5, 0.5, 1 ) )
				for p in intersection_points:
					draw_polyline( [(0,0,0), p]  , 1 , RGBA(0.3,0.3,0.9,1), line_type = GL_LINES)


			self.trackball.pop()


			glfwSwapBuffers(self.window)
			glfwPollEvents()
			glfwMakeContextCurrent(active_window)

	def close_window(self):
		if self.window:
			active_window = glfwGetCurrentContext();
			glfwDestroyWindow(self.window)
			self.window = None
			glfwMakeContextCurrent(active_window)

	############ window callbacks #################
	def on_resize(self,window,w, h):
		h = max(h,1)
		w = max(w,1)
		self.trackball.set_window_size(w,h)

		self.window_size = (w,h)
		active_window = glfwGetCurrentContext()
		glfwMakeContextCurrent(window)
		self.adjust_gl_view(w,h)
		glfwMakeContextCurrent(active_window)

	def on_char(self,window,char):
		if char == ord('r'):
			self.trackball.distance = [0,0,-0.1]
			self.trackball.pitch = 0
			self.trackball.roll = 180

	def on_button(self,window,button, action, mods):
		# self.gui.update_button(button,action,mods)
		if action == GLFW_PRESS:
			self.input['button'] = button
			self.input['mouse'] = glfwGetCursorPos(window)
		if action == GLFW_RELEASE:
			self.input['button'] = None

	def on_pos(self,window,x, y):
		hdpi_factor = float(glfwGetFramebufferSize(window)[0]/glfwGetWindowSize(window)[0])
		x,y = x*hdpi_factor,y*hdpi_factor
		# self.gui.update_mouse(x,y)
		if self.input['button']==GLFW_MOUSE_BUTTON_RIGHT:
			old_x,old_y = self.input['mouse']
			self.trackball.drag_to(x-old_x,y-old_y)
			self.input['mouse'] = x,y
		if self.input['button']==GLFW_MOUSE_BUTTON_LEFT:
			old_x,old_y = self.input['mouse']
			self.trackball.pan_to(x-old_x,y-old_y)
			self.input['mouse'] = x,y

	def on_scroll(self,window,x,y):
		self.trackball.zoom_to(y)

	def on_iconify(self,window,iconified): pass

	def on_key(self,window, key, scancode, action, mods): pass
Beispiel #7
0
class Calibration_Visualizer(object):
	def __init__(self, g_pool, world_camera_intrinsics , cal_ref_points_3d, eye_to_world_matrix0 , cal_gaze_points0_3d, eye_to_world_matrix1 = np.eye(4) , cal_gaze_points1_3d = [],   name = "Debug Calibration Visualizer", run_independently = False):
       # super(Visualizer, self).__init__()

		self.g_pool = g_pool
		self.image_width = 640 # right values are assigned in update
		self.focal_length = 620
		self.image_height = 480

		self.eye_to_world_matrix0 = eye_to_world_matrix0
		self.eye_to_world_matrix1 = eye_to_world_matrix1

		self.cal_ref_points_3d = cal_ref_points_3d
		self.cal_gaze_points0_3d = cal_gaze_points0_3d
		self.cal_gaze_points1_3d = cal_gaze_points1_3d

		self.world_camera_width = world_camera_intrinsics['resolution'][0]
		self.world_camera_height = world_camera_intrinsics['resolution'][1]
		self.world_camera_focal = (world_camera_intrinsics['camera_matrix'][0][0] +  world_camera_intrinsics['camera_matrix'][1][1] ) / 2.0

		# transformation matrices
		self.anthromorphic_matrix = self.get_anthropomorphic_matrix()
		self.adjusted_pixel_space_matrix = self.get_adjusted_pixel_space_matrix(1)

		self.name = name
		self.window_size = (640,480)
		self.window = None
		self.input = None
		self.run_independently = run_independently

		camera_fov = math.degrees(2.0 * math.atan( self.window_size[0] / (2.0 * self.focal_length)))
		self.trackball = Trackball(camera_fov)
		self.trackball.distance = [0,0,-0.1]
		self.trackball.pitch = 0
		self.trackball.roll = 180

	############## MATRIX FUNCTIONS ##############################

	def get_anthropomorphic_matrix(self):
		temp =  np.identity(4)
		return temp

	def get_adjusted_pixel_space_matrix(self,scale  = 1.0):
		# returns a homoegenous matrix
		temp = self.get_anthropomorphic_matrix()
		temp[3,3] *= scale
		return temp

	def get_image_space_matrix(self,scale=1.):
		temp = self.get_adjusted_pixel_space_matrix(scale)
		#temp[1,1] *=-1 #image origin is top left
		#temp[2,2] *=-1 #image origin is top left
		temp[0,3] = -self.world_camera_width/2.0
		temp[1,3] = -self.world_camera_height/2.0
		temp[2,3] = self.world_camera_focal
		return temp.T

	def get_pupil_transformation_matrix(self,circle_normal,circle_center, circle_scale = 1.0):
		"""
			OpenGL matrix convention for typical GL software
			with positive Y=up and positive Z=rearward direction
			RT = right
			UP = up
			BK = back
			POS = position/translation
			US = uniform scale

			float transform[16];

			[0] [4] [8 ] [12]
			[1] [5] [9 ] [13]
			[2] [6] [10] [14]
			[3] [7] [11] [15]

			[RT.x] [UP.x] [BK.x] [POS.x]
			[RT.y] [UP.y] [BK.y] [POS.y]
			[RT.z] [UP.z] [BK.z] [POS.Z]
			[    ] [    ] [    ] [US   ]
		"""
		temp = self.get_anthropomorphic_matrix()
		right = temp[:3,0]
		up = temp[:3,1]
		back = temp[:3,2]
		translation = temp[:3,3]
		back[:] = np.array(circle_normal)

		# if np.linalg.norm(back) != 0:
		back[:] /= np.linalg.norm(back)
		right[:] = get_perpendicular_vector(back)/np.linalg.norm(get_perpendicular_vector(back))
		up[:] = np.cross(right,back)/np.linalg.norm(np.cross(right,back))
		right[:] *= circle_scale
		back[:] *=circle_scale
		up[:] *=circle_scale
		translation[:] = np.array(circle_center)
		return   temp.T

	############## DRAWING FUNCTIONS ##############################

	def draw_frustum(self, width, height , length):

		W = width/2.0
		H = height/2.0
		Z = length
		# draw it
		glLineWidth(1)
		glColor4f( 1, 0.5, 0, 0.5 )
		glBegin( GL_LINE_LOOP )
		glVertex3f( 0, 0, 0 )
		glVertex3f( -W, H, Z )
		glVertex3f( W, H, Z )
		glVertex3f( 0, 0, 0 )
		glVertex3f( W, H, Z )
		glVertex3f( W, -H, Z )
		glVertex3f( 0, 0, 0 )
		glVertex3f( W, -H, Z )
		glVertex3f( -W, -H, Z )
		glVertex3f( 0, 0, 0 )
		glVertex3f( -W, -H, Z )
		glVertex3f( -W, H, Z )
		glEnd( )

	def draw_coordinate_system(self,l=1):
		# Draw x-axis line. RED
		glLineWidth(2)
		glColor3f( 1, 0, 0 )
		glBegin( GL_LINES )
		glVertex3f( 0, 0, 0 )
		glVertex3f( l, 0, 0 )
		glEnd( )

		# Draw y-axis line. GREEN.
		glColor3f( 0, 1, 0 )
		glBegin( GL_LINES )
		glVertex3f( 0, 0, 0 )
		glVertex3f( 0, l, 0 )
		glEnd( )

		# Draw z-axis line. BLUE
		glColor3f( 0, 0, 1 )
		glBegin( GL_LINES )
		glVertex3f( 0, 0, 0 )
		glVertex3f( 0, 0, l )
		glEnd( )

	def draw_sphere(self,sphere_position, sphere_radius,contours = 45, color =RGBA(.2,.5,0.5,.5) ):
		# this function draws the location of the eye sphere
		glPushMatrix()

		glTranslatef(sphere_position[0],sphere_position[1],sphere_position[2]) #sphere[0] contains center coordinates (x,y,z)
		glTranslatef(0,0,sphere_radius) #sphere[1] contains radius
		for i in xrange(1,contours+1):

			glTranslatef(0,0, -sphere_radius/contours*2)
			position = sphere_radius - i*sphere_radius*2/contours
			draw_radius = np.sqrt(sphere_radius**2 - position**2)
			glPushMatrix()
			glScalef(draw_radius,draw_radius,1)
			draw_polyline((circle_xy),2,color)
			glPopMatrix()

		glPopMatrix()


	def draw_circle(self, circle_center, circle_normal, circle_radius, color=RGBA(1.1,0.2,.8), num_segments = 20):
		vertices = []
		vertices.append( (0,0,0) )  # circle center

		#create circle vertices in the xy plane
		for i in np.linspace(0.0, 2.0*math.pi , num_segments ):
			x = math.sin(i)
			y = math.cos(i)
			z = 0
			vertices.append((x,y,z))

		glPushMatrix()
		glMatrixMode(GL_MODELVIEW )
		glLoadMatrixf(self.get_pupil_transformation_matrix(circle_normal,circle_center, circle_radius))
		draw_polyline((vertices),color=color, line_type = GL_TRIANGLE_FAN) # circle
		draw_polyline( [ (0,0,0), (0,0, 4) ] ,color=RGBA(0,0,0), line_type = GL_LINES) #normal
		glPopMatrix()



	def basic_gl_setup(self):
		glEnable(GL_POINT_SPRITE )
		glEnable(GL_VERTEX_PROGRAM_POINT_SIZE) # overwrite pointsize
		glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
		glEnable(GL_BLEND)
		glClearColor(.8,.8,.8,1.)
		glEnable(GL_LINE_SMOOTH)
		# glEnable(GL_POINT_SMOOTH)
		glHint(GL_LINE_SMOOTH_HINT, GL_NICEST)
		glEnable(GL_LINE_SMOOTH)
		glEnable(GL_POLYGON_SMOOTH)
		glHint(GL_POLYGON_SMOOTH_HINT, GL_NICEST)

	def adjust_gl_view(self,w,h):
		"""
		adjust view onto our scene.
		"""
		glViewport(0, 0, w, h)
		glMatrixMode(GL_PROJECTION)
		glLoadIdentity()
		glOrtho(0, w, h, 0, -1, 1)
		glMatrixMode(GL_MODELVIEW)
		glLoadIdentity()

	def clear_gl_screen(self):
		glClearColor(.9,.9,0.9,1.)
		glClear(GL_COLOR_BUFFER_BIT)

	########### Open, update, close #####################


	def open_window(self):
		if not self.window:
			self.input = {'button':None, 'mouse':(0,0)}

			# get glfw started
			if self.run_independently:
				glfwInit()
			self.window = glfwCreateWindow(self.window_size[0], self.window_size[1], self.name, None, share=self.g_pool.main_window )
			active_window = glfwGetCurrentContext();
			glfwMakeContextCurrent(self.window)

			glfwSetWindowPos(self.window,0,0)
			# Register callbacks window
			glfwSetFramebufferSizeCallback(self.window,self.on_resize)
			glfwSetWindowIconifyCallback(self.window,self.on_iconify)
			glfwSetKeyCallback(self.window,self.on_key)
			glfwSetCharCallback(self.window,self.on_char)
			glfwSetMouseButtonCallback(self.window,self.on_button)
			glfwSetCursorPosCallback(self.window,self.on_pos)
			glfwSetScrollCallback(self.window,self.on_scroll)

			# get glfw started
			if self.run_independently:
				init()
			self.basic_gl_setup()

			self.glfont = fs.Context()
			self.glfont.add_font('opensans',get_opensans_font_path())
			self.glfont.set_size(22)
			self.glfont.set_color_float((0.2,0.5,0.9,1.0))
			self.on_resize(self.window,*glfwGetFramebufferSize(self.window))
			glfwMakeContextCurrent(active_window)

			# self.gui = ui.UI()

	def update_window(self, g_pool , gaze_points0 , sphere0 , gaze_points1 = [] , sphere1 = None, intersection_points = []  ):
		if self.window:
			if glfwWindowShouldClose(self.window):
				self.close_window()
				return

			active_window = glfwGetCurrentContext()
			glfwMakeContextCurrent(self.window)

			self.clear_gl_screen()
			self.trackball.push()

			# use opencv coordinate system
			#glMatrixMode( GL_PROJECTION )
			#glScalef( 1. ,-1. , -1. )
			glMatrixMode( GL_MODELVIEW )

			# draw things in world camera coordinate system
			glPushMatrix()
			glLoadIdentity()

			calibration_points_line_color = RGBA(0.5,0.5,0.5,0.05);
			error_line_color = RGBA(1.0,0.0,0.0,0.5)

			self.draw_coordinate_system(200)
			self.draw_frustum( self.world_camera_width/ 10.0 , self.world_camera_height/ 10.0 , self.world_camera_focal / 10.0)

			for p in self.cal_ref_points_3d:
				draw_polyline( [ (0,0,0), p]  , 1 , calibration_points_line_color, line_type = GL_LINES)
			#calibration points
			draw_points( self.cal_ref_points_3d , 4 , RGBA( 0, 1, 1, 1 ) )


			glPopMatrix()

			if sphere0:

				# draw things in first eye oordinate system
				glPushMatrix()
				glLoadMatrixf( self.eye_to_world_matrix0.T )

				sphere_center0 = list(sphere0['center'])
				sphere_radius0 = sphere0['radius']

				self.draw_sphere(sphere_center0,sphere_radius0,  color = RGBA(1,1,0,1))

				for p in self.cal_gaze_points0_3d:
					draw_polyline( [ sphere_center0, p]  , 1 , calibration_points_line_color, line_type = GL_LINES)
				#calibration points
				draw_points( self.cal_gaze_points0_3d , 4 , RGBA( 1, 0, 1, 1 ) )

				# eye camera
				self.draw_coordinate_system(60)
				self.draw_frustum( self.image_width / 10.0, self.image_height / 10.0, self.focal_length /10.)

				draw_points( gaze_points0 , 2 , RGBA( 1, 0, 0, 1 ) )
				for p in gaze_points0:
					draw_polyline( [sphere_center0, p]  , 1 , RGBA(0,0,0,1), line_type = GL_LINES)

				glPopMatrix()

				#draw error lines form eye gaze points to world camera ref points
				for(cal_gaze_point,ref_point) in zip(self.cal_gaze_points0_3d, self.cal_ref_points_3d):
					point = np.zeros(4)
					point[:3] = cal_gaze_point
					point[3] = 1.0
					point =  self.eye_to_world_matrix0.dot( point )
					point = np.squeeze(np.asarray(point))
					draw_polyline( [ point[:3], ref_point]  , 1 , error_line_color, line_type = GL_LINES)


			# if we have a second eye
			if sphere1:
				# draw things in second eye oordinate system
				glPushMatrix()
				glLoadMatrixf( self.eye_to_world_matrix1.T )

				sphere_center1 = list(sphere1['center'])
				sphere_radius1 = sphere1['radius']

				self.draw_sphere(sphere_center1,sphere_radius1,  color = RGBA(1,1,0,1))

				for p in self.cal_gaze_points1_3d:
					draw_polyline( [ sphere_center1, p]  , 1 , calibration_points_line_color, line_type = GL_LINES)
				#calibration points
				draw_points( self.cal_gaze_points1_3d , 4 , RGBA( 1, 0, 1, 1 ) )

				# eye camera
				self.draw_coordinate_system(60)
				self.draw_frustum( self.image_width / 10.0, self.image_height / 10.0, self.focal_length /10.)

				draw_points( gaze_points1 , 2 , RGBA( 1, 0, 0, 1 ) )
				for p in gaze_points1:
					draw_polyline( [sphere_center1, p]  , 1 , RGBA(0,0,0,1), line_type = GL_LINES)

				glPopMatrix()


				#draw error lines form eye gaze points to world camera ref points
				for(cal_gaze_point,ref_point) in zip(self.cal_gaze_points1_3d, self.cal_ref_points_3d):
					point = np.zeros(4)
					point[:3] = cal_gaze_point
					point[3] = 1.0
					point =  self.eye_to_world_matrix1.dot( point )
					point = np.squeeze(np.asarray(point))
					draw_polyline( [ point[:3], ref_point]  , 1 , error_line_color, line_type = GL_LINES)


			#intersection points in world coordinate system
			if len(intersection_points) > 0:
				draw_points( intersection_points , 2 , RGBA( 1, 0.5, 0.5, 1 ) )
				for p in intersection_points:
					draw_polyline( [(0,0,0), p]  , 1 , RGBA(0.3,0.3,0.9,1), line_type = GL_LINES)


			self.trackball.pop()


			glfwSwapBuffers(self.window)
			glfwPollEvents()
			glfwMakeContextCurrent(active_window)

	def close_window(self):
		if self.window:
			glfwDestroyWindow(self.window)
			self.window = None

	############ window callbacks #################
	def on_resize(self,window,w, h):
		h = max(h,1)
		w = max(w,1)
		self.trackball.set_window_size(w,h)

		self.window_size = (w,h)
		active_window = glfwGetCurrentContext()
		glfwMakeContextCurrent(window)
		self.adjust_gl_view(w,h)
		glfwMakeContextCurrent(active_window)

	def on_char(self,window,char):
		if char == ord('r'):
			self.trackball.distance = [0,0,-0.1]
			self.trackball.pitch = 0
			self.trackball.roll = 180

	def on_button(self,window,button, action, mods):
		# self.gui.update_button(button,action,mods)
		if action == GLFW_PRESS:
			self.input['button'] = button
			self.input['mouse'] = glfwGetCursorPos(window)
		if action == GLFW_RELEASE:
			self.input['button'] = None

	def on_pos(self,window,x, y):
		hdpi_factor = float(glfwGetFramebufferSize(window)[0]/glfwGetWindowSize(window)[0])
		x,y = x*hdpi_factor,y*hdpi_factor
		# self.gui.update_mouse(x,y)
		if self.input['button']==GLFW_MOUSE_BUTTON_RIGHT:
			old_x,old_y = self.input['mouse']
			self.trackball.drag_to(x-old_x,y-old_y)
			self.input['mouse'] = x,y
		if self.input['button']==GLFW_MOUSE_BUTTON_LEFT:
			old_x,old_y = self.input['mouse']
			self.trackball.pan_to(x-old_x,y-old_y)
			self.input['mouse'] = x,y

	def on_scroll(self,window,x,y):
		self.trackball.zoom_to(y)

	def on_iconify(self,window,iconified): pass

	def on_key(self,window, key, scancode, action, mods): pass
Beispiel #8
0
class Eye_Visualizer(Visualizer):
    def __init__(self, g_pool, focal_length):
        super().__init__(g_pool, "Debug Visualizer", False)

        self.focal_length = focal_length
        self.image_width = 192  # right values are assigned in update
        self.image_height = 192

        camera_fov = math.degrees(2.0 * math.atan(self.image_height /
                                                  (2.0 * self.focal_length)))
        self.trackball = Trackball(camera_fov)

        # self.residuals_single_means = deque(np.zeros(50), 50)
        # self.residuals_single_stds = deque(np.zeros(50), 50)
        # self.residuals_means = deque(np.zeros(50), 50)
        # self.residuals_stds = deque(np.zeros(50), 50)

        self.eye = LeGrandEye()
        self.cost_history = deque([], maxlen=200)
        self.optimization_number = 0

    ############## MATRIX FUNCTIONS ############

    def get_anthropomorphic_matrix(self):
        temp = np.identity(4)
        temp[2, 2] *= -1
        return temp

    def get_adjusted_pixel_space_matrix(self, scale):
        # returns a homoegenous matrix
        temp = self.get_anthropomorphic_matrix()
        temp[3, 3] *= scale
        return temp

    def get_image_space_matrix(self, scale=1.0):
        temp = self.get_adjusted_pixel_space_matrix(scale)
        temp[1, 1] *= -1  # image origin is top left
        temp[0, 3] = -self.image_width / 2.0
        temp[1, 3] = self.image_height / 2.0
        temp[2, 3] = -self.focal_length
        return temp.T

    ############## DRAWING FUNCTIONS ###########

    def draw_eye(self, result):

        self.eye.pupil_radius = result["circle_3d"]["radius"]
        self.eye.move_to_point([
            result["sphere"]["center"][0],
            -result["sphere"]["center"][1],
            -result["sphere"]["center"][2],
        ])
        if result["confidence"] > 0.0 and not np.isnan(
                result["circle_3d"]["normal"][0]):
            self.eye.update_from_gaze_vector([
                result["circle_3d"]["normal"][0],
                -result["circle_3d"]["normal"][1],
                result["circle_3d"]["normal"][2],
            ])
            self.eye.draw_gl(alpha=0.7)

    def draw_debug_info(self, result):

        sphere_center = result["sphere"]["center"]
        gaze_vector = result["circle_3d"]["normal"]
        pupil_radius = result["circle_3d"]["radius"]

        status = ("Eyeball center : X: %.2fmm Y: %.2fmm Z: %.2fmm \n"
                  "Gaze vector:  X: %.2f Y: %.2f Z: %.2f\n"
                  "Pupil Diameter: %.2fmm\n"
                  "No. of supporting pupil observations: %i\n" % (
                      sphere_center[0],
                      sphere_center[1],
                      sphere_center[2],
                      gaze_vector[0],
                      gaze_vector[1],
                      gaze_vector[2],
                      pupil_radius * 2,
                      len(result["debug_info"]["Dierkes_lines"]),
                  ))

        self.glfont.push_state()
        self.glfont.set_color_float((0, 0, 0, 1))
        self.glfont.draw_multi_line_text(7, 20, status)
        self.glfont.pop_state()

    def draw_residuals(self, result):

        glMatrixMode(GL_PROJECTION)
        glPushMatrix()
        glLoadIdentity()
        glOrtho(0, 1, 0, 1, -1, 1)  # gl coord convention

        glMatrixMode(GL_MODELVIEW)
        glPushMatrix()
        glLoadIdentity()

        glTranslatef(0.01, 0.01, 0)

        glScale(1.5, 1.5, 1)

        glColor4f(1, 1, 1, 0.3)
        glBegin(GL_QUADS)
        glVertex3f(0, 0, 0)
        glVertex3f(0, 0.15, 0)
        glVertex3f(0.15, 0.15, 0)
        glVertex3f(0.15, 0, 0)
        glEnd()

        glTranslatef(0.01, 0.01, 0)

        glScale(0.13, 0.13, 1)

        vertices = [[0, 0], [0, 1]]
        glutils.draw_polyline(vertices,
                              thickness=2,
                              color=RGBA(1.0, 1.0, 1.0, 0.9))
        vertices = [[0, 0], [1, 0]]
        glutils.draw_polyline(vertices,
                              thickness=2,
                              color=RGBA(1.0, 1.0, 1.0, 0.9))

        glScale(1, 0.33, 1)

        vertices = [[0, 1], [1, 1]]
        glutils.draw_polyline(vertices,
                              thickness=1,
                              color=RGBA(1.0, 1.0, 1.0, 0.9))
        vertices = [[0, 2], [1, 2]]
        glutils.draw_polyline(vertices,
                              thickness=1,
                              color=RGBA(1.0, 1.0, 1.0, 0.9))
        vertices = [[0, 3], [1, 3]]
        glutils.draw_polyline(vertices,
                              thickness=1,
                              color=RGBA(1.0, 1.0, 1.0, 0.9))

        try:

            vertices = list(
                zip(
                    np.clip((np.asarray(result["debug_info"]["angles"]) - 10) /
                            40.0, 0, 1),
                    np.clip(
                        np.log10(np.array(result["debug_info"]["residuals"])) +
                        2,
                        0.1,
                        3.9,
                    ),
                ))

            alpha = 0.2 / (len(result["debug_info"]["angles"])**0.2)
            size = 2 + 10 / (len(result["debug_info"]["angles"])**0.1)

            glutils.draw_points(vertices,
                                size=size,
                                color=RGBA(255 / 255, 165 / 255, 0, alpha))

        except:

            pass

        glPopMatrix()
        glMatrixMode(GL_PROJECTION)
        glPopMatrix()

    def draw_Dierkes_lines(self, result):

        glPushMatrix()
        glMatrixMode(GL_MODELVIEW)
        glColor4f(1.0, 0.0, 0.0, 0.1)
        glLineWidth(1.0)
        for line in result["debug_info"]["Dierkes_lines"][::4]:
            glBegin(GL_LINES)
            glVertex3f(line[0], line[1], line[2])
            glVertex3f(line[3], line[4], line[5])
            glEnd()
        glPopMatrix()

    def update_window(self, g_pool, result):

        if not result:
            return

        if not self.window:
            return

        self.begin_update_window()
        self.image_width, self.image_height = g_pool.capture.frame_size
        self.clear_gl_screen()
        self.trackball.push()

        glLoadMatrixf(self.get_image_space_matrix(15))

        g_pool.image_tex.draw(
            quad=(
                (0, self.image_height),
                (self.image_width, self.image_height),
                (self.image_width, 0),
                (0, 0),
            ),
            alpha=1.0,
        )

        glLoadMatrixf(self.get_adjusted_pixel_space_matrix(15))
        self.draw_frustum(self.image_width, self.image_height,
                          self.focal_length)
        glLoadMatrixf(self.get_anthropomorphic_matrix())

        self.eye.pose = self.get_anthropomorphic_matrix()

        self.draw_coordinate_system(4)
        self.draw_eye(result)
        self.draw_Dierkes_lines(result)
        self.trackball.pop()

        self.draw_debug_info(result)
        self.draw_residuals(result)

        self.end_update_window()

        return True

    ############ WINDOW CALLBACKS ###############

    def on_resize(self, window, w, h):
        Visualizer.on_resize(self, window, w, h)
        self.trackball.set_window_size(w, h)

    def on_window_char(self, window, char):
        if char == ord("r"):
            self.trackball.distance = [0, 0, -0.1]
            self.trackball.pitch = 0
            self.trackball.roll = 0

    def on_scroll(self, window, x, y):
        self.trackball.zoom_to(y)