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)
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
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)
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
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
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)