Ejemplo n.º 1
0
	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
Ejemplo n.º 2
0
	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
Ejemplo n.º 3
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)
Ejemplo n.º 4
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,
                    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
Ejemplo n.º 6
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)
Ejemplo n.º 7
0
	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
Ejemplo n.º 8
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)
Ejemplo n.º 9
0
    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
Ejemplo n.º 10
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)
Ejemplo n.º 11
0
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()
Ejemplo n.º 12
0
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
Ejemplo n.º 13
0
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()
Ejemplo n.º 14
0
class Calibration_Visualizer(Visualizer):
    def __init__(self,
                 g_pool,
                 world_camera_intrinsics,
                 cal_ref_points_3d,
                 cal_observed_points_3d,
                 eye_camera_to_world_matrix0,
                 cal_gaze_points0_3d,
                 eye_camera_to_world_matrix1=np.eye(4),
                 cal_gaze_points1_3d=[],
                 run_independently=False,
                 name="Calibration Visualizer"):
        super().__init__(g_pool, name, run_independently)

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

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

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

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

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

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

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

        if not self.window:
            return

        self.begin_update_window()  #sets context

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

        glMatrixMode(GL_MODELVIEW)

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

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

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

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

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

        glPopMatrix()

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

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

            #everything else is in world coordinates

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

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

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

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

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

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

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

            #everything else is in world coordinates

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

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

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

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

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

        self.trackball.pop()

        self.end_update_window()  #swap buffers, handle context

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

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

    def on_scroll(self, window, x, y):
        self.trackball.zoom_to(y)
Ejemplo n.º 15
0
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()
Ejemplo n.º 16
0
class Eye_Visualizer(Visualizer):
    def __init__(self, g_pool, focal_length):
        super().__init__(g_pool, "Debug Visualizer", False)

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

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

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

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

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

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

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

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

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

    def draw_eye(self, result):

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

    def draw_debug_info(self, result):

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

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

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

    def draw_residuals(self, result):

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

        glMatrixMode(GL_MODELVIEW)
        glPushMatrix()
        glLoadIdentity()

        glTranslatef(0.01, 0.01, 0)

        glScale(1.5, 1.5, 1)

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

        glTranslatef(0.01, 0.01, 0)

        glScale(0.13, 0.13, 1)

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

        glScale(1, 0.33, 1)

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

        try:

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

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

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

        except:

            pass

        glPopMatrix()
        glMatrixMode(GL_PROJECTION)
        glPopMatrix()

    def draw_Dierkes_lines(self, result):

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

    def update_window(self, g_pool, result):

        if not result:
            return

        if not self.window:
            return

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

        glLoadMatrixf(self.get_image_space_matrix(15))

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

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

        self.eye.pose = self.get_anthropomorphic_matrix()

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

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

        self.end_update_window()

        return True

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

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

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

    def on_scroll(self, window, x, y):
        self.trackball.zoom_to(y)
Ejemplo n.º 17
0
class Eye_Visualizer(Visualizer):
	def __init__(self,g_pool , focal_length ):
		super(Eye_Visualizer, self).__init__(g_pool , "Debug Visualizer", False)

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

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

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

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


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

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

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

			float transform[16];

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

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

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

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

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

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

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

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


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

			delta_y += 160

		self.glfont.pop_state()

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

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

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

	def update_window(self, g_pool, result ):

		if not result:
			return

		if not self.window:
			return

		self.begin_update_window()

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

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

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

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

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

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

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

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

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

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

			model_count += 1


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

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

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

		self.trackball.pop()

		self.draw_debug_info(result)

		self.end_update_window()

		return True


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

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


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

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

		self.eye_to_world_matrix0 = eye_to_world_matrix0
		self.eye_to_world_matrix1 = eye_to_world_matrix1

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

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

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

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

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

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

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

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

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

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

			float transform[16];

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

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

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

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

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

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

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

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

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

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

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

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

		glPopMatrix()


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

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

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



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

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

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

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


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

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

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

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

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

			# self.gui = ui.UI()

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

			active_window = glfwGetCurrentContext()
			glfwMakeContextCurrent(self.window)

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

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

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

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

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

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


			glPopMatrix()

			if sphere0:

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

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

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

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

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

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

				glPopMatrix()

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


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

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

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

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

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

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

				glPopMatrix()


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


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


			self.trackball.pop()


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

	def close_window(self):
		if self.window:
			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)
Ejemplo n.º 20
0
class Eye_Visualizer(Visualizer):
    def __init__(self, g_pool, focal_length):
        super().__init__(g_pool, "Debug Visualizer", False)

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

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

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

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

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

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

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

            float transform[16];

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

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

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

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

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

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

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

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

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

            delta_y += 160

        self.glfont.pop_state()

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

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

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

    def update_window(self, g_pool, result):

        if not result:
            return

        if not self.window:
            return

        self.begin_update_window()

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

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

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

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

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

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

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

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

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

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

            model_count += 1

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

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

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

        self.trackball.pop()

        self.draw_debug_info(result)

        self.end_update_window()

        return True

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

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

    def on_scroll(self, window, x, y):
        self.trackball.zoom_to(y)
Ejemplo n.º 21
0
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()
Ejemplo n.º 22
0
class Calibration_Visualizer(object):
	def __init__(self, g_pool, world_camera_intrinsics , cal_ref_points_3d, eye_to_world_matrix0 , cal_gaze_points0_3d, eye_to_world_matrix1 = np.eye(4) , cal_gaze_points1_3d = [],   name = "Debug Calibration Visualizer", run_independently = False):
       # super(Visualizer, self).__init__()

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

		self.eye_to_world_matrix0 = eye_to_world_matrix0
		self.eye_to_world_matrix1 = eye_to_world_matrix1

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

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

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

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

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

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

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

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

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

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

			float transform[16];

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

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

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

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

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

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

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

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

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

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

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

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

		glPopMatrix()


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

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

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



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

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

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

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


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

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

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

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

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

			# self.gui = ui.UI()

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

			active_window = glfwGetCurrentContext()
			glfwMakeContextCurrent(self.window)

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

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

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

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

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

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


			glPopMatrix()

			if sphere0:

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

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

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

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

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

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

				glPopMatrix()

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


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

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

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

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

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

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

				glPopMatrix()


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


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


			self.trackball.pop()


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

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

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

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

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

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

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

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

	def on_iconify(self,window,iconified): pass

	def on_key(self,window, key, scancode, action, mods): pass