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
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 = 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
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)
def open_window(self):
if not self._window:
if self.fullscreen:
monitor = glfwGetMonitors()[self.monitor_idx]
mode = glfwGetVideoMode(monitor)
height, width = mode[0], mode[1]
else:
monitor = None
height, width = (
640,
int(640.0 / (self.real_world_size["x"] /
self.real_world_size["y"])),
) # open with same aspect ratio as surface
self._window = glfwCreateWindow(
height,
width,
"Reference Surface: " + self.name,
monitor=monitor,
share=glfwGetCurrentContext(),
)
if not self.fullscreen:
glfwSetWindowPos(
self._window,
self.window_position_default[0],
self.window_position_default[1],
)
self.trackball = Trackball()
self.input = {"down": False, "mouse": (0, 0)}
# Register callbacks
glfwSetFramebufferSizeCallback(self._window, self.on_resize)
glfwSetKeyCallback(self._window, self.on_window_key)
glfwSetWindowCloseCallback(self._window, self.on_close)
glfwSetMouseButtonCallback(self._window,
self.on_window_mouse_button)
glfwSetCursorPosCallback(self._window, self.on_pos)
glfwSetScrollCallback(self._window, self.on_scroll)
self.on_resize(self._window, *glfwGetFramebufferSize(self._window))
# gl_state settings
active_window = glfwGetCurrentContext()
glfwMakeContextCurrent(self._window)
basic_gl_setup()
make_coord_system_norm_based()
# refresh speed settings
glfwSwapInterval(0)
glfwMakeContextCurrent(active_window)
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
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)
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
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)
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
def open_window(self):
if not self._window:
if self.fullscreen:
monitor = glfwGetMonitors()[self.monitor_idx]
mode = glfwGetVideoMode(monitor)
height, width = mode[0], mode[1]
else:
monitor = None
height, width = (
640,
int(640.0 / (self.real_world_size["x"] / self.real_world_size["y"])),
) # open with same aspect ratio as surface
self._window = glfwCreateWindow(
height, width, "Reference Surface: " + self.name, monitor=monitor, share=glfwGetCurrentContext()
)
if not self.fullscreen:
glfwSetWindowPos(self._window, 200, 0)
self.trackball = Trackball()
self.input = {"down": False, "mouse": (0, 0)}
# Register callbacks
glfwSetFramebufferSizeCallback(self._window, self.on_resize)
glfwSetKeyCallback(self._window, self.on_key)
glfwSetWindowCloseCallback(self._window, self.on_close)
glfwSetMouseButtonCallback(self._window, self.on_button)
glfwSetCursorPosCallback(self._window, self.on_pos)
glfwSetScrollCallback(self._window, self.on_scroll)
self.on_resize(self._window, *glfwGetFramebufferSize(self._window))
# gl_state settings
active_window = glfwGetCurrentContext()
glfwMakeContextCurrent(self._window)
basic_gl_setup()
make_coord_system_norm_based()
# refresh speed settings
glfwSwapInterval(0)
glfwMakeContextCurrent(active_window)
class Reference_Surface(object):
"""docstring for Reference Surface
The surface coodinate system is 0-1.
Origin is the bottom left corner, (1,1) is the top right
The first scalar in the pos vector is width we call this 'u'.
The second is height we call this 'v'.
The surface is thus defined by 4 vertecies:
Our convention is this order: (0,0),(1,0),(1,1),(0,1)
The surface is supported by a set of n>=1 Markers:
Each marker has an id, you can not not have markers with the same id twice.
Each marker has 4 verts (order is the same as the surface verts)
Each maker vertex has a uv coord that places it on the surface
When we find the surface in locate() we use the correspondence
of uv and screen coords of all 4 verts of all detected markers to get the
surface to screen homography.
This allows us to get homographies for partially visible surfaces,
all we need are 2 visible markers. (We could get away with just
one marker but in pracise this is to noisy.)
The more markers we find the more accurate the homography.
"""
def __init__(self,name="unnamed",saved_definition=None):
self.name = name
self.markers = {}
self.detected_markers = 0
self.defined = False
self.build_up_status = 0
self.required_build_up = 90.
self.detected = False
self.m_to_screen = None
self.m_from_screen = None
self.camera_pose_3d = None
self.crop_region = None
self.uid = str(time())
self.real_world_size = {'x':1.,'y':1.}
###window and gui vars
self._window = None
self.fullscreen = False
self.window_should_open = False
self.window_should_close = False
self.gaze_on_srf = [] # points on surface for realtime feedback display
self.glfont = fontstash.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))
if saved_definition is not None:
self.load_from_dict(saved_definition)
def save_to_dict(self):
"""
save all markers and name of this surface to a dict.
"""
markers = dict([(m_id,m.uv_coords) for m_id,m in self.markers.iteritems()])
return {'name':self.name,'uid':self.uid,'markers':markers,'real_world_size':self.real_world_size}
def load_from_dict(self,d):
"""
load all markers of this surface to a dict.
"""
self.name = d['name']
self.uid = d['uid']
self.real_world_size = d.get('real_world_size',{'x':1.,'y':1.})
marker_dict = d['markers']
for m_id,uv_coords in marker_dict.iteritems():
self.markers[m_id] = Support_Marker(m_id)
self.markers[m_id].load_uv_coords(uv_coords)
#flag this surface as fully defined
self.defined = True
self.build_up_status = self.required_build_up
def build_correspondance(self, visible_markers):
"""
- use all visible markers
- fit a convex quadrangle around it
- use quadrangle verts to establish perpective transform
- map all markers into surface space
- build up list of found markers and their uv coords
"""
if visible_markers == []:
self.m_to_screen = None
self.m_from_screen = None
self.detected = False
return
all_verts = np.array([[m['verts_norm'] for m in visible_markers]])
all_verts.shape = (-1,1,2) # [vert,vert,vert,vert,vert...] with vert = [[r,c]]
hull = cv2.convexHull(all_verts,clockwise=False)
#simplify until we have excatly 4 verts
if hull.shape[0]>4:
new_hull = cv2.approxPolyDP(hull,epsilon=1,closed=True)
if new_hull.shape[0]>=4:
hull = new_hull
if hull.shape[0]>4:
curvature = abs(GetAnglesPolyline(hull,closed=True))
most_acute_4_threshold = sorted(curvature)[3]
hull = hull[curvature<=most_acute_4_threshold]
#now we need to roll the hull verts until we have the right orientation:
distance_to_origin = np.sqrt(hull[:,:,0]**2+hull[:,:,1]**2)
top_left_idx = np.argmin(distance_to_origin)
hull = np.roll(hull,-top_left_idx,axis=0)
#based on these 4 verts we calculate the transformations into a 0,0 1,1 square space
self.m_to_screen = m_verts_to_screen(hull)
self.m_from_screen = m_verts_from_screen(hull)
self.detected = True
# map the markers vertices in to the surface space (one can think of these as texture coordinates u,v)
marker_uv_coords = cv2.perspectiveTransform(all_verts,self.m_from_screen)
marker_uv_coords.shape = (-1,4,1,2) #[marker,marker...] marker = [ [[r,c]],[[r,c]] ]
# build up a dict of discovered markers. Each with a history of uv coordinates
for m,uv in zip (visible_markers,marker_uv_coords):
try:
self.markers[m['id']].add_uv_coords(uv)
except KeyError:
self.markers[m['id']] = Support_Marker(m['id'])
self.markers[m['id']].add_uv_coords(uv)
#average collection of uv correspondences accros detected markers
self.build_up_status = sum([len(m.collected_uv_coords) for m in self.markers.values()])/float(len(self.markers))
if self.build_up_status >= self.required_build_up:
self.finalize_correnspondance()
self.defined = True
def finalize_correnspondance(self):
"""
- prune markers that have been visible in less than x percent of frames.
- of those markers select a good subset of uv coords and compute mean.
- this mean value will be used from now on to estable surface transform
"""
persistent_markers = {}
for k,m in self.markers.iteritems():
if len(m.collected_uv_coords)>self.required_build_up*.5:
persistent_markers[k] = m
self.markers = persistent_markers
for m in self.markers.values():
m.compute_robust_mean()
def locate(self, visible_markers, locate_3d=False, camera_intrinsics = None):
"""
- find overlapping set of surface markers and visible_markers
- compute homography (and inverse) based on this subset
"""
if not self.defined:
self.build_correspondance(visible_markers)
else:
marker_by_id = dict([(m['id'],m) for m in visible_markers])
visible_ids = set(marker_by_id.keys())
requested_ids = set(self.markers.keys())
overlap = visible_ids & requested_ids
self.detected_markers = len(overlap)
if len(overlap)>=min(1,len(requested_ids)):
self.detected = True
yx = np.array( [marker_by_id[i]['verts_norm'] for i in overlap] )
uv = np.array( [self.markers[i].uv_coords for i in overlap] )
yx.shape=(-1,1,2)
uv.shape=(-1,1,2)
#print 'uv',uv
#print 'yx',yx
self.m_to_screen,mask = cv2.findHomography(uv,yx)
self.m_from_screen = np.linalg.inv(self.m_to_screen)
#self.m_from_screen,mask = cv2.findHomography(yx,uv)
if locate_3d:
K,dist_coef,img_size = camera_intrinsics
###marker support pose estiamtion:
# denormalize image reference points to pixel space
yx.shape = -1,2
yx *= img_size
yx.shape = -1, 1, 2
print 'yx', yx
print '----------'
# scale normalized object points to world space units (think m,cm,mm)
uv.shape = -1,2
uv *= [self.real_world_size['x'], self.real_world_size['y']]
# convert object points to lie on z==0 plane in 3d space
uv3d = np.zeros((uv.shape[0], uv.shape[1]+1))
uv3d[:,:-1] = uv
# compute pose of object relative to camera center
self.is3dPoseAvailable, rot3d_cam_to_object, translate3d_cam_to_object = cv2.solvePnP(uv3d, yx, K, dist_coef,flags=cv2.CV_EPNP)
# not verifed, potentially usefull info: http://stackoverflow.com/questions/17423302/opencv-solvepnp-tvec-units-and-axes-directions
###marker posed estimation from virtually projected points.
# object_pts = np.array([[[0,0],[0,1],[1,1],[1,0]]],dtype=np.float32)
# projected_pts = cv2.perspectiveTransform(object_pts,self.m_to_screen)
# projected_pts.shape = -1,2
# projected_pts *= img_size
# projected_pts.shape = -1, 1, 2
# # scale object points to world space units (think m,cm,mm)
# object_pts.shape = -1,2
# object_pts *= self.real_world_size
# # convert object points to lie on z==0 plane in 3d space
# object_pts_3d = np.zeros((4,3))
# object_pts_3d[:,:-1] = object_pts
# self.is3dPoseAvailable, rot3d_cam_to_object, translate3d_cam_to_object = cv2.solvePnP(object_pts_3d, projected_pts, K, dist_coef,flags=cv2.CV_EPNP)
# transformation from Camera Optical Center:
# first: translate from Camera center to object origin.
# second: rotate x,y,z
# coordinate system is x,y,z where z goes out from the camera into the viewed volume.
# print rot3d_cam_to_object[0],rot3d_cam_to_object[1],rot3d_cam_to_object[2], translate3d_cam_to_object[0],translate3d_cam_to_object[1],translate3d_cam_to_object[2]
#turn translation vectors into 3x3 rot mat.
rot3d_cam_to_object_mat, _ = cv2.Rodrigues(rot3d_cam_to_object)
#to get the transformation from object to camera we need to reverse rotation and translation
translate3d_object_to_cam = - translate3d_cam_to_object
# rotation matrix inverse == transpose
rot3d_object_to_cam_mat = rot3d_cam_to_object_mat.T
# we assume that the volume of the object grows out of the marker surface and not into it. We thus have to flip the z-Axis:
flip_z_axix_hm = np.eye(4, dtype=np.float32)
flip_z_axix_hm[2,2] = -1
# create a homogenous tranformation matrix from the rotation mat
rot3d_object_to_cam_hm = np.eye(4, dtype=np.float32)
rot3d_object_to_cam_hm[:-1,:-1] = rot3d_object_to_cam_mat
# create a homogenous tranformation matrix from the translation vect
translate3d_object_to_cam_hm = np.eye(4, dtype=np.float32)
translate3d_object_to_cam_hm[:-1, -1] = translate3d_object_to_cam.reshape(3)
# combine all tranformations into of matrix that decribes the move from object origin and orientation to camera origin and orientation
tranform3d_object_to_cam = np.matrix(flip_z_axix_hm) * np.matrix(rot3d_object_to_cam_hm) * np.matrix(translate3d_object_to_cam_hm)
self.camera_pose_3d = tranform3d_object_to_cam
else:
self.is3dPoseAvailable = False
else:
self.detected = False
self.is3dPoseAvailable = False
self.m_from_screen = None
self.m_to_screen = None
def img_to_ref_surface(self,pos):
if self.m_from_screen is not None:
#convenience lines to allow 'simple' vectors (x,y) to be used
shape = pos.shape
pos.shape = (-1,1,2)
new_pos = cv2.perspectiveTransform(pos,self.m_from_screen )
new_pos.shape = shape
return new_pos
else:
return None
def ref_surface_to_img(self,pos):
if self.m_to_screen is not None:
#convenience lines to allow 'simple' vectors (x,y) to be used
shape = pos.shape
pos.shape = (-1,1,2)
new_pos = cv2.perspectiveTransform(pos,self.m_to_screen )
new_pos.shape = shape
return new_pos
else:
return None
def move_vertex(self,vert_idx,new_pos):
"""
this fn is used to manipulate the surface boundary (coordinate system)
new_pos is in uv-space coords
if we move one vertex of the surface we need to find
the tranformation from old quadrangle to new quardangle
and apply that transformation to our marker uv-coords
"""
before = np.array(((0,0),(1,0),(1,1),(0,1)),dtype=np.float32)
after = before.copy()
after[vert_idx] = new_pos
transform = cv2.getPerspectiveTransform(after,before)
for m in self.markers.values():
m.uv_coords = cv2.perspectiveTransform(m.uv_coords,transform)
def marker_status(self):
return "%s %s/%s" %(self.name,self.detected_markers,len(self.markers))
def gl_draw_frame(self,img_size):
"""
draw surface and markers
"""
if self.detected:
frame = np.array([[[0,0],[1,0],[1,1],[0,1],[0,0]]],dtype=np.float32)
hat = np.array([[[.3,.7],[.7,.7],[.5,.9],[.3,.7]]],dtype=np.float32)
hat = cv2.perspectiveTransform(hat,self.m_to_screen)
frame = cv2.perspectiveTransform(frame,self.m_to_screen)
alpha = min(1,self.build_up_status/self.required_build_up)
draw_polyline_norm(frame.reshape((5,2)),1,RGBA(1.0,0.2,0.6,alpha))
draw_polyline_norm(hat.reshape((4,2)),1,RGBA(1.0,0.2,0.6,alpha))
#grid = frame.reshape((5,2))
# self.crop_region = np.array([
# [grid[0][0] * img_size[1], grid[0][1] * img_size[0]],
# [grid[1][0] * img_size[1], grid[1][1] * img_size[0]],
# [grid[2][0] * img_size[1], grid[2][1] * img_size[0]],
# [grid[3][0] * img_size[1], grid[3][1] * img_size[0]]],
# dtype=np.float32)
draw_points_norm(frame.reshape((5,-1))[0:1])
text_anchor = frame.reshape((5,-1))[2]
text_anchor[1] = 1-text_anchor[1]
text_anchor *=img_size[1],img_size[0]
self.glfont.draw_text(text_anchor[0],text_anchor[1],self.marker_status())
def gl_draw_corners(self):
"""
draw surface and markers
"""
if self.detected:
frame = np.array([[[0,0],[1,0],[1,1],[0,1]]],dtype=np.float32)
frame = cv2.perspectiveTransform(frame,self.m_to_screen)
draw_points_norm(frame.reshape((4,2)),15,RGBA(1.0,0.2,0.6,.5))
#### fns to draw surface in separate window
def gl_display_in_window(self,world_tex_id):
"""
here we map a selected surface onto a seperate window.
"""
if self._window and self.detected:
active_window = glfwGetCurrentContext()
glfwMakeContextCurrent(self._window)
clear_gl_screen()
# cv uses 3x3 gl uses 4x4 tranformation matricies
m = cvmat_to_glmat(self.m_from_screen)
glMatrixMode(GL_PROJECTION)
glPushMatrix()
glLoadIdentity()
glOrtho(0, 1, 0, 1,-1,1) # gl coord convention
glMatrixMode(GL_MODELVIEW)
glPushMatrix()
#apply m to our quad - this will stretch the quad such that the ref suface will span the window extends
glLoadMatrixf(m)
draw_named_texture(world_tex_id)
glMatrixMode(GL_PROJECTION)
glPopMatrix()
glMatrixMode(GL_MODELVIEW)
glPopMatrix()
# now lets get recent pupil positions on this surface:
draw_points_norm(self.gaze_on_srf,color=RGBA(0.,8.,.5,.8), size=80)
glfwSwapBuffers(self._window)
glfwMakeContextCurrent(active_window)
if self.window_should_close:
self.close_window()
#### fns to draw surface in separate window
def gl_display_in_window_3d(self,world_tex_id,camera_intrinsics):
"""
here we map a selected surface onto a seperate window.
"""
K,dist_coef,img_size = camera_intrinsics
if self._window and self.detected:
active_window = glfwGetCurrentContext()
glfwMakeContextCurrent(self._window)
glClearColor(.8,.8,.8,1.)
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
glClearDepth(1.0)
glDepthFunc(GL_LESS)
glEnable(GL_DEPTH_TEST)
self.trackball.push()
glMatrixMode(GL_MODELVIEW)
draw_coordinate_system(l=self.real_world_size['x'])
glPushMatrix()
glScalef(self.real_world_size['x'],self.real_world_size['y'],1)
draw_polyline([[0,0],[0,1],[1,1],[1,0]],color = RGBA(.5,.3,.1,.5),thickness=3)
glPopMatrix()
# Draw the world window as projected onto the plane using the homography mapping
glPushMatrix()
glScalef(self.real_world_size['x'], self.real_world_size['y'], 1)
# cv uses 3x3 gl uses 4x4 tranformation matricies
m = cvmat_to_glmat(self.m_from_screen)
glMultMatrixf(m)
glTranslatef(0,0,-.01)
draw_named_texture(world_tex_id)
draw_polyline([[0,0],[0,1],[1,1],[1,0]],color = RGBA(.5,.3,.6,.5),thickness=3)
glPopMatrix()
# Draw the camera frustum and origin using the 3d tranformation obtained from solvepnp
glPushMatrix()
glMultMatrixf(self.camera_pose_3d.T.flatten())
draw_frustum(self.img_size, K, 150)
glLineWidth(1)
draw_frustum(self.img_size, K, .1)
draw_coordinate_system(l=5)
glPopMatrix()
self.trackball.pop()
glfwSwapBuffers(self._window)
glfwMakeContextCurrent(active_window)
def open_window(self):
if not self._window:
if self.fullscreen:
monitor = glfwGetMonitors()[self.monitor_idx]
mode = glfwGetVideoMode(monitor)
height,width= mode[0],mode[1]
else:
monitor = None
height,width= 640,int(640./(self.real_world_size['x']/self.real_world_size['y'])) #open with same aspect ratio as surface
self._window = glfwCreateWindow(height, width, "Reference Surface: " + self.name, monitor=monitor, share=glfwGetCurrentContext())
if not self.fullscreen:
glfwSetWindowPos(self._window,200,0)
self.trackball = Trackball()
self.input = {'down':False, 'mouse':(0,0)}
#Register callbacks
glfwSetFramebufferSizeCallback(self._window,self.on_resize)
glfwSetKeyCallback(self._window,self.on_key)
glfwSetWindowCloseCallback(self._window,self.on_close)
glfwSetMouseButtonCallback(self._window,self.on_button)
glfwSetCursorPosCallback(self._window,self.on_pos)
glfwSetScrollCallback(self._window,self.on_scroll)
self.on_resize(self._window,*glfwGetFramebufferSize(self._window))
# gl_state settings
active_window = glfwGetCurrentContext()
glfwMakeContextCurrent(self._window)
basic_gl_setup()
make_coord_system_norm_based()
# refresh speed settings
glfwSwapInterval(0)
glfwMakeContextCurrent(active_window)
def close_window(self):
if self._window:
glfwDestroyWindow(self._window)
self._window = None
self.window_should_close = False
def open_close_window(self):
if self._window:
self.close_window()
else:
self.open_window()
# window calbacks
def on_resize(self,window,w, h):
self.trackball.set_window_size(w,h)
active_window = glfwGetCurrentContext()
glfwMakeContextCurrent(window)
adjust_gl_view(w,h)
glfwMakeContextCurrent(active_window)
def on_key(self,window, key, scancode, action, mods):
if action == GLFW_PRESS:
if key == GLFW_KEY_ESCAPE:
self.on_close()
def on_close(self,window=None):
self.window_should_close = True
def on_button(self,window,button, action, mods):
if action == GLFW_PRESS:
self.input['down'] = True
self.input['mouse'] = glfwGetCursorPos(window)
if action == GLFW_RELEASE:
self.input['down'] = False
def on_pos(self,window,x, y):
if self.input['down']:
old_x,old_y = self.input['mouse']
self.trackball.drag_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 cleanup(self):
if self._window:
self.close_window()
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 Reference_Surface(object):
"""docstring for Reference Surface
The surface coodinate system is 0-1.
Origin is the bottom left corner, (1,1) is the top right
The first scalar in the pos vector is width we call this 'u'.
The second is height we call this 'v'.
The surface is thus defined by 4 vertecies:
Our convention is this order: (0,0),(1,0),(1,1),(0,1)
The surface is supported by a set of n>=1 Markers:
Each marker has an id, you can not not have markers with the same id twice.
Each marker has 4 verts (order is the same as the surface verts)
Each maker vertex has a uv coord that places it on the surface
When we find the surface in locate() we use the correspondence
of uv and screen coords of all 4 verts of all detected markers to get the
surface to screen homography.
This allows us to get homographies for partially visible surfaces,
all we need are 2 visible markers. (We could get away with just
one marker but in pracise this is to noisy.)
The more markers we find the more accurate the homography.
"""
def __init__(self, g_pool, name="unnamed", saved_definition=None):
self.g_pool = g_pool
self.name = name
self.markers = {}
self.detected_markers = 0
self.defined = False
self.build_up_status = 0
self.required_build_up = 90.
self.detected = False
self.m_to_screen = None
self.m_from_screen = None
self.camera_pose_3d = None
self.use_distortion = True
self.uid = str(time())
self.real_world_size = {'x': 1., 'y': 1.}
self.heatmap = np.ones(0)
self.heatmap_detail = .2
self.heatmap_texture = Named_Texture()
self.gaze_history = deque()
self.gaze_history_length = 1.0 # unit: seconds
# window and gui vars
self._window = None
self.fullscreen = False
self.window_should_open = False
self.window_should_close = False
self.gaze_on_srf = [
] # points on surface for realtime feedback display
self.glfont = fontstash.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.old_corners_robust = None
if saved_definition is not None:
self.load_from_dict(saved_definition)
# UI Platform tweaks
if system() == 'Linux':
self.window_position_default = (0, 0)
elif system() == 'Windows':
self.window_position_default = (8, 31)
else:
self.window_position_default = (0, 0)
def save_to_dict(self):
"""
save all markers and name of this surface to a dict.
"""
markers = dict([(m_id, m.uv_coords.tolist())
for m_id, m in self.markers.items()])
return {
'name': self.name,
'uid': self.uid,
'markers': markers,
'real_world_size': self.real_world_size,
'gaze_history_length': self.gaze_history_length
}
def load_from_dict(self, d):
"""
load all markers of this surface to a dict.
"""
self.name = d['name']
self.uid = d['uid']
self.gaze_history_length = d.get('gaze_history_length',
self.gaze_history_length)
self.real_world_size = d.get('real_world_size', {'x': 1., 'y': 1.})
marker_dict = d['markers']
for m_id, uv_coords in marker_dict.items():
self.markers[m_id] = Support_Marker(m_id)
self.markers[m_id].load_uv_coords(np.asarray(uv_coords))
# flag this surface as fully defined
self.defined = True
self.build_up_status = self.required_build_up
def build_correspondence(self, visible_markers, min_marker_perimeter,
min_id_confidence):
"""
- use all visible markers
- fit a convex quadrangle around it
- use quadrangle verts to establish perpective transform
- map all markers into surface space
- build up list of found markers and their uv coords
"""
usable_markers = [
m for m in visible_markers
if m['perimeter'] >= min_marker_perimeter
]
all_verts = [m['verts'] for m in usable_markers]
if not all_verts:
return
all_verts = np.array(all_verts, dtype=np.float32)
all_verts.shape = (
-1, 1, 2) # [vert,vert,vert,vert,vert...] with vert = [[r,c]]
# all_verts_undistorted_normalized centered in img center flipped in y and range [-1,1]
all_verts_undistorted_normalized = self.g_pool.capture.intrinsics.undistortPoints(
all_verts, use_distortion=self.use_distortion)
hull = cv2.convexHull(all_verts_undistorted_normalized.astype(
np.float32),
clockwise=False)
#simplify until we have excatly 4 verts
if hull.shape[0] > 4:
new_hull = cv2.approxPolyDP(hull, epsilon=1, closed=True)
if new_hull.shape[0] >= 4:
hull = new_hull
if hull.shape[0] > 4:
curvature = abs(GetAnglesPolyline(hull, closed=True))
most_acute_4_threshold = sorted(curvature)[3]
hull = hull[curvature <= most_acute_4_threshold]
# all_verts_undistorted_normalized space is flipped in y.
# we need to change the order of the hull vertecies
hull = hull[[1, 0, 3, 2], :, :]
# now we need to roll the hull verts until we have the right orientation:
# all_verts_undistorted_normalized space has its origin at the image center.
# adding 1 to the coordinates puts the origin at the top left.
distance_to_top_left = np.sqrt((hull[:, :, 0] + 1)**2 +
(hull[:, :, 1] + 1)**2)
bot_left_idx = np.argmin(distance_to_top_left) + 1
hull = np.roll(hull, -bot_left_idx, axis=0)
#based on these 4 verts we calculate the transformations into a 0,0 1,1 square space
m_from_undistored_norm_space = m_verts_from_screen(hull)
self.detected = True
# map the markers vertices into the surface space (one can think of these as texture coordinates u,v)
marker_uv_coords = cv2.perspectiveTransform(
all_verts_undistorted_normalized, m_from_undistored_norm_space)
marker_uv_coords.shape = (
-1, 4, 1, 2) #[marker,marker...] marker = [ [[r,c]],[[r,c]] ]
# build up a dict of discovered markers. Each with a history of uv coordinates
for m, uv in zip(usable_markers, marker_uv_coords):
try:
self.markers[m['id']].add_uv_coords(uv)
except KeyError:
self.markers[m['id']] = Support_Marker(m['id'])
self.markers[m['id']].add_uv_coords(uv)
#average collection of uv correspondences across detected markers
self.build_up_status = sum(
[len(m.collected_uv_coords)
for m in self.markers.values()]) / float(len(self.markers))
if self.build_up_status >= self.required_build_up:
self.finalize_correspondence()
def finalize_correspondence(self):
"""
- prune markers that have been visible in less than x percent of frames.
- of those markers select a good subset of uv coords and compute mean.
- this mean value will be used from now on to estable surface transform
"""
persistent_markers = {}
for k, m in self.markers.items():
if len(m.collected_uv_coords) > self.required_build_up * .5:
persistent_markers[k] = m
self.markers = persistent_markers
for m in self.markers.values():
m.compute_robust_mean()
self.defined = True
if hasattr(self, 'on_finish_define'):
self.on_finish_define()
del self.on_finish_define
def update_gaze_history(self):
self.gaze_history.extend(self.gaze_on_srf)
try: # use newest gaze point to determine age threshold
age_threshold = self.gaze_history[-1][
'timestamp'] - self.gaze_history_length
while self.gaze_history[1]['timestamp'] < age_threshold:
self.gaze_history.popleft() # remove outdated gaze points
except IndexError:
pass
def generate_heatmap(self):
data = [
gp['norm_pos'] for gp in self.gaze_history
if gp['confidence'] > 0.6
]
self._generate_heatmap(data)
def _generate_heatmap(self, data):
if not data:
return
grid = int(self.real_world_size['y']), int(self.real_world_size['x'])
xvals, yvals = zip(*((x, 1. - y) for x, y in data))
hist, *edges = np.histogram2d(yvals,
xvals,
bins=grid,
range=[[0, 1.], [0, 1.]],
normed=False)
filter_h = int(self.heatmap_detail * grid[0]) // 2 * 2 + 1
filter_w = int(self.heatmap_detail * grid[1]) // 2 * 2 + 1
hist = cv2.GaussianBlur(hist, (filter_h, filter_w), 0)
hist_max = hist.max()
hist *= (255. / hist_max) if hist_max else 0.
hist = hist.astype(np.uint8)
c_map = cv2.applyColorMap(hist, cv2.COLORMAP_JET)
# reuse allocated memory if possible
if self.heatmap.shape != (*grid, 4):
self.heatmap = np.ones((*grid, 4), dtype=np.uint8)
self.heatmap[:, :, 3] = 125
self.heatmap[:, :, :3] = c_map
self.heatmap_texture.update_from_ndarray(self.heatmap)
def gl_display_heatmap(self):
if self.detected:
m = cvmat_to_glmat(self.m_to_screen)
glMatrixMode(GL_PROJECTION)
glPushMatrix()
glLoadIdentity()
glOrtho(0, 1, 0, 1, -1, 1) # gl coord convention
glMatrixMode(GL_MODELVIEW)
glPushMatrix()
# apply m to our quad - this will stretch the quad such that the ref suface will span the window extends
glLoadMatrixf(m)
self.heatmap_texture.draw()
glMatrixMode(GL_PROJECTION)
glPopMatrix()
glMatrixMode(GL_MODELVIEW)
glPopMatrix()
def locate(
self,
visible_markers,
min_marker_perimeter,
min_id_confidence,
locate_3d=False,
):
"""
- find overlapping set of surface markers and visible_markers
- compute homography (and inverse) based on this subset
"""
if not self.defined:
self.build_correspondence(visible_markers, min_marker_perimeter,
min_id_confidence)
res = self._get_location(visible_markers, min_marker_perimeter,
min_id_confidence, locate_3d)
self.detected = res['detected']
self.detected_markers = res['detected_markers']
self.m_to_screen = res['m_to_screen']
self.m_from_screen = res['m_from_screen']
self.camera_pose_3d = res['camera_pose_3d']
def _get_location(self,
visible_markers,
min_marker_perimeter,
min_id_confidence,
locate_3d=False):
filtered_markers = [
m for m in visible_markers
if m['perimeter'] >= min_marker_perimeter
and m['id_confidence'] > min_id_confidence
]
marker_by_id = {}
#if an id shows twice we use the bigger marker (usually this is a screen camera echo artifact.)
for m in filtered_markers:
if m["id"] in marker_by_id and m["perimeter"] < marker_by_id[
m['id']]['perimeter']:
pass
else:
marker_by_id[m["id"]] = m
visible_ids = set(marker_by_id.keys())
requested_ids = set(self.markers.keys())
overlap = visible_ids & requested_ids
# need at least two markers per surface when the surface is more that 1 marker.
if overlap and len(overlap) >= min(2, len(requested_ids)):
detected = True
xy = np.array([marker_by_id[i]['verts'] for i in overlap])
uv = np.array([self.markers[i].uv_coords for i in overlap])
uv.shape = (-1, 1, 2)
# our camera lens creates distortions we want to get a good 2d estimate despite that so we:
# compute the homography transform from marker into the undistored normalized image space
# (the line below is the same as what you find in methods.undistort_unproject_pts, except that we ommit the z corrd as it is always one.)
xy_undistorted_normalized = self.g_pool.capture.intrinsics.undistortPoints(
xy.reshape(-1, 2), use_distortion=self.use_distortion)
m_to_undistored_norm_space, mask = cv2.findHomography(
uv,
xy_undistorted_normalized,
method=cv2.RANSAC,
ransacReprojThreshold=100)
if not mask.all():
detected = False
m_from_undistored_norm_space, mask = cv2.findHomography(
xy_undistorted_normalized, uv)
# project the corners of the surface to undistored space
corners_undistored_space = cv2.perspectiveTransform(
marker_corners_norm.reshape(-1, 1, 2),
m_to_undistored_norm_space)
# project and distort these points and normalize them
corners_redistorted = self.g_pool.capture.intrinsics.projectPoints(
cv2.convertPointsToHomogeneous(corners_undistored_space),
use_distortion=self.use_distortion)
corners_nulldistorted = self.g_pool.capture.intrinsics.projectPoints(
cv2.convertPointsToHomogeneous(corners_undistored_space),
use_distortion=self.use_distortion)
#normalize to pupil norm space
corners_redistorted.shape = -1, 2
corners_redistorted /= self.g_pool.capture.intrinsics.resolution
corners_redistorted[:, -1] = 1 - corners_redistorted[:, -1]
#normalize to pupil norm space
corners_nulldistorted.shape = -1, 2
corners_nulldistorted /= self.g_pool.capture.intrinsics.resolution
corners_nulldistorted[:, -1] = 1 - corners_nulldistorted[:, -1]
# maps for extreme lens distortions will behave irratically beyond the image bounds
# since our surfaces often extend beyond the screen we need to interpolate
# between a distored projection and undistored one.
# def ratio(val):
# centered_val = abs(.5 - val)
# # signed distance to img cennter .5 is imag bound
# # we look to interpolate between .7 and .9
# inter = max()
corners_robust = []
for nulldist, redist in zip(corners_nulldistorted,
corners_redistorted):
if -.4 < nulldist[0] < 1.4 and -.4 < nulldist[1] < 1.4:
corners_robust.append(redist)
else:
corners_robust.append(nulldist)
corners_robust = np.array(corners_robust)
if self.old_corners_robust is not None and np.mean(
np.abs(corners_robust - self.old_corners_robust)) < 0.02:
smooth_corners_robust = self.old_corners_robust
smooth_corners_robust += .5 * (corners_robust -
self.old_corners_robust)
corners_robust = smooth_corners_robust
self.old_corners_robust = smooth_corners_robust
else:
self.old_corners_robust = corners_robust
#compute a perspective thransform from from the marker norm space to the apparent image.
# The surface corners will be at the right points
# However the space between the corners may be distored due to distortions of the lens,
m_to_screen = m_verts_to_screen(corners_robust)
m_from_screen = m_verts_from_screen(corners_robust)
camera_pose_3d = None
if locate_3d:
# 3d marker support pose estimation:
# scale normalized object points to world space units (think m,cm,mm)
uv.shape = -1, 2
uv *= [self.real_world_size['x'], self.real_world_size['y']]
# convert object points to lie on z==0 plane in 3d space
uv3d = np.zeros((uv.shape[0], uv.shape[1] + 1))
uv3d[:, :-1] = uv
xy.shape = -1, 1, 2
# compute pose of object relative to camera center
is3dPoseAvailable, rot3d_cam_to_object, translate3d_cam_to_object = self.g_pool.capture.intrinsics.solvePnP(
uv3d, xy)
print("{} \t {} \t {}".format(translate3d_cam_to_object[0],
translate3d_cam_to_object[1],
translate3d_cam_to_object[2]))
if is3dPoseAvailable:
# not verifed, potentially usefull info: http://stackoverflow.com/questions/17423302/opencv-solvepnp-tvec-units-and-axes-directions
###marker posed estimation from virtually projected points.
# object_pts = np.array([[[0,0],[0,1],[1,1],[1,0]]],dtype=np.float32)
# projected_pts = cv2.perspectiveTransform(object_pts,self.m_to_screen)
# projected_pts.shape = -1,2
# projected_pts *= img_size
# projected_pts.shape = -1, 1, 2
# # scale object points to world space units (think m,cm,mm)
# object_pts.shape = -1,2
# object_pts *= self.real_world_size
# # convert object points to lie on z==0 plane in 3d space
# object_pts_3d = np.zeros((4,3))
# object_pts_3d[:,:-1] = object_pts
# self.is3dPoseAvailable, rot3d_cam_to_object, translate3d_cam_to_object = cv2.solvePnP(object_pts_3d, projected_pts, K, dist_coef,flags=cv2.CV_EPNP)
# transformation from Camera Optical Center:
# first: translate from Camera center to object origin.
# second: rotate x,y,z
# coordinate system is x,y,z where z goes out from the camera into the viewed volume.
# print rot3d_cam_to_object[0],rot3d_cam_to_object[1],rot3d_cam_to_object[2], translate3d_cam_to_object[0],translate3d_cam_to_object[1],translate3d_cam_to_object[2]
#turn translation vectors into 3x3 rot mat.
rot3d_cam_to_object_mat, _ = cv2.Rodrigues(
rot3d_cam_to_object)
#to get the transformation from object to camera we need to reverse rotation and translation
translate3d_object_to_cam = -translate3d_cam_to_object
# rotation matrix inverse == transpose
rot3d_object_to_cam_mat = rot3d_cam_to_object_mat.T
# we assume that the volume of the object grows out of the marker surface and not into it. We thus have to flip the z-Axis:
flip_z_axix_hm = np.eye(4, dtype=np.float32)
flip_z_axix_hm[2, 2] = -1
# create a homogenous tranformation matrix from the rotation mat
rot3d_object_to_cam_hm = np.eye(4, dtype=np.float32)
rot3d_object_to_cam_hm[:-1, :-1] = rot3d_object_to_cam_mat
# create a homogenous tranformation matrix from the translation vect
translate3d_object_to_cam_hm = np.eye(4, dtype=np.float32)
translate3d_object_to_cam_hm[:-1,
-1] = translate3d_object_to_cam.reshape(
3)
# combine all tranformations into transformation matrix that decribes the move from object origin and orientation to camera origin and orientation
tranform3d_object_to_cam = np.matrix(
flip_z_axix_hm) * np.matrix(
rot3d_object_to_cam_hm) * np.matrix(
translate3d_object_to_cam_hm)
camera_pose_3d = tranform3d_object_to_cam
if detected == False:
camera_pose_3d = None
m_from_screen = None
m_to_screen = None
m_from_undistored_norm_space = None
m_to_undistored_norm_space = None
else:
detected = False
camera_pose_3d = None
m_from_screen = None
m_to_screen = None
m_from_undistored_norm_space = None
m_to_undistored_norm_space = None
return {
'detected': detected,
'detected_markers': len(overlap),
'm_from_undistored_norm_space': m_from_undistored_norm_space,
'm_to_undistored_norm_space': m_to_undistored_norm_space,
'm_from_screen': m_from_screen,
'm_to_screen': m_to_screen,
'camera_pose_3d': camera_pose_3d
}
def img_to_ref_surface(self, pos):
#convenience lines to allow 'simple' vectors (x,y) to be used
shape = pos.shape
pos.shape = (-1, 1, 2)
new_pos = cv2.perspectiveTransform(pos, self.m_from_screen)
new_pos.shape = shape
return new_pos
def ref_surface_to_img(self, pos):
#convenience lines to allow 'simple' vectors (x,y) to be used
shape = pos.shape
pos.shape = (-1, 1, 2)
new_pos = cv2.perspectiveTransform(pos, self.m_to_screen)
new_pos.shape = shape
return new_pos
@staticmethod
def map_datum_to_surface(d, m_from_screen):
pos = np.array([d['norm_pos']]).reshape(1, 1, 2)
mapped_pos = cv2.perspectiveTransform(pos, m_from_screen)
mapped_pos.shape = (2)
on_srf = bool((0 <= mapped_pos[0] <= 1) and (0 <= mapped_pos[1] <= 1))
return {
'topic': d['topic'] + "_on_surface",
'norm_pos': (mapped_pos[0], mapped_pos[1]),
'confidence': d['confidence'],
'on_srf': on_srf,
'base_data': d,
'timestamp': d['timestamp']
}
def map_data_to_surface(self, data, m_from_screen):
return [self.map_datum_to_surface(d, m_from_screen) for d in data]
def move_vertex(self, vert_idx, new_pos):
"""
this fn is used to manipulate the surface boundary (coordinate system)
new_pos is in uv-space coords
if we move one vertex of the surface we need to find
the tranformation from old quadrangle to new quardangle
and apply that transformation to our marker uv-coords
"""
before = marker_corners_norm
after = before.copy()
after[vert_idx] = new_pos
transform = cv2.getPerspectiveTransform(after, before)
for m in self.markers.values():
m.uv_coords = cv2.perspectiveTransform(m.uv_coords, transform)
def add_marker(self, marker, visible_markers, min_marker_perimeter,
min_id_confidence):
'''
add marker to surface.
'''
res = self._get_location(visible_markers,
min_marker_perimeter,
min_id_confidence,
locate_3d=False)
if res['detected']:
support_marker = Support_Marker(marker['id'])
marker_verts = np.array(marker['verts'])
marker_verts.shape = (-1, 1, 2)
if self.use_distortion:
marker_verts_undistorted_normalized = self.g_pool.capture.intrinsics.undistortPoints(
marker_verts)
else:
marker_verts_undistorted_normalized = self.g_pool.capture.intrinsics.undistortPoints(
marker_verts)
marker_uv_coords = cv2.perspectiveTransform(
marker_verts_undistorted_normalized,
res['m_from_undistored_norm_space'])
support_marker.load_uv_coords(marker_uv_coords)
self.markers[marker['id']] = support_marker
def remove_marker(self, marker):
if len(self.markers) == 1:
logger.warning(
"Need at least one marker per surface. Will not remove this last marker."
)
return
self.markers.pop(marker['id'])
def marker_status(self):
return "{} {}/{}".format(self.name, self.detected_markers,
len(self.markers))
def get_mode_toggle(self, pos, img_shape):
if self.detected and self.defined:
x, y = pos
frame = np.array([[[0, 0], [1, 0], [1, 1], [0, 1], [0, 0]]],
dtype=np.float32)
frame = cv2.perspectiveTransform(frame, self.m_to_screen)
text_anchor = frame.reshape((5, -1))[2]
text_anchor[1] = 1 - text_anchor[1]
text_anchor *= img_shape[1], img_shape[0]
text_anchor = text_anchor[0], text_anchor[1] - 75
surface_edit_anchor = text_anchor[0], text_anchor[1] + 25
marker_edit_anchor = text_anchor[0], text_anchor[1] + 50
if np.sqrt((x - surface_edit_anchor[0])**2 +
(y - surface_edit_anchor[1])**2) < 15:
return 'surface_mode'
elif np.sqrt((x - marker_edit_anchor[0])**2 +
(y - marker_edit_anchor[1])**2) < 15:
return 'marker_mode'
else:
return None
else:
return None
def gl_draw_frame(self,
img_size,
color=(1.0, 0.2, 0.6, 1.0),
highlight=False,
surface_mode=False,
marker_mode=False):
"""
draw surface and markers
"""
if self.detected:
r, g, b, a = color
frame = np.array([[[0, 0], [1, 0], [1, 1], [0, 1], [0, 0]]],
dtype=np.float32)
hat = np.array([[[.3, .7], [.7, .7], [.5, .9], [.3, .7]]],
dtype=np.float32)
hat = cv2.perspectiveTransform(hat, self.m_to_screen)
frame = cv2.perspectiveTransform(frame, self.m_to_screen)
alpha = min(1, self.build_up_status / self.required_build_up)
if highlight:
draw_polyline_norm(frame.reshape((5, 2)),
1,
RGBA(r, g, b, a * .1),
line_type=GL_POLYGON)
draw_polyline_norm(frame.reshape((5, 2)), 1,
RGBA(r, g, b, a * alpha))
draw_polyline_norm(hat.reshape((4, 2)), 1,
RGBA(r, g, b, a * alpha))
text_anchor = frame.reshape((5, -1))[2]
text_anchor[1] = 1 - text_anchor[1]
text_anchor *= img_size[1], img_size[0]
text_anchor = text_anchor[0], text_anchor[1] - 75
surface_edit_anchor = text_anchor[0], text_anchor[1] + 25
marker_edit_anchor = text_anchor[0], text_anchor[1] + 50
if self.defined:
if marker_mode:
draw_points([marker_edit_anchor], color=RGBA(0, .8, .7))
else:
draw_points([marker_edit_anchor])
if surface_mode:
draw_points([surface_edit_anchor], color=RGBA(0, .8, .7))
else:
draw_points([surface_edit_anchor])
self.glfont.set_blur(3.9)
self.glfont.set_color_float((0, 0, 0, .8))
self.glfont.draw_text(text_anchor[0] + 15, text_anchor[1] + 6,
self.marker_status())
self.glfont.draw_text(surface_edit_anchor[0] + 15,
surface_edit_anchor[1] + 6,
'edit surface')
self.glfont.draw_text(marker_edit_anchor[0] + 15,
marker_edit_anchor[1] + 6,
'add/remove markers')
self.glfont.set_blur(0.0)
self.glfont.set_color_float((0.1, 8., 8., .9))
self.glfont.draw_text(text_anchor[0] + 15, text_anchor[1] + 6,
self.marker_status())
self.glfont.draw_text(surface_edit_anchor[0] + 15,
surface_edit_anchor[1] + 6,
'edit surface')
self.glfont.draw_text(marker_edit_anchor[0] + 15,
marker_edit_anchor[1] + 6,
'add/remove markers')
else:
progress = (self.build_up_status /
float(self.required_build_up)) * 100
progress_text = '%.0f%%' % progress
self.glfont.set_blur(3.9)
self.glfont.set_color_float((0, 0, 0, .8))
self.glfont.draw_text(text_anchor[0] + 15, text_anchor[1] + 6,
self.marker_status())
self.glfont.draw_text(surface_edit_anchor[0] + 15,
surface_edit_anchor[1] + 6,
'Learning affiliated markers...')
self.glfont.draw_text(marker_edit_anchor[0] + 15,
marker_edit_anchor[1] + 6, progress_text)
self.glfont.set_blur(0.0)
self.glfont.set_color_float((0.1, 8., 8., .9))
self.glfont.draw_text(text_anchor[0] + 15, text_anchor[1] + 6,
self.marker_status())
self.glfont.draw_text(surface_edit_anchor[0] + 15,
surface_edit_anchor[1] + 6,
'Learning affiliated markers...')
self.glfont.draw_text(marker_edit_anchor[0] + 15,
marker_edit_anchor[1] + 6, progress_text)
def gl_draw_corners(self):
"""
draw surface and markers
"""
if self.detected:
frame = cv2.perspectiveTransform(
marker_corners_norm.reshape(-1, 1, 2), self.m_to_screen)
draw_points_norm(frame.reshape((4, 2)), 20,
RGBA(1.0, 0.2, 0.6, .5))
#### fns to draw surface in seperate window
def gl_display_in_window(self, world_tex):
"""
here we map a selected surface onto a seperate window.
"""
if self._window and self.detected:
active_window = glfwGetCurrentContext()
glfwMakeContextCurrent(self._window)
clear_gl_screen()
# cv uses 3x3 gl uses 4x4 tranformation matricies
m = cvmat_to_glmat(self.m_from_screen)
glMatrixMode(GL_PROJECTION)
glPushMatrix()
glLoadIdentity()
glOrtho(0, 1, 0, 1, -1, 1) # gl coord convention
glMatrixMode(GL_MODELVIEW)
glPushMatrix()
#apply m to our quad - this will stretch the quad such that the ref suface will span the window extends
glLoadMatrixf(m)
world_tex.draw()
glMatrixMode(GL_PROJECTION)
glPopMatrix()
glMatrixMode(GL_MODELVIEW)
glPopMatrix()
# now lets get recent pupil positions on this surface:
for gp in self.gaze_on_srf:
draw_points_norm([gp['norm_pos']],
color=RGBA(0.0, 0.8, 0.5, 0.8),
size=80)
glfwSwapBuffers(self._window)
glfwMakeContextCurrent(active_window)
#### fns to draw surface in separate window
def gl_display_in_window_3d(self, world_tex):
"""
here we map a selected surface onto a seperate window.
"""
K, img_size = self.g_pool.capture.intrinsics.K, self.g_pool.capture.intrinsics.resolution
if self._window and self.camera_pose_3d is not None:
active_window = glfwGetCurrentContext()
glfwMakeContextCurrent(self._window)
glClearColor(.8, .8, .8, 1.)
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
glClearDepth(1.0)
glDepthFunc(GL_LESS)
glEnable(GL_DEPTH_TEST)
self.trackball.push()
glMatrixMode(GL_MODELVIEW)
draw_coordinate_system(l=self.real_world_size['x'])
glPushMatrix()
glScalef(self.real_world_size['x'], self.real_world_size['y'], 1)
draw_polyline([[0, 0], [0, 1], [1, 1], [1, 0]],
color=RGBA(.5, .3, .1, .5),
thickness=3)
glPopMatrix()
# Draw the world window as projected onto the plane using the homography mapping
glPushMatrix()
glScalef(self.real_world_size['x'], self.real_world_size['y'], 1)
# cv uses 3x3 gl uses 4x4 tranformation matricies
m = cvmat_to_glmat(self.m_from_screen)
glMultMatrixf(m)
glTranslatef(0, 0, -.01)
world_tex.draw()
draw_polyline([[0, 0], [0, 1], [1, 1], [1, 0]],
color=RGBA(.5, .3, .6, .5),
thickness=3)
glPopMatrix()
# Draw the camera frustum and origin using the 3d tranformation obtained from solvepnp
glPushMatrix()
glMultMatrixf(self.camera_pose_3d.T.flatten())
draw_frustum(img_size, K, 150)
glLineWidth(1)
draw_frustum(img_size, K, .1)
draw_coordinate_system(l=5)
glPopMatrix()
self.trackball.pop()
glfwSwapBuffers(self._window)
glfwMakeContextCurrent(active_window)
def open_window(self):
if not self._window:
if self.fullscreen:
monitor = glfwGetMonitors()[self.monitor_idx]
mode = glfwGetVideoMode(monitor)
height, width = mode[0], mode[1]
else:
monitor = None
height, width = 640, int(
640. /
(self.real_world_size['x'] / self.real_world_size['y'])
) #open with same aspect ratio as surface
self._window = glfwCreateWindow(height,
width,
"Reference Surface: " + self.name,
monitor=monitor,
share=glfwGetCurrentContext())
if not self.fullscreen:
glfwSetWindowPos(self._window, self.window_position_default[0],
self.window_position_default[1])
self.trackball = Trackball()
self.input = {'down': False, 'mouse': (0, 0)}
#Register callbacks
glfwSetFramebufferSizeCallback(self._window, self.on_resize)
glfwSetKeyCallback(self._window, self.on_window_key)
glfwSetWindowCloseCallback(self._window, self.on_close)
glfwSetMouseButtonCallback(self._window,
self.on_window_mouse_button)
glfwSetCursorPosCallback(self._window, self.on_pos)
glfwSetScrollCallback(self._window, self.on_scroll)
self.on_resize(self._window, *glfwGetFramebufferSize(self._window))
# gl_state settings
active_window = glfwGetCurrentContext()
glfwMakeContextCurrent(self._window)
basic_gl_setup()
make_coord_system_norm_based()
# refresh speed settings
glfwSwapInterval(0)
glfwMakeContextCurrent(active_window)
def close_window(self):
if self._window:
glfwDestroyWindow(self._window)
self._window = None
self.window_should_close = False
def open_close_window(self):
if self._window:
self.close_window()
else:
self.open_window()
# window calbacks
def on_resize(self, window, w, h):
self.trackball.set_window_size(w, h)
active_window = glfwGetCurrentContext()
glfwMakeContextCurrent(window)
adjust_gl_view(w, h)
glfwMakeContextCurrent(active_window)
def on_window_key(self, window, key, scancode, action, mods):
if action == GLFW_PRESS:
if key == GLFW_KEY_ESCAPE:
self.on_close()
def on_close(self, window=None):
self.close_window()
def on_window_mouse_button(self, window, button, action, mods):
if action == GLFW_PRESS:
self.input['down'] = True
self.input['mouse'] = glfwGetCursorPos(window)
if action == GLFW_RELEASE:
self.input['down'] = False
def on_pos(self, window, x, y):
if self.input['down']:
old_x, old_y = self.input['mouse']
self.trackball.drag_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 cleanup(self):
if self._window:
self.close_window()
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 Reference_Surface(object):
"""docstring for Reference Surface
The surface coodinate system is 0-1.
Origin is the bottom left corner, (1,1) is the top right
The first scalar in the pos vector is width we call this 'u'.
The second is height we call this 'v'.
The surface is thus defined by 4 vertecies:
Our convention is this order: (0,0),(1,0),(1,1),(0,1)
The surface is supported by a set of n>=1 Markers:
Each marker has an id, you can not not have markers with the same id twice.
Each marker has 4 verts (order is the same as the surface verts)
Each maker vertex has a uv coord that places it on the surface
When we find the surface in locate() we use the correspondence
of uv and screen coords of all 4 verts of all detected markers to get the
surface to screen homography.
This allows us to get homographies for partially visible surfaces,
all we need are 2 visible markers. (We could get away with just
one marker but in pracise this is to noisy.)
The more markers we find the more accurate the homography.
"""
def __init__(self,name="unnamed",saved_definition=None):
self.name = name
self.markers = {}
self.detected_markers = 0
self.defined = False
self.build_up_status = 0
self.required_build_up = 90.
self.detected = False
self.m_to_screen = None
self.m_from_screen = None
self.camera_pose_3d = None
self.use_distortion = 0
self.uid = str(time())
self.real_world_size = {'x':1.,'y':1.}
###window and gui vars
self._window = None
self.fullscreen = False
self.window_should_open = False
self.window_should_close = False
self.gaze_on_srf = [] # points on surface for realtime feedback display
self.glfont = fontstash.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.old_corners_robust = None
if saved_definition is not None:
self.load_from_dict(saved_definition)
def save_to_dict(self):
"""
save all markers and name of this surface to a dict.
"""
markers = dict([(m_id,m.uv_coords) for m_id,m in self.markers.iteritems()])
return {'name':self.name,'uid':self.uid,'markers':markers,'real_world_size':self.real_world_size}
def load_from_dict(self,d):
"""
load all markers of this surface to a dict.
"""
self.name = d['name']
self.uid = d['uid']
self.real_world_size = d.get('real_world_size',{'x':1.,'y':1.})
marker_dict = d['markers']
for m_id,uv_coords in marker_dict.iteritems():
self.markers[m_id] = Support_Marker(m_id)
self.markers[m_id].load_uv_coords(uv_coords)
#flag this surface as fully defined
self.defined = True
self.build_up_status = self.required_build_up
def build_correspondance(self, visible_markers,camera_calibration,min_marker_perimeter,min_id_confidence):
"""
- use all visible markers
- fit a convex quadrangle around it
- use quadrangle verts to establish perpective transform
- map all markers into surface space
- build up list of found markers and their uv coords
"""
all_verts = [m['verts'] for m in visible_markers if m['perimeter']>=min_marker_perimeter]
if not all_verts:
return
all_verts = np.array(all_verts)
all_verts.shape = (-1,1,2) # [vert,vert,vert,vert,vert...] with vert = [[r,c]]
# all_verts_undistorted_normalized centered in img center flipped in y and range [-1,1]
all_verts_undistorted_normalized = cv2.undistortPoints(all_verts, camera_calibration['camera_matrix'],camera_calibration['dist_coefs']*self.use_distortion)
hull = cv2.convexHull(all_verts_undistorted_normalized,clockwise=False)
#simplify until we have excatly 4 verts
if hull.shape[0]>4:
new_hull = cv2.approxPolyDP(hull,epsilon=1,closed=True)
if new_hull.shape[0]>=4:
hull = new_hull
if hull.shape[0]>4:
curvature = abs(GetAnglesPolyline(hull,closed=True))
most_acute_4_threshold = sorted(curvature)[3]
hull = hull[curvature<=most_acute_4_threshold]
# all_verts_undistorted_normalized space is flipped in y.
# we need to change the order of the hull vertecies
hull = hull[[1,0,3,2],:,:]
# now we need to roll the hull verts until we have the right orientation:
# all_verts_undistorted_normalized space has its origin at the image center.
# adding 1 to the coordinates puts the origin at the top left.
distance_to_top_left = np.sqrt((hull[:,:,0]+1)**2+(hull[:,:,1]+1)**2)
bot_left_idx = np.argmin(distance_to_top_left)+1
hull = np.roll(hull,-bot_left_idx,axis=0)
#based on these 4 verts we calculate the transformations into a 0,0 1,1 square space
m_from_undistored_norm_space = m_verts_from_screen(hull)
self.detected = True
# map the markers vertices into the surface space (one can think of these as texture coordinates u,v)
marker_uv_coords = cv2.perspectiveTransform(all_verts_undistorted_normalized,m_from_undistored_norm_space)
marker_uv_coords.shape = (-1,4,1,2) #[marker,marker...] marker = [ [[r,c]],[[r,c]] ]
# build up a dict of discovered markers. Each with a history of uv coordinates
for m,uv in zip (visible_markers,marker_uv_coords):
try:
self.markers[m['id']].add_uv_coords(uv)
except KeyError:
self.markers[m['id']] = Support_Marker(m['id'])
self.markers[m['id']].add_uv_coords(uv)
#average collection of uv correspondences accros detected markers
self.build_up_status = sum([len(m.collected_uv_coords) for m in self.markers.values()])/float(len(self.markers))
if self.build_up_status >= self.required_build_up:
self.finalize_correnspondance()
def finalize_correnspondance(self):
"""
- prune markers that have been visible in less than x percent of frames.
- of those markers select a good subset of uv coords and compute mean.
- this mean value will be used from now on to estable surface transform
"""
persistent_markers = {}
for k,m in self.markers.iteritems():
if len(m.collected_uv_coords)>self.required_build_up*.5:
persistent_markers[k] = m
self.markers = persistent_markers
for m in self.markers.values():
m.compute_robust_mean()
self.defined = True
def locate(self, visible_markers,camera_calibration,min_marker_perimeter,min_id_confidence, locate_3d=False,):
"""
- find overlapping set of surface markers and visible_markers
- compute homography (and inverse) based on this subset
"""
if not self.defined:
self.build_correspondance(visible_markers,camera_calibration,min_marker_perimeter,min_id_confidence)
res = self._get_location(visible_markers,camera_calibration,min_marker_perimeter,min_id_confidence,locate_3d)
self.detected = res['detected']
self.detected_markers = res['detected_markers']
self.m_to_screen = res['m_to_screen']
self.m_from_screen = res['m_from_screen']
self.camera_pose_3d = res['camera_pose_3d']
def _get_location(self,visible_markers,camera_calibration,min_marker_perimeter,min_id_confidence, locate_3d=False):
filtered_markers = [m for m in visible_markers if m['perimeter']>=min_marker_perimeter and m['id_confidence']>min_id_confidence]
marker_by_id ={}
#if an id shows twice we use the bigger marker (usually this is a screen camera echo artifact.)
for m in filtered_markers:
if m["id"] in marker_by_id and m["perimeter"] < marker_by_id[m['id']]['perimeter']:
pass
else:
marker_by_id[m["id"]] = m
visible_ids = set(marker_by_id.keys())
requested_ids = set(self.markers.keys())
overlap = visible_ids & requested_ids
# need at least two markers per surface when the surface is more that 1 marker.
if overlap and len(overlap) >= min(2,len(requested_ids)):
detected = True
xy = np.array( [marker_by_id[i]['verts'] for i in overlap] )
uv = np.array( [self.markers[i].uv_coords for i in overlap] )
uv.shape=(-1,1,2)
# our camera lens creates distortions we want to get a good 2d estimate despite that so we:
# compute the homography transform from marker into the undistored normalized image space
# (the line below is the same as what you find in methods.undistort_unproject_pts, except that we ommit the z corrd as it is always one.)
xy_undistorted_normalized = cv2.undistortPoints(xy.reshape(-1,1,2), camera_calibration['camera_matrix'],camera_calibration['dist_coefs']*self.use_distortion)
m_to_undistored_norm_space,mask = cv2.findHomography(uv,xy_undistorted_normalized, method=cv2.cv.CV_RANSAC,ransacReprojThreshold=0.1)
if not mask.all():
detected = False
m_from_undistored_norm_space,mask = cv2.findHomography(xy_undistorted_normalized,uv)
# project the corners of the surface to undistored space
corners_undistored_space = cv2.perspectiveTransform(marker_corners_norm.reshape(-1,1,2),m_to_undistored_norm_space)
# project and distort these points and normalize them
corners_redistorted, corners_redistorted_jacobian = cv2.projectPoints(cv2.convertPointsToHomogeneous(corners_undistored_space), np.array([0,0,0], dtype=np.float32) , np.array([0,0,0], dtype=np.float32), camera_calibration['camera_matrix'], camera_calibration['dist_coefs']*self.use_distortion)
corners_nulldistorted, corners_nulldistorted_jacobian = cv2.projectPoints(cv2.convertPointsToHomogeneous(corners_undistored_space), np.array([0,0,0], dtype=np.float32) , np.array([0,0,0], dtype=np.float32), camera_calibration['camera_matrix'], camera_calibration['dist_coefs']*0)
#normalize to pupil norm space
corners_redistorted.shape = -1,2
corners_redistorted /= camera_calibration['resolution']
corners_redistorted[:,-1] = 1-corners_redistorted[:,-1]
#normalize to pupil norm space
corners_nulldistorted.shape = -1,2
corners_nulldistorted /= camera_calibration['resolution']
corners_nulldistorted[:,-1] = 1-corners_nulldistorted[:,-1]
# maps for extreme lens distortions will behave irratically beyond the image bounds
# since our surfaces often extend beyond the screen we need to interpolate
# between a distored projection and undistored one.
# def ratio(val):
# centered_val = abs(.5 - val)
# # signed distance to img cennter .5 is imag bound
# # we look to interpolate between .7 and .9
# inter = max()
corners_robust = []
for nulldist,redist in zip(corners_nulldistorted,corners_redistorted):
if -.4 < nulldist[0] <1.4 and -.4 < nulldist[1] <1.4:
corners_robust.append(redist)
else:
corners_robust.append(nulldist)
corners_robust = np.array(corners_robust)
if self.old_corners_robust is not None and np.mean(np.abs(corners_robust-self.old_corners_robust)) < 0.02:
smooth_corners_robust = self.old_corners_robust
smooth_corners_robust += .5*(corners_robust-self.old_corners_robust )
corners_robust = smooth_corners_robust
self.old_corners_robust = smooth_corners_robust
else:
self.old_corners_robust = corners_robust
#compute a perspective thransform from from the marker norm space to the apparent image.
# The surface corners will be at the right points
# However the space between the corners may be distored due to distortions of the lens,
m_to_screen = m_verts_to_screen(corners_robust)
m_from_screen = m_verts_from_screen(corners_robust)
camera_pose_3d = None
if locate_3d:
dist_coef, = camera_calibration['dist_coefs']
img_size = camera_calibration['resolution']
K = camera_calibration['camera_matrix']
# 3d marker support pose estimation:
# scale normalized object points to world space units (think m,cm,mm)
uv.shape = -1,2
uv *= [self.real_world_size['x'], self.real_world_size['y']]
# convert object points to lie on z==0 plane in 3d space
uv3d = np.zeros((uv.shape[0], uv.shape[1]+1))
uv3d[:,:-1] = uv
xy.shape = -1,1,2
# compute pose of object relative to camera center
is3dPoseAvailable, rot3d_cam_to_object, translate3d_cam_to_object = cv2.solvePnP(uv3d, xy, K, dist_coef,flags=cv2.CV_EPNP)
if is3dPoseAvailable:
# not verifed, potentially usefull info: http://stackoverflow.com/questions/17423302/opencv-solvepnp-tvec-units-and-axes-directions
###marker posed estimation from virtually projected points.
# object_pts = np.array([[[0,0],[0,1],[1,1],[1,0]]],dtype=np.float32)
# projected_pts = cv2.perspectiveTransform(object_pts,self.m_to_screen)
# projected_pts.shape = -1,2
# projected_pts *= img_size
# projected_pts.shape = -1, 1, 2
# # scale object points to world space units (think m,cm,mm)
# object_pts.shape = -1,2
# object_pts *= self.real_world_size
# # convert object points to lie on z==0 plane in 3d space
# object_pts_3d = np.zeros((4,3))
# object_pts_3d[:,:-1] = object_pts
# self.is3dPoseAvailable, rot3d_cam_to_object, translate3d_cam_to_object = cv2.solvePnP(object_pts_3d, projected_pts, K, dist_coef,flags=cv2.CV_EPNP)
# transformation from Camera Optical Center:
# first: translate from Camera center to object origin.
# second: rotate x,y,z
# coordinate system is x,y,z where z goes out from the camera into the viewed volume.
# print rot3d_cam_to_object[0],rot3d_cam_to_object[1],rot3d_cam_to_object[2], translate3d_cam_to_object[0],translate3d_cam_to_object[1],translate3d_cam_to_object[2]
#turn translation vectors into 3x3 rot mat.
rot3d_cam_to_object_mat, _ = cv2.Rodrigues(rot3d_cam_to_object)
#to get the transformation from object to camera we need to reverse rotation and translation
translate3d_object_to_cam = - translate3d_cam_to_object
# rotation matrix inverse == transpose
rot3d_object_to_cam_mat = rot3d_cam_to_object_mat.T
# we assume that the volume of the object grows out of the marker surface and not into it. We thus have to flip the z-Axis:
flip_z_axix_hm = np.eye(4, dtype=np.float32)
flip_z_axix_hm[2,2] = -1
# create a homogenous tranformation matrix from the rotation mat
rot3d_object_to_cam_hm = np.eye(4, dtype=np.float32)
rot3d_object_to_cam_hm[:-1,:-1] = rot3d_object_to_cam_mat
# create a homogenous tranformation matrix from the translation vect
translate3d_object_to_cam_hm = np.eye(4, dtype=np.float32)
translate3d_object_to_cam_hm[:-1, -1] = translate3d_object_to_cam.reshape(3)
# combine all tranformations into transformation matrix that decribes the move from object origin and orientation to camera origin and orientation
tranform3d_object_to_cam = np.matrix(flip_z_axix_hm) * np.matrix(rot3d_object_to_cam_hm) * np.matrix(translate3d_object_to_cam_hm)
camera_pose_3d = tranform3d_object_to_cam
if detected == False:
camera_pose_3d = None
m_from_screen = None
m_to_screen = None
m_from_undistored_norm_space = None
m_to_undistored_norm_space = None
else:
detected = False
camera_pose_3d = None
m_from_screen = None
m_to_screen = None
m_from_undistored_norm_space = None
m_to_undistored_norm_space = None
return {'detected':detected,'detected_markers':len(overlap),'m_from_undistored_norm_space':m_from_undistored_norm_space,'m_to_undistored_norm_space':m_to_undistored_norm_space,'m_from_screen':m_from_screen,'m_to_screen':m_to_screen,'camera_pose_3d':camera_pose_3d}
def img_to_ref_surface(self,pos):
#convenience lines to allow 'simple' vectors (x,y) to be used
shape = pos.shape
pos.shape = (-1,1,2)
new_pos = cv2.perspectiveTransform(pos,self.m_from_screen )
new_pos.shape = shape
return new_pos
def ref_surface_to_img(self,pos):
#convenience lines to allow 'simple' vectors (x,y) to be used
shape = pos.shape
pos.shape = (-1,1,2)
new_pos = cv2.perspectiveTransform(pos,self.m_to_screen )
new_pos.shape = shape
return new_pos
@staticmethod
def map_datum_to_surface(d,m_from_screen):
pos = np.array([d['norm_pos']]).reshape(1,1,2)
mapped_pos = cv2.perspectiveTransform(pos , m_from_screen )
mapped_pos.shape = (2)
on_srf = bool((0 <= mapped_pos[0] <= 1) and (0 <= mapped_pos[1] <= 1))
return {'topic':d['topic']+"_on_surface",'norm_pos':(mapped_pos[0],mapped_pos[1]),'confidence':d['confidence'],'on_srf':on_srf,'base_data':d }
def map_data_to_surface(self,data,m_from_screen):
return [self.map_datum_to_surface(d,m_from_screen) for d in data]
def move_vertex(self,vert_idx,new_pos):
"""
this fn is used to manipulate the surface boundary (coordinate system)
new_pos is in uv-space coords
if we move one vertex of the surface we need to find
the tranformation from old quadrangle to new quardangle
and apply that transformation to our marker uv-coords
"""
before = marker_corners_norm
after = before.copy()
after[vert_idx] = new_pos
transform = cv2.getPerspectiveTransform(after,before)
for m in self.markers.values():
m.uv_coords = cv2.perspectiveTransform(m.uv_coords,transform)
def add_marker(self,marker,visible_markers,camera_calibration,min_marker_perimeter,min_id_confidence):
'''
add marker to surface.
'''
res = self._get_location(visible_markers,camera_calibration,min_marker_perimeter,min_id_confidence,locate_3d=False)
if res['detected']:
support_marker = Support_Marker(marker['id'])
marker_verts = np.array(marker['verts'])
marker_verts.shape = (-1,1,2)
marker_verts_undistorted_normalized = cv2.undistortPoints(marker_verts, camera_calibration['camera_matrix'],camera_calibration['dist_coefs']*self.use_distortion)
marker_uv_coords = cv2.perspectiveTransform(marker_verts_undistorted_normalized,res['m_from_undistored_norm_space'])
support_marker.load_uv_coords(marker_uv_coords)
self.markers[marker['id']] = support_marker
def remove_marker(self,marker):
if len(self.markers) == 1:
logger.warning("Need at least one marker per surface. Will not remove this last marker.")
return
self.markers.pop(marker['id'])
def marker_status(self):
return "%s %s/%s" %(self.name,self.detected_markers,len(self.markers))
def get_mode_toggle(self,pos,img_shape):
if self.detected and self.defined:
x,y = pos
frame = np.array([[[0,0],[1,0],[1,1],[0,1],[0,0]]],dtype=np.float32)
frame = cv2.perspectiveTransform(frame,self.m_to_screen)
text_anchor = frame.reshape((5,-1))[2]
text_anchor[1] = 1-text_anchor[1]
text_anchor *=img_shape[1],img_shape[0]
text_anchor = text_anchor[0],text_anchor[1]-75
surface_edit_anchor = text_anchor[0],text_anchor[1]+25
marker_edit_anchor = text_anchor[0],text_anchor[1]+50
if np.sqrt((x-surface_edit_anchor[0])**2 + (y-surface_edit_anchor[1])**2) <15:
return 'surface_mode'
elif np.sqrt((x-marker_edit_anchor[0])**2 + (y-marker_edit_anchor[1])**2) <15:
return 'marker_mode'
else:
return None
else:
return None
def gl_draw_frame(self,img_size,color = (1.0,0.2,0.6,1.0),highlight=False,surface_mode=False,marker_mode=False):
"""
draw surface and markers
"""
if self.detected:
r,g,b,a = color
frame = np.array([[[0,0],[1,0],[1,1],[0,1],[0,0]]],dtype=np.float32)
hat = np.array([[[.3,.7],[.7,.7],[.5,.9],[.3,.7]]],dtype=np.float32)
hat = cv2.perspectiveTransform(hat,self.m_to_screen)
frame = cv2.perspectiveTransform(frame,self.m_to_screen)
alpha = min(1,self.build_up_status/self.required_build_up)
if highlight:
draw_polyline_norm(frame.reshape((5,2)),1,RGBA(r,g,b,a*.1),line_type=GL_POLYGON)
draw_polyline_norm(frame.reshape((5,2)),1,RGBA(r,g,b,a*alpha))
draw_polyline_norm(hat.reshape((4,2)),1,RGBA(r,g,b,a*alpha))
text_anchor = frame.reshape((5,-1))[2]
text_anchor[1] = 1-text_anchor[1]
text_anchor *=img_size[1],img_size[0]
text_anchor = text_anchor[0],text_anchor[1]-75
surface_edit_anchor = text_anchor[0],text_anchor[1]+25
marker_edit_anchor = text_anchor[0],text_anchor[1]+50
if self.defined:
if marker_mode:
draw_points([marker_edit_anchor],color=RGBA(0,.8,.7))
else:
draw_points([marker_edit_anchor])
if surface_mode:
draw_points([surface_edit_anchor],color=RGBA(0,.8,.7))
else:
draw_points([surface_edit_anchor])
self.glfont.set_blur(3.9)
self.glfont.set_color_float((0,0,0,.8))
self.glfont.draw_text(text_anchor[0]+15,text_anchor[1]+6,self.marker_status())
self.glfont.draw_text(surface_edit_anchor[0]+15,surface_edit_anchor[1]+6,'edit surface')
self.glfont.draw_text(marker_edit_anchor[0]+15,marker_edit_anchor[1]+6,'add/remove markers')
self.glfont.set_blur(0.0)
self.glfont.set_color_float((0.1,8.,8.,.9))
self.glfont.draw_text(text_anchor[0]+15,text_anchor[1]+6,self.marker_status())
self.glfont.draw_text(surface_edit_anchor[0]+15,surface_edit_anchor[1]+6,'edit surface')
self.glfont.draw_text(marker_edit_anchor[0]+15,marker_edit_anchor[1]+6,'add/remove markers')
else:
progress = (self.build_up_status/float(self.required_build_up))*100
progress_text = '%.0f%%'%progress
self.glfont.set_blur(3.9)
self.glfont.set_color_float((0,0,0,.8))
self.glfont.draw_text(text_anchor[0]+15,text_anchor[1]+6,self.marker_status())
self.glfont.draw_text(surface_edit_anchor[0]+15,surface_edit_anchor[1]+6,'Learning affiliated markers...')
self.glfont.draw_text(marker_edit_anchor[0]+15,marker_edit_anchor[1]+6,progress_text)
self.glfont.set_blur(0.0)
self.glfont.set_color_float((0.1,8.,8.,.9))
self.glfont.draw_text(text_anchor[0]+15,text_anchor[1]+6,self.marker_status())
self.glfont.draw_text(surface_edit_anchor[0]+15,surface_edit_anchor[1]+6,'Learning affiliated markers...')
self.glfont.draw_text(marker_edit_anchor[0]+15,marker_edit_anchor[1]+6,progress_text)
def gl_draw_corners(self):
"""
draw surface and markers
"""
if self.detected:
frame = cv2.perspectiveTransform(marker_corners_norm.reshape(-1,1,2),self.m_to_screen)
draw_points_norm(frame.reshape((4,2)),20,RGBA(1.0,0.2,0.6,.5))
#### fns to draw surface in seperate window
def gl_display_in_window(self,world_tex):
"""
here we map a selected surface onto a seperate window.
"""
if self._window and self.detected:
active_window = glfwGetCurrentContext()
glfwMakeContextCurrent(self._window)
clear_gl_screen()
# cv uses 3x3 gl uses 4x4 tranformation matricies
m = cvmat_to_glmat(self.m_from_screen)
glMatrixMode(GL_PROJECTION)
glPushMatrix()
glLoadIdentity()
glOrtho(0, 1, 0, 1,-1,1) # gl coord convention
glMatrixMode(GL_MODELVIEW)
glPushMatrix()
#apply m to our quad - this will stretch the quad such that the ref suface will span the window extends
glLoadMatrixf(m)
world_tex.draw()
glMatrixMode(GL_PROJECTION)
glPopMatrix()
glMatrixMode(GL_MODELVIEW)
glPopMatrix()
# now lets get recent pupil positions on this surface:
for gp in self.gaze_on_srf:
draw_points_norm([gp['norm_pos']],color=RGBA(0.0,0.8,0.5,0.8), size=80)
glfwSwapBuffers(self._window)
glfwMakeContextCurrent(active_window)
#### fns to draw surface in separate window
def gl_display_in_window_3d(self,world_tex,camera_intrinsics):
"""
here we map a selected surface onto a seperate window.
"""
K,dist_coef,img_size = camera_intrinsics['camera_matrix'],camera_intrinsics['dist_coefs'],camera_intrinsics['resolution']
if self._window and self.camera_pose_3d is not None:
active_window = glfwGetCurrentContext()
glfwMakeContextCurrent(self._window)
glClearColor(.8,.8,.8,1.)
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
glClearDepth(1.0)
glDepthFunc(GL_LESS)
glEnable(GL_DEPTH_TEST)
self.trackball.push()
glMatrixMode(GL_MODELVIEW)
draw_coordinate_system(l=self.real_world_size['x'])
glPushMatrix()
glScalef(self.real_world_size['x'],self.real_world_size['y'],1)
draw_polyline([[0,0],[0,1],[1,1],[1,0]],color = RGBA(.5,.3,.1,.5),thickness=3)
glPopMatrix()
# Draw the world window as projected onto the plane using the homography mapping
glPushMatrix()
glScalef(self.real_world_size['x'], self.real_world_size['y'], 1)
# cv uses 3x3 gl uses 4x4 tranformation matricies
m = cvmat_to_glmat(self.m_from_screen)
glMultMatrixf(m)
glTranslatef(0,0,-.01)
world_tex.draw()
draw_polyline([[0,0],[0,1],[1,1],[1,0]],color = RGBA(.5,.3,.6,.5),thickness=3)
glPopMatrix()
# Draw the camera frustum and origin using the 3d tranformation obtained from solvepnp
glPushMatrix()
glMultMatrixf(self.camera_pose_3d.T.flatten())
draw_frustum(img_size, K, 150)
glLineWidth(1)
draw_frustum(img_size, K, .1)
draw_coordinate_system(l=5)
glPopMatrix()
self.trackball.pop()
glfwSwapBuffers(self._window)
glfwMakeContextCurrent(active_window)
def open_window(self):
if not self._window:
if self.fullscreen:
monitor = glfwGetMonitors()[self.monitor_idx]
mode = glfwGetVideoMode(monitor)
height,width= mode[0],mode[1]
else:
monitor = None
height,width= 640,int(640./(self.real_world_size['x']/self.real_world_size['y'])) #open with same aspect ratio as surface
self._window = glfwCreateWindow(height, width, "Reference Surface: " + self.name, monitor=monitor, share=glfwGetCurrentContext())
if not self.fullscreen:
glfwSetWindowPos(self._window,200,0)
self.trackball = Trackball()
self.input = {'down':False, 'mouse':(0,0)}
#Register callbacks
glfwSetFramebufferSizeCallback(self._window,self.on_resize)
glfwSetKeyCallback(self._window,self.on_key)
glfwSetWindowCloseCallback(self._window,self.on_close)
glfwSetMouseButtonCallback(self._window,self.on_button)
glfwSetCursorPosCallback(self._window,self.on_pos)
glfwSetScrollCallback(self._window,self.on_scroll)
self.on_resize(self._window,*glfwGetFramebufferSize(self._window))
# gl_state settings
active_window = glfwGetCurrentContext()
glfwMakeContextCurrent(self._window)
basic_gl_setup()
make_coord_system_norm_based()
# refresh speed settings
glfwSwapInterval(0)
glfwMakeContextCurrent(active_window)
def close_window(self):
if self._window:
glfwDestroyWindow(self._window)
self._window = None
self.window_should_close = False
def open_close_window(self):
if self._window:
self.close_window()
else:
self.open_window()
# window calbacks
def on_resize(self,window,w, h):
self.trackball.set_window_size(w,h)
active_window = glfwGetCurrentContext()
glfwMakeContextCurrent(window)
adjust_gl_view(w,h)
glfwMakeContextCurrent(active_window)
def on_key(self,window, key, scancode, action, mods):
if action == GLFW_PRESS:
if key == GLFW_KEY_ESCAPE:
self.on_close()
def on_close(self,window=None):
self.window_should_close = True
def on_button(self,window,button, action, mods):
if action == GLFW_PRESS:
self.input['down'] = True
self.input['mouse'] = glfwGetCursorPos(window)
if action == GLFW_RELEASE:
self.input['down'] = False
def on_pos(self,window,x, y):
if self.input['down']:
old_x,old_y = self.input['mouse']
self.trackball.drag_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 cleanup(self):
if self._window:
self.close_window()
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)
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(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 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 Reference_Surface(object):
"""docstring for Reference Surface
The surface coodinate system is 0-1.
Origin is the bottom left corner, (1,1) is the top right
The first scalar in the pos vector is width we call this 'u'.
The second is height we call this 'v'.
The surface is thus defined by 4 vertecies:
Our convention is this order: (0,0),(1,0),(1,1),(0,1)
The surface is supported by a set of n>=1 Markers:
Each marker has an id, you can not not have markers with the same id twice.
Each marker has 4 verts (order is the same as the surface verts)
Each maker vertex has a uv coord that places it on the surface
When we find the surface in locate() we use the correspondence
of uv and screen coords of all 4 verts of all detected markers to get the
surface to screen homography.
This allows us to get homographies for partially visible surfaces,
all we need are 2 visible markers. (We could get away with just
one marker but in pracise this is to noisy.)
The more markers we find the more accurate the homography.
"""
def __init__(self, name="unnamed", saved_definition=None):
self.name = name
self.markers = {}
self.detected_markers = 0
self.defined = False
self.build_up_status = 0
self.required_build_up = 90.
self.detected = False
self.m_to_screen = None
self.m_from_screen = None
self.camera_pose_3d = None
self.uid = str(time())
self.real_world_size = {'x': 1., 'y': 1.}
###window and gui vars
self._window = None
self.fullscreen = False
self.window_should_open = False
self.window_should_close = False
self.gaze_on_srf = [
] # points on surface for realtime feedback display
self.glfont = fontstash.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))
if saved_definition is not None:
self.load_from_dict(saved_definition)
def save_to_dict(self):
"""
save all markers and name of this surface to a dict.
"""
markers = dict([(m_id, m.uv_coords)
for m_id, m in self.markers.iteritems()])
return {
'name': self.name,
'uid': self.uid,
'markers': markers,
'real_world_size': self.real_world_size
}
def load_from_dict(self, d):
"""
load all markers of this surface to a dict.
"""
self.name = d['name']
self.uid = d['uid']
self.real_world_size = d.get('real_world_size', {'x': 1., 'y': 1.})
marker_dict = d['markers']
for m_id, uv_coords in marker_dict.iteritems():
self.markers[m_id] = Support_Marker(m_id)
self.markers[m_id].load_uv_coords(uv_coords)
#flag this surface as fully defined
self.defined = True
self.build_up_status = self.required_build_up
def build_correspondance(self, visible_markers):
"""
- use all visible markers
- fit a convex quadrangle around it
- use quadrangle verts to establish perpective transform
- map all markers into surface space
- build up list of found markers and their uv coords
"""
if visible_markers == []:
self.m_to_screen = None
self.m_from_screen = None
self.detected = False
return
all_verts = np.array([[m['verts_norm'] for m in visible_markers]])
all_verts.shape = (
-1, 1, 2) # [vert,vert,vert,vert,vert...] with vert = [[r,c]]
hull = cv2.convexHull(all_verts, clockwise=False)
#simplify until we have excatly 4 verts
if hull.shape[0] > 4:
new_hull = cv2.approxPolyDP(hull, epsilon=1, closed=True)
if new_hull.shape[0] >= 4:
hull = new_hull
if hull.shape[0] > 4:
curvature = abs(GetAnglesPolyline(hull, closed=True))
most_acute_4_threshold = sorted(curvature)[3]
hull = hull[curvature <= most_acute_4_threshold]
#now we need to roll the hull verts until we have the right orientation:
distance_to_origin = np.sqrt(hull[:, :, 0]**2 + hull[:, :, 1]**2)
top_left_idx = np.argmin(distance_to_origin)
hull = np.roll(hull, -top_left_idx, axis=0)
#based on these 4 verts we calculate the transformations into a 0,0 1,1 square space
self.m_to_screen = m_verts_to_screen(hull)
self.m_from_screen = m_verts_from_screen(hull)
self.detected = True
# map the markers vertices in to the surface space (one can think of these as texture coordinates u,v)
marker_uv_coords = cv2.perspectiveTransform(all_verts,
self.m_from_screen)
marker_uv_coords.shape = (
-1, 4, 1, 2) #[marker,marker...] marker = [ [[r,c]],[[r,c]] ]
# build up a dict of discovered markers. Each with a history of uv coordinates
for m, uv in zip(visible_markers, marker_uv_coords):
try:
self.markers[m['id']].add_uv_coords(uv)
except KeyError:
self.markers[m['id']] = Support_Marker(m['id'])
self.markers[m['id']].add_uv_coords(uv)
#average collection of uv correspondences accros detected markers
self.build_up_status = sum(
[len(m.collected_uv_coords)
for m in self.markers.values()]) / float(len(self.markers))
if self.build_up_status >= self.required_build_up:
self.finalize_correnspondance()
self.defined = True
def finalize_correnspondance(self):
"""
- prune markers that have been visible in less than x percent of frames.
- of those markers select a good subset of uv coords and compute mean.
- this mean value will be used from now on to estable surface transform
"""
persistent_markers = {}
for k, m in self.markers.iteritems():
if len(m.collected_uv_coords) > self.required_build_up * .5:
persistent_markers[k] = m
self.markers = persistent_markers
for m in self.markers.values():
m.compute_robust_mean()
def locate(self, visible_markers, locate_3d=False, camera_intrinsics=None):
"""
- find overlapping set of surface markers and visible_markers
- compute homography (and inverse) based on this subset
"""
if not self.defined:
self.build_correspondance(visible_markers)
else:
marker_by_id = dict([(m['id'], m) for m in visible_markers])
visible_ids = set(marker_by_id.keys())
requested_ids = set(self.markers.keys())
overlap = visible_ids & requested_ids
self.detected_markers = len(overlap)
if len(overlap) >= min(1, len(requested_ids)):
self.detected = True
yx = np.array([marker_by_id[i]['verts_norm'] for i in overlap])
uv = np.array([self.markers[i].uv_coords for i in overlap])
yx.shape = (-1, 1, 2)
uv.shape = (-1, 1, 2)
# print 'uv',uv
# print 'yx',yx
self.m_to_screen, mask = cv2.findHomography(uv, yx)
self.m_from_screen = np.linalg.inv(self.m_to_screen)
#self.m_from_screen,mask = cv2.findHomography(yx,uv)
if locate_3d:
K, dist_coef, img_size = camera_intrinsics
###marker support pose estiamtion:
# denormalize image reference points to pixel space
yx.shape = -1, 2
yx *= img_size
yx.shape = -1, 1, 2
# scale normalized object points to world space units (think m,cm,mm)
uv.shape = -1, 2
uv *= [
self.real_world_size['x'], self.real_world_size['y']
]
# convert object points to lie on z==0 plane in 3d space
uv3d = np.zeros((uv.shape[0], uv.shape[1] + 1))
uv3d[:, :-1] = uv
# compute pose of object relative to camera center
self.is3dPoseAvailable, rot3d_cam_to_object, translate3d_cam_to_object = cv2.solvePnP(
uv3d, yx, K, dist_coef, flags=cv2.CV_EPNP)
# not verifed, potentially usefull info: http://stackoverflow.com/questions/17423302/opencv-solvepnp-tvec-units-and-axes-directions
###marker posed estimation from virtually projected points.
# object_pts = np.array([[[0,0],[0,1],[1,1],[1,0]]],dtype=np.float32)
# projected_pts = cv2.perspectiveTransform(object_pts,self.m_to_screen)
# projected_pts.shape = -1,2
# projected_pts *= img_size
# projected_pts.shape = -1, 1, 2
# # scale object points to world space units (think m,cm,mm)
# object_pts.shape = -1,2
# object_pts *= self.real_world_size
# # convert object points to lie on z==0 plane in 3d space
# object_pts_3d = np.zeros((4,3))
# object_pts_3d[:,:-1] = object_pts
# self.is3dPoseAvailable, rot3d_cam_to_object, translate3d_cam_to_object = cv2.solvePnP(object_pts_3d, projected_pts, K, dist_coef,flags=cv2.CV_EPNP)
# transformation from Camera Optical Center:
# first: translate from Camera center to object origin.
# second: rotate x,y,z
# coordinate system is x,y,z where z goes out from the camera into the viewed volume.
# print rot3d_cam_to_object[0],rot3d_cam_to_object[1],rot3d_cam_to_object[2], translate3d_cam_to_object[0],translate3d_cam_to_object[1],translate3d_cam_to_object[2]
#turn translation vectors into 3x3 rot mat.
rot3d_cam_to_object_mat, _ = cv2.Rodrigues(
rot3d_cam_to_object)
#to get the transformation from object to camera we need to reverse rotation and translation
translate3d_object_to_cam = -translate3d_cam_to_object
# rotation matrix inverse == transpose
rot3d_object_to_cam_mat = rot3d_cam_to_object_mat.T
# we assume that the volume of the object grows out of the marker surface and not into it. We thus have to flip the z-Axis:
flip_z_axix_hm = np.eye(4, dtype=np.float32)
flip_z_axix_hm[2, 2] = -1
# create a homogenous tranformation matrix from the rotation mat
rot3d_object_to_cam_hm = np.eye(4, dtype=np.float32)
rot3d_object_to_cam_hm[:-1, :-1] = rot3d_object_to_cam_mat
# create a homogenous tranformation matrix from the translation vect
translate3d_object_to_cam_hm = np.eye(4, dtype=np.float32)
translate3d_object_to_cam_hm[:-1,
-1] = translate3d_object_to_cam.reshape(
3)
# combine all tranformations into of matrix that decribes the move from object origin and orientation to camera origin and orientation
tranform3d_object_to_cam = np.matrix(
flip_z_axix_hm) * np.matrix(
rot3d_object_to_cam_hm) * np.matrix(
translate3d_object_to_cam_hm)
self.camera_pose_3d = tranform3d_object_to_cam
else:
self.is3dPoseAvailable = False
else:
self.detected = False
self.is3dPoseAvailable = False
self.m_from_screen = None
self.m_to_screen = None
def img_to_ref_surface(self, pos):
if self.m_from_screen is not None:
#convenience lines to allow 'simple' vectors (x,y) to be used
shape = pos.shape
pos.shape = (-1, 1, 2)
new_pos = cv2.perspectiveTransform(pos, self.m_from_screen)
new_pos.shape = shape
return new_pos
else:
return None
def ref_surface_to_img(self, pos):
if self.m_to_screen is not None:
#convenience lines to allow 'simple' vectors (x,y) to be used
shape = pos.shape
pos.shape = (-1, 1, 2)
new_pos = cv2.perspectiveTransform(pos, self.m_to_screen)
new_pos.shape = shape
return new_pos
else:
return None
def move_vertex(self, vert_idx, new_pos):
"""
this fn is used to manipulate the surface boundary (coordinate system)
new_pos is in uv-space coords
if we move one vertex of the surface we need to find
the tranformation from old quadrangle to new quardangle
and apply that transformation to our marker uv-coords
"""
before = np.array(((0, 0), (1, 0), (1, 1), (0, 1)), dtype=np.float32)
after = before.copy()
after[vert_idx] = new_pos
transform = cv2.getPerspectiveTransform(after, before)
for m in self.markers.values():
m.uv_coords = cv2.perspectiveTransform(m.uv_coords, transform)
def marker_status(self):
return "%s %s/%s" % (self.name, self.detected_markers,
len(self.markers))
def gl_draw_frame(self, img_size):
"""
draw surface and markers
"""
if self.detected:
frame = np.array([[[0, 0], [1, 0], [1, 1], [0, 1], [0, 0]]],
dtype=np.float32)
hat = np.array([[[.3, .7], [.7, .7], [.5, .9], [.3, .7]]],
dtype=np.float32)
hat = cv2.perspectiveTransform(hat, self.m_to_screen)
frame = cv2.perspectiveTransform(frame, self.m_to_screen)
alpha = min(1, self.build_up_status / self.required_build_up)
draw_polyline_norm(frame.reshape((5, 2)), 1,
RGBA(1.0, 0.2, 0.6, alpha))
draw_polyline_norm(hat.reshape((4, 2)), 1,
RGBA(1.0, 0.2, 0.6, alpha))
draw_points_norm(frame.reshape((5, -1))[0:1])
text_anchor = frame.reshape((5, -1))[2]
text_anchor[1] = 1 - text_anchor[1]
text_anchor *= img_size[1], img_size[0]
self.glfont.draw_text(text_anchor[0], text_anchor[1],
self.marker_status())
def gl_draw_corners(self):
"""
draw surface and markers
"""
if self.detected:
frame = np.array([[[0, 0], [1, 0], [1, 1], [0, 1]]],
dtype=np.float32)
frame = cv2.perspectiveTransform(frame, self.m_to_screen)
draw_points_norm(frame.reshape((4, 2)), 15,
RGBA(1.0, 0.2, 0.6, .5))
#### fns to draw surface in separate window
def gl_display_in_window(self, world_tex_id):
"""
here we map a selected surface onto a seperate window.
"""
if self._window and self.detected:
active_window = glfwGetCurrentContext()
glfwMakeContextCurrent(self._window)
clear_gl_screen()
# cv uses 3x3 gl uses 4x4 tranformation matricies
m = cvmat_to_glmat(self.m_from_screen)
glMatrixMode(GL_PROJECTION)
glPushMatrix()
glLoadIdentity()
glOrtho(0, 1, 0, 1, -1, 1) # gl coord convention
glMatrixMode(GL_MODELVIEW)
glPushMatrix()
#apply m to our quad - this will stretch the quad such that the ref suface will span the window extends
glLoadMatrixf(m)
draw_named_texture(world_tex_id)
glMatrixMode(GL_PROJECTION)
glPopMatrix()
glMatrixMode(GL_MODELVIEW)
glPopMatrix()
# now lets get recent pupil positions on this surface:
draw_points_norm(self.gaze_on_srf,
color=RGBA(0., 8., .5, .8),
size=80)
glfwSwapBuffers(self._window)
glfwMakeContextCurrent(active_window)
if self.window_should_close:
self.close_window()
#### fns to draw surface in separate window
def gl_display_in_window_3d(self, world_tex_id, camera_intrinsics):
"""
here we map a selected surface onto a seperate window.
"""
K, dist_coef, img_size = camera_intrinsics
if self._window and self.detected:
active_window = glfwGetCurrentContext()
glfwMakeContextCurrent(self._window)
glClearColor(.8, .8, .8, 1.)
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
glClearDepth(1.0)
glDepthFunc(GL_LESS)
glEnable(GL_DEPTH_TEST)
self.trackball.push()
glMatrixMode(GL_MODELVIEW)
draw_coordinate_system(l=self.real_world_size['x'])
glPushMatrix()
glScalef(self.real_world_size['x'], self.real_world_size['y'], 1)
draw_gl_polyline([[0, 0], [0, 1], [1, 1], [1, 0]],
color=RGBA(.5, .3, .1, .5),
thickness=3)
glPopMatrix()
# Draw the world window as projected onto the plane using the homography mapping
glPushMatrix()
glScalef(self.real_world_size['x'], self.real_world_size['y'], 1)
# cv uses 3x3 gl uses 4x4 tranformation matricies
m = cvmat_to_glmat(self.m_from_screen)
glMultMatrixf(m)
glTranslatef(0, 0, -.01)
draw_named_texture(world_tex_id)
draw_gl_polyline([[0, 0], [0, 1], [1, 1], [1, 0]],
color=RGBA(.5, .3, .6, .5),
thickness=3)
glPopMatrix()
# Draw the camera frustum and origin using the 3d tranformation obtained from solvepnp
glPushMatrix()
glMultMatrixf(self.camera_pose_3d.T.flatten())
draw_frustum(self.img_size, K, 150)
glLineWidth(1)
draw_frustum(self.img_size, K, .1)
draw_coordinate_system(l=5)
glPopMatrix()
self.trackball.pop()
glfwSwapBuffers(self._window)
glfwMakeContextCurrent(active_window)
def open_window(self):
if not self._window:
if self.fullscreen:
monitor = glfwGetMonitors()[self.monitor_idx]
mode = glfwGetVideoMode(monitor)
height, width = mode[0], mode[1]
else:
monitor = None
height, width = 640, int(
640. /
(self.real_world_size['x'] / self.real_world_size['y'])
) #open with same aspect ratio as surface
self._window = glfwCreateWindow(height,
width,
"Reference Surface: " + self.name,
monitor=monitor,
share=glfwGetCurrentContext())
if not self.fullscreen:
glfwSetWindowPos(self._window, 200, 0)
self.trackball = Trackball()
self.input = {'down': False, 'mouse': (0, 0)}
self.on_resize(self._window, height, width)
#Register callbacks
glfwSetWindowSizeCallback(self._window, self.on_resize)
glfwSetKeyCallback(self._window, self.on_key)
glfwSetWindowCloseCallback(self._window, self.on_close)
glfwSetMouseButtonCallback(self._window, self.on_button)
glfwSetCursorPosCallback(self._window, self.on_pos)
glfwSetScrollCallback(self._window, self.on_scroll)
# gl_state settings
active_window = glfwGetCurrentContext()
glfwMakeContextCurrent(self._window)
basic_gl_setup()
make_coord_system_norm_based()
# refresh speed settings
glfwSwapInterval(0)
glfwMakeContextCurrent(active_window)
def close_window(self):
if self._window:
glfwDestroyWindow(self._window)
self._window = None
self.window_should_close = False
def open_close_window(self):
if self._window:
self.close_window()
else:
self.open_window()
# window calbacks
def on_resize(self, window, w, h):
self.trackball.set_window_size(w, h)
active_window = glfwGetCurrentContext()
glfwMakeContextCurrent(window)
adjust_gl_view(w, h)
glfwMakeContextCurrent(active_window)
def on_key(self, window, key, scancode, action, mods):
if action == GLFW_PRESS:
if key == GLFW_KEY_ESCAPE:
self.on_close()
def on_close(self, window=None):
self.window_should_close = True
def on_button(self, window, button, action, mods):
if action == GLFW_PRESS:
self.input['down'] = True
self.input['mouse'] = glfwGetCursorPos(window)
if action == GLFW_RELEASE:
self.input['down'] = False
def on_pos(self, window, x, y):
if self.input['down']:
old_x, old_y = self.input['mouse']
self.trackball.drag_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 cleanup(self):
if self._window:
self.close_window()
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