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)