Exemple #1
0
    def viewer_refresh(self, q):
        gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
        gl.glClearColor(1.0, 1.0, 1.0, 1.0)
        self.dcam.Activate(self.scam)

        # Draw Camera
        gl.glLineWidth(3)
        gl.glColor3f(0.0, 0.0, 1.0)
        pangolin.DrawCamera(self.cameras[self.cam_id], 2, 0.75, 2)

        # Draw previous positions
        prev_positions = []
        gl.glPointSize(1)
        gl.glLineWidth(1)
        gl.glColor3f(0.8, 0.8, 0.7)
        for i in range(0, self.cam_id + 1):
            prev_positions.append(self.cameras[i][:3, 3])
        pangolin.DrawPoints(np.vstack(prev_positions))
        pangolin.DrawLine(np.vstack(prev_positions))

        self.cam_id = (self.cam_id + 1) % len(self.cameras)

        # Draw lines
        gl.glLineWidth(3)
        gl.glPointSize(6)
        colors = [(0.7, 0, 0), (0, 0.7, 0), (0.7, 0.7, 0)]
        for i in range(3):
            gl.glColor3f(*colors[i])
            pangolin.DrawPoints(P[i * 4:i * 4 + 4])
            for j, k in [(0, 1), (2, 3), (0, 2), (1, 3)]:
                pangolin.DrawLine(self.points[[i * 4 + j, i * 4 + k], :])

        pangolin.FinishFrame()
Exemple #2
0
 def handler_bboxes2d(boxes_2d, attrs):
     if boxes_2d is None:
         return
     if attrs:
         gl.glColor3f(*attrs['Color'])
     else:
         gl.glColor3f(0.0, 0.0, 1.0)
     z = attrs.get('z', 0.0)
     for b in boxes_2d:
         b2d = b[0:8].reshape(4, 2)
         b3d = []
         for pt in b2d:
             x, y = pt
             b3d.append([x, y, z])
         b3d.append([
             0.5 * (b2d[0, 0] + b2d[1, 0]), 0.5 * (b2d[0, 1] + b2d[1, 1]), z
         ])
         b3d_to_draw = []
         to_draw_idx = [3, 0, 1, 2, 4, 3, 2]
         for di in to_draw_idx:
             b3d_to_draw.append(b3d[di])
         i_cls = int(b[9]) if int(b[9]) <= 5 else 5
         cls = VisualizerPangoV2.CLS2COLOR[i_cls]
         gl.glColor3f(*cls)
         pangolin.DrawLine(b3d_to_draw,
                           point_size=int(attrs.get('PointSize', 5)))
         gl.glColor3f(*attrs['Color'])
Exemple #3
0
def t_draw2(self):
    trajectory = [[0, -6, 6]]
    for i in range(300):
        trajectory.append(trajectory[-1] + np.random.random(3) - 0.5)
    trajectory = np.array(trajectory)

    while not pangolin.ShouldQuit():
        gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
        gl.glClearColor(1.0, 1.0, 1.0, 1.0)
        self._dcam.Activate(self._scam)

        # Render OpenGL Cube
        pangolin.glDrawColouredCube(0.1)

        # Draw Point Cloud
        points = np.random.random((10000, 3)) * 3 - 4
        gl.glPointSize(1)
        gl.glColor3f(1.0, 0.0, 0.0)
        pangolin.DrawPoints(points)

        # Draw Point Cloud
        points = np.random.random((10000, 3))
        colors = np.zeros((len(points), 3))
        colors[:, 1] = 1 - points[:, 0]
        colors[:, 2] = 1 - points[:, 1]
        colors[:, 0] = 1 - points[:, 2]
        points = points * 3 + 1
        gl.glPointSize(1)
        pangolin.DrawPoints(points, colors)

        # Draw lines
        gl.glLineWidth(1)
        gl.glColor3f(0.0, 0.0, 0.0)
        pangolin.DrawLine(trajectory)  # consecutive
        gl.glColor3f(0.0, 1.0, 0.0)
        pangolin.DrawLines(trajectory,
                           trajectory + np.random.randn(len(trajectory), 3),
                           point_size=5)  # separate

        # Draw camera
        pose = np.identity(4)
        pose[:3, 3] = np.random.randn(3)
        gl.glLineWidth(1)
        gl.glColor3f(0.0, 0.0, 1.0)
        pangolin.DrawCamera(pose, 0.5, 0.75, 0.8)

        # Draw boxes
        poses = [np.identity(4) for i in range(10)]
        for pose in poses:
            pose[:3, 3] = np.random.randn(3) + np.array([5, -3, 0])
        sizes = np.random.random((len(poses), 3))
        gl.glLineWidth(1)
        gl.glColor3f(1.0, 0.0, 1.0)
        pangolin.DrawBoxes(poses, sizes)

        pangolin.FinishFrame()
Exemple #4
0
 def handler_lines(lines, attrs):
     if lines is None:
         return
     if attrs:
         gl.glColor3f(*attrs['Color'])
         point_size = attrs['PointSize']
     else:
         gl.glColor3f(0.0, 0.0, 1.0)
         point_size = 5.0
     pangolin.DrawLine(lines, point_size=point_size)
Exemple #5
0
    def draw(self, poseList_nx4x4, drawCamera=False):
        while not pangolin.ShouldQuit():
            gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
            gl.glClearColor(1.0, 1.0, 1.0, 1.0)
            self._dcam.Activate(self._scam)

            # pangolin.glDrawColouredCube()
            # points = np.random.random((10000, 3)) *10

            gl.glPointSize(2)
            gl.glColor3f(1.0, 0.0, 0.0)
            pangolin.DrawLine(np.array([[0, 0, 0], [1.0, 0.0, 0.0]]))
            gl.glColor3f(0.0, 1.0, 0.0)
            pangolin.DrawLine(np.array([[0, 0, 0], [0.0, 1.0, 0.0]]))
            gl.glColor3f(.0, 0.0, 1.0)
            pangolin.DrawLine(np.array([[0, 0, 0], [0.0, 0.0, 1.0]]))

            origin = [[0, 0, 0]]
            originHomo = h**o(np.zeros((3, 1)))
            originT = np.eye(4)

            # pangolin.Draw
            index = 0
            for T in poseList_nx4x4:
                originT = originT.dot(T)
                if drawCamera:
                    gl.glColor3f(0.5, 0.75, 0.8)
                    pangolin.DrawCamera(originT)
                pAfter = originT.dot(originHomo)
                p = unHomo(pAfter)
                origin.append([p[0][0], p[1][0], p[2][0]])
                # line

                if np.mod(index, 2) == 0:
                    gl.glColor3f(0.0, 0.0, 0.0)
                else:
                    gl.glColor3f(0.0, 1.0, 0.0)
                pangolin.DrawLine(origin)
                index += 1
                # time.sleep(0.01)
            pangolin.FinishFrame()
Exemple #6
0
def work(q, qclose, w=960, h=540):
    pangolin.CreateWindowAndBind('pangolin', w, h)
    gl.glEnable(gl.GL_DEPTH_TEST)

    # Define Projection and initial ModelView matrix
    scam = pangolin.OpenGlRenderState(
        pangolin.ProjectionMatrix(w, h, 420, 420, w // 2, h // 2, 0.2, 10000),
        pangolin.ModelViewLookAt(-2, -2, -8, 0, 0, 0,
                                 pangolin.AxisDirection.AxisNegY))
    handler = pangolin.Handler3D(scam)

    # Create Interactive View in window
    dcam = pangolin.CreateDisplay()
    dcam.SetBounds(0.0, 1.0, 0.0, 1.0, -w / h)
    dcam.SetHandler(handler)

    pose = np.eye(4)
    opath = np.zeros((0, 3))
    opts = np.zeros((2, 3))
    colors = np.zeros((2, 3))

    while not pangolin.ShouldQuit():
        if not qclose.empty():
            if qclose.get():
                pangolin.Quit()

        if not q.empty():
            pose, opath, opts, colors = q.get()
            colors /= 256.0

        gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
        gl.glClearColor(0.0, 0.0, 0.0, 1.0)
        dcam.Activate(scam)

        #        draw_axes()

        # Draw optimized cloud
        gl.glPointSize(2)
        gl.glColor3f(0.5, 0.8, 0.5)
        pangolin.DrawPoints(opts, colors)

        # Draw camera
        gl.glLineWidth(1)
        gl.glColor3f(0.4, 0.4, 0.4)
        pangolin.DrawCamera(pose, 10, 1, 1)

        # Optimized path
        if len(opath) > 2:
            gl.glLineWidth(1)
            gl.glColor3f(0.4, 0.4, 0.4)
            pangolin.DrawLine(np.array(opath))

        pangolin.FinishFrame()
Exemple #7
0
def main():
    pangolin.CreateWindowAndBind('Main', 640, 480)
    gl.glEnable(gl.GL_DEPTH_TEST)

    # Define Projection and initial ModelView matrix
    scam = pangolin.OpenGlRenderState(
        pangolin.ProjectionMatrix(640, 480, 420, 420, 320, 240, 0.2, 200),
        pangolin.ModelViewLookAt(-2, 2, -2, 0, 0, 0,
                                 pangolin.AxisDirection.AxisY))
    handler = pangolin.Handler3D(scam)

    # Create Interactive View in window
    dcam = pangolin.CreateDisplay()
    dcam.SetBounds(0.0, 1.0, 0.0, 1.0, -640.0 / 480.0)
    dcam.SetHandler(handler)

    trajectory = [[0, -6, 6]]
    for i in range(300):
        trajectory.append(trajectory[-1] + np.random.random(3) - 0.5)
    trajectory = np.array(trajectory)

    while not pangolin.ShouldQuit():
        gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
        gl.glClearColor(1.0, 1.0, 1.0, 1.0)
        dcam.Activate(scam)

        # Render OpenGL Cube
        pangolin.glDrawColouredCube(0.1)

        # Draw Point Cloud
        points = np.random.random((10000, 3)) * 3 - 4
        gl.glPointSize(1)
        gl.glColor3f(1.0, 0.0, 0.0)
        pangolin.DrawPoints(points)

        # Draw Point Cloud
        points = np.random.random((10000, 3))
        colors = np.zeros((len(points), 3))
        colors[:, 1] = 1 - points[:, 0]
        colors[:, 2] = 1 - points[:, 1]
        colors[:, 0] = 1 - points[:, 2]
        points = points * 3 + 1

        print(points.shape)
        print(colors.shape)
        print(points[0:3])
        print(colors[0:3])
        gl.glPointSize(1)
        pangolin.DrawPoints(points, colors)

        # Draw lines
        gl.glLineWidth(1)
        gl.glColor3f(0.0, 0.0, 0.0)
        pangolin.DrawLine(trajectory)  # consecutive
        gl.glColor3f(0.0, 1.0, 0.0)
        pangolin.DrawLines(trajectory,
                           trajectory + np.random.randn(len(trajectory), 3),
                           point_size=5)  # separate

        # Draw camera
        pose = np.identity(4)
        pose[:3, 3] = np.ones(3)
        gl.glLineWidth(1)
        gl.glColor3f(0.0, 0.0, 1.0)
        pangolin.DrawCamera(pose, 0.5, 0.75, 0.8)

        # Draw boxes
        poses = [np.identity(4) for i in range(10)]
        for pose in poses:
            pose[:3, 3] = np.random.randn(3) + np.array([5, -3, 0])
        sizes = np.random.random((len(poses), 3))
        gl.glLineWidth(1)
        gl.glColor3f(1.0, 0.0, 1.0)
        pangolin.DrawBoxes(poses, sizes)

        pangolin.FinishFrame()
Exemple #8
0
 def draw_line(self, points, color=(1.0, 0.0, 0.0)):
     self.dcam.Activate(self.scam)
     gl.glLineWidth(2)
     gl.glColor3f(*color)
     pango.DrawLine(points)
Exemple #9
0
    def viewer_refresh(self, qmap, qvo, is_paused):

        while not qmap.empty():
            self.map_state = qmap.get()

        while not qvo.empty():
            self.vo_state = qvo.get()

        # if pangolin.Pushed(self.button):
        #    print('You Pushed a button!')

        self.do_follow = self.checkboxFollow.Get()
        self.is_grid = self.checkboxGrid.Get()        
        self.draw_cameras = self.checkboxCams.Get()
        self.draw_covisibility = self.checkboxCovisibility.Get()
        self.draw_spanning_tree = self.checkboxSpanningTree.Get()
        
        #if pangolin.Pushed(self.checkboxPause):
        if self.checkboxPause.Get():
            is_paused.value = 0  
        else:
            is_paused.value = 1  
                    
        # self.int_slider.SetVal(int(self.float_slider))
        self.pointSize = self.int_slider.Get()
            
        if self.do_follow and self.is_following:
            self.scam.Follow(self.Twc, True)
        elif self.do_follow and not self.is_following:
            self.scam.SetModelViewMatrix(self.look_view)
            self.scam.Follow(self.Twc, True)
            self.is_following = True
        elif not self.do_follow and self.is_following:
            self.is_following = False            

        gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
        gl.glClearColor(1.0, 1.0, 1.0, 1.0)
        
        self.dcam.Activate(self.scam)

        if self.is_grid:
            Viewer3D.drawPlane()

        # ==============================
        # draw map 
        if self.map_state is not None:
            if self.map_state.cur_pose is not None:
                # draw current pose in blue
                gl.glColor3f(0.0, 0.0, 1.0)
                gl.glLineWidth(2)                
                pangolin.DrawCamera(self.map_state.cur_pose)
                gl.glLineWidth(1)                
                self.updateTwc(self.map_state.cur_pose)
                
            if self.map_state.predicted_pose is not None and kDrawCameraPrediction:
                # draw predicted pose in red
                gl.glColor3f(1.0, 0.0, 0.0)
                pangolin.DrawCamera(self.map_state.predicted_pose)           
                
            if len(self.map_state.poses) >1:
                # draw keyframe poses in green
                if self.draw_cameras:
                    gl.glColor3f(0.0, 1.0, 0.0)
                    pangolin.DrawCameras(self.map_state.poses[:])

            if len(self.map_state.points)>0:
                # draw keypoints with their color
                gl.glPointSize(self.pointSize)
                #gl.glColor3f(1.0, 0.0, 0.0)
                pangolin.DrawPoints(self.map_state.points, self.map_state.colors)    
                
            if self.map_state.reference_pose is not None and kDrawReferenceCamera:
                # draw predicted pose in purple
                gl.glColor3f(0.5, 0.0, 0.5)
                gl.glLineWidth(2)                
                pangolin.DrawCamera(self.map_state.reference_pose)      
                gl.glLineWidth(1)          
                
            if len(self.map_state.covisibility_graph)>0:
                if self.draw_covisibility:
                    gl.glLineWidth(1)
                    gl.glColor3f(0.0, 1.0, 0.0)
                    pangolin.DrawLines(self.map_state.covisibility_graph,3)                                             
                    
            if len(self.map_state.spanning_tree)>0:
                if self.draw_spanning_tree:
                    gl.glLineWidth(1)
                    gl.glColor3f(0.0, 0.0, 1.0)
                    pangolin.DrawLines(self.map_state.spanning_tree,3)              
                    
            if len(self.map_state.loops)>0:
                if self.draw_spanning_tree:
                    gl.glLineWidth(2)
                    gl.glColor3f(0.5, 0.0, 0.5)
                    pangolin.DrawLines(self.map_state.loops,3)        
                    gl.glLineWidth(1)                                               

        # ==============================
        # draw vo 
        if self.vo_state is not None:
            if self.vo_state.poses.shape[0] >= 2:
                # draw poses in green
                if self.draw_cameras:
                    gl.glColor3f(0.0, 1.0, 0.0)
                    pangolin.DrawCameras(self.vo_state.poses[:-1])

            if self.vo_state.poses.shape[0] >= 1:
                # draw current pose in blue
                gl.glColor3f(0.0, 0.0, 1.0)
                current_pose = self.vo_state.poses[-1:]
                pangolin.DrawCameras(current_pose)
                self.updateTwc(current_pose[0])

            if self.vo_state.traj3d_est.shape[0] != 0:
                # draw blue estimated trajectory 
                gl.glPointSize(self.pointSize)
                gl.glColor3f(0.0, 0.0, 1.0)
                pangolin.DrawLine(self.vo_state.traj3d_est)

            if self.vo_state.traj3d_gt.shape[0] != 0:
                # draw red ground-truth trajectory 
                gl.glPointSize(self.pointSize)
                gl.glColor3f(1.0, 0.0, 0.0)
                pangolin.DrawLine(self.vo_state.traj3d_gt)                


        pangolin.FinishFrame()
Exemple #10
0
    def viewer_refresh(self, qmap, qvo):

        gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
        gl.glClearColor(1.0, 1.0, 1.0, 1.0)

        while not qmap.empty():
            self.state_map = qmap.get()

        while not qvo.empty():
            self.state_vo = qvo.get()

        # if pangolin.Pushed(self.button):
        #    print('You Pushed a button!')

        self.do_follow = self.checkboxFollow.Get()
        self.is_grid = self.checkboxGrid.Get()
        self.draw_cameras = self.checkboxCams.Get()
        # self.int_slider.SetVal(int(self.float_slider))
        self.pointSize = self.int_slider.Get()

        self.dcam.Activate(self.scam)
        if self.do_follow:
            self.scam.Follow(self.Twc, True)

        if self.is_grid:
            Viewer3D.drawPlane()

        # draw map
        if self.state_map is not None:
            if self.state_map[0].shape[0] >= 2:
                # draw poses in green
                if self.draw_cameras:
                    gl.glColor3f(0.0, 1.0, 0.0)
                    pangolin.DrawCameras(self.state_map[0][:-1])

            if self.state_map[0].shape[0] >= 1:
                # draw current pose in blue
                gl.glColor3f(0.0, 0.0, 1.0)
                currentPose = self.state_map[0][-1:]
                pangolin.DrawCameras(currentPose)
                self.updateTwc(currentPose)

            if self.state_map[1].shape[0] != 0:
                # draw keypoints with their color
                gl.glPointSize(self.pointSize)
                #gl.glColor3f(1.0, 0.0, 0.0)
                pangolin.DrawPoints(self.state_map[1], self.state_map[2])

    # draw vo
        if self.state_vo is not None:
            if self.state_vo[0].shape[0] >= 2:
                # draw poses in green
                if self.draw_cameras:
                    gl.glColor3f(0.0, 1.0, 0.0)
                    pangolin.DrawCameras(self.state_vo[0][:-1])

            if self.state_vo[0].shape[0] >= 1:
                # draw current pose in blue
                gl.glColor3f(0.0, 0.0, 1.0)
                currentPose = self.state_vo[0][-1:]
                pangolin.DrawCameras(currentPose)
                self.updateTwc(currentPose)

            if self.state_vo[1].shape[0] != 0:
                # draw blue estimated trajectory
                gl.glPointSize(self.pointSize)
                gl.glColor3f(0.0, 0.0, 1.0)
                pangolin.DrawLine(self.state_vo[1])

            if self.state_vo[2].shape[0] != 0:
                # draw red ground-truth trajectory
                gl.glPointSize(self.pointSize)
                gl.glColor3f(1.0, 0.0, 0.0)
                pangolin.DrawLine(self.state_vo[2])

        pangolin.FinishFrame()
Exemple #11
0
    def view(self):
        pangolin.CreateWindowAndBind('Viewer', 1024, 768)
        gl.glEnable(gl.GL_DEPTH_TEST)
        gl.glEnable(gl.GL_BLEND)
        gl.glBlendFunc(gl.GL_SRC_ALPHA, gl.GL_ONE_MINUS_SRC_ALPHA)
        viewpoint_x = 0
        viewpoint_y = -5  # -10
        viewpoint_z = -10  # -0.1
        viewpoint_f = 200
        camera_width = 0.5

        proj = pangolin.ProjectionMatrix(1024, 768, viewpoint_f, viewpoint_f,
                                         512, 389, 0.1, 5000)
        look_view = pangolin.ModelViewLookAt(viewpoint_x, viewpoint_y,
                                             viewpoint_z, 0, 0, 0, 0, -1, 0)

        scam = pangolin.OpenGlRenderState(proj, look_view)

        dcam = pangolin.CreateDisplay()
        dcam.SetBounds(0.0, 1.0, 0.0, 1.0, -1024. / 768.)
        dcam.SetHandler(pangolin.Handler3D(scam))

        dimg = pangolin.Display('image')
        dimg.SetBounds(0.0, self.h / 768., 0.0, self.w / 1024.,
                       float(self.w) / self.h)
        dimg.SetLock(pangolin.Lock.LockLeft, pangolin.Lock.LockTop)
        texture = pangolin.GlTexture(self.w, self.h, gl.GL_RGB, False, 0,
                                     gl.GL_RGB, gl.GL_UNSIGNED_BYTE)

        ddepth = pangolin.Display('depth')
        ddepth.SetBounds(self.h / 768., self.h / 768. * 2.0, 0.0,
                         self.w / 1024.,
                         float(self.w) / float(self.h))
        ddepth.SetLock(pangolin.Lock.LockLeft, pangolin.Lock.LockTop)

        if self.use_custom_depth_image:
            print("Use RGB depth buffer")
            texture_depth = pangolin.GlTexture(self.w, self.h, gl.GL_RGB,
                                               False, 0, gl.GL_RGB,
                                               gl.GL_UNSIGNED_BYTE)
        else:
            texture_depth = pangolin.GlTexture(self.w, self.h, gl.GL_LUMINANCE,
                                               False, 0, gl.GL_LUMINANCE,
                                               gl.GL_UNSIGNED_BYTE)

        cameras = []
        trajectory = []
        pose = pangolin.OpenGlMatrix()
        points = np.empty(shape=(0, 3))
        colors = np.empty(shape=(0, 3))
        # image = random_image(self.w, self.h)
        image = 255 * np.ones((self.h, self.w, 3), 'uint8')
        if self.use_custom_depth_image:
            depth = 255 * np.ones((self.h, self.w, 3), 'uint8')
        else:
            depth = 255 * np.ones((self.h, self.w), 'uint8')

        gl.glPointSize(3)
        gl.glLineWidth(2)
        while not pangolin.ShouldQuit():
            if not self.q_camera.empty():
                cameras = self.q_camera.get()

            gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
            gl.glClearColor(0.75, 0.75, 0.75, 1.0)
            if not self.q_pose.empty():
                pose.m = self.q_pose.get()
            # scam.Follow(pose, True)

            dcam.Activate(scam)

            gl.glColor3f(0.0, 0.0, 1.0)
            if len(cameras) > 0:
                pangolin.DrawCameras(cameras, camera_width)

            if not self.q_trajectory.empty():
                trajectory = self.q_trajectory.get()
            if len(trajectory) > 1:
                gl.glColor3f(0.0, 0.0, 0.0)
                pangolin.DrawLine(trajectory)

            if not self.q_point.empty():
                points = self.q_point.get()

            if not self.q_color.empty():
                colors = self.q_color.get()
            # if len(points) > 0:
            # pangolin.DrawPoints(points, colors)

            if not self.q_image.empty():
                image = self.q_image.get()
                # print(image.shape, image.dtype)

            texture.Upload(image, gl.GL_RGB, gl.GL_UNSIGNED_BYTE)
            dimg.Activate()
            gl.glColor3f(1.0, 1.0, 1.0)
            texture.RenderToViewport()

            if not self.q_depth.empty():
                depth = self.q_depth.get()
                print('^^^')
                print(depth.shape, depth.dtype)

            if self.use_custom_depth_image:
                texture_depth.Upload(depth, gl.GL_RGB, gl.GL_UNSIGNED_BYTE)
            else:
                texture_depth.Upload(depth, gl.GL_LUMINANCE,
                                     gl.GL_UNSIGNED_BYTE)
            ddepth.Activate()
            gl.glColor3f(1.0, 1.0, 1.0)
            texture_depth.RenderToViewport()

            pangolin.FinishFrame()
Exemple #12
0
    def viewer_refresh(self, qmap, qvo, is_paused):

        while not qmap.empty():
            self.map_state = qmap.get()

        while not qvo.empty():
            self.vo_state = qvo.get()

        # if pangolin.Pushed(self.button):
        #    print('You Pushed a button!')

        self.do_follow = self.checkboxFollow.Get()
        self.is_grid = self.checkboxGrid.Get()
        self.draw_cameras = self.checkboxCams.Get()
        self.draw_covisibility = self.checkboxCovisibility.Get()
        self.draw_spanning_tree = self.checkboxSpanningTree.Get()

        #if pangolin.Pushed(self.checkboxPause):
        if self.checkboxPause.Get():
            is_paused.value = 0
        else:
            is_paused.value = 1

        # self.int_slider.SetVal(int(self.float_slider))
        self.pointSize = self.int_slider.Get()

        if self.do_follow and self.is_following:
            self.scam.Follow(self.Twc, True)
        elif self.do_follow and not self.is_following:
            self.scam.SetModelViewMatrix(self.look_view)
            self.scam.Follow(self.Twc, True)
            self.is_following = True
        elif not self.do_follow and self.is_following:
            self.is_following = False

        gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT)
        gl.glClearColor(1.0, 1.0, 1.0, 1.0)

        self.dcam.Activate(self.scam)

        if self.is_grid:
            Viewer3D.drawPlane()

        # ==============================
        # draw map
        if self.map_state is not None:
            if self.map_state.cur_pose is not None:
                # draw current pose in blue
                gl.glColor3f(0.0, 0.0, 1.0)
                gl.glLineWidth(2)
                pangolin.DrawCamera(self.map_state.cur_pose)
                gl.glLineWidth(1)
                self.updateTwc(self.map_state.cur_pose)

            if self.map_state.predicted_pose is not None and kDrawCameraPrediction:
                # draw predicted pose in red
                gl.glColor3f(1.0, 0.0, 0.0)
                pangolin.DrawCamera(self.map_state.predicted_pose)

            if len(self.map_state.poses) > 1:
                # draw keyframe poses in green
                if self.draw_cameras:
                    gl.glColor3f(0.0, 1.0, 0.0)
                    pangolin.DrawCameras(self.map_state.poses[:])

            # rysowaine wektora za którym powinno wyznaczać boxy
            if len(self.map_state.line) > 0:
                #     # wyznaczam nowy wektor obrócony o -90 i 90 stopni o długości 10
                gl.glLineWidth(3)
                gl.glPointSize(self.pointSize)
                gl.glColor3f(0.0, 0.0, 0.0)
                pangolin.DrawPoints(self.map_state.line)
            #     # pangolin.DrawLines([self.map_state.line[0], self.map_state.line[0]])

            if len(self.map_state.points) > 0:
                # draw keypoints with their color
                gl.glPointSize(self.pointSize)
                #gl.glColor3f(1.0, 0.0, 0.0)
                pangolin.DrawPoints(self.map_state.points,
                                    self.map_state.colors)

            # TODO dopisać rozwiązanie drukujące na ekran zaznaczone obiekty w postaci obszarów
            if len(self.map_state.dict_pose) > 0:
                for k, v in self.map_state.dict_pose.items():
                    sizes = []
                    poses = [np.identity(4) for i in range(len(v))]
                    for pose, point in zip(poses, v):
                        pose[:3, 3] = np.array(point[0])
                        sizes.append(np.array(point[1]))
                    gl.glLineWidth(2)
                    d_color = k[1:-1].split(' ')
                    gl.glColor3f(float(d_color[0]), float(d_color[1]),
                                 float(d_color[2]))
                    # gl.glColor3f(1.0, 0.0, 1.0)
                    # gl.glColor3f(self.map_state.box_color)
                    #     print("{} {}".format(poses, sizes))
                    pangolin.DrawBoxes(poses, sizes)
                # pangolin.DrawBoxes(self.map_state.box_left_botton, self.map_state.box_size)

            if self.map_state.reference_pose is not None and kDrawReferenceCamera:
                # draw predicted pose in purple
                gl.glColor3f(0.5, 0.0, 0.5)
                gl.glLineWidth(2)
                pangolin.DrawCamera(self.map_state.reference_pose)
                gl.glLineWidth(1)

            if len(self.map_state.covisibility_graph) > 0:
                if self.draw_covisibility:
                    gl.glLineWidth(1)
                    gl.glColor3f(0.0, 1.0, 0.0)
                    pangolin.DrawLines(self.map_state.covisibility_graph, 3)

            if len(self.map_state.spanning_tree) > 0:
                if self.draw_spanning_tree:
                    gl.glLineWidth(1)
                    gl.glColor3f(0.0, 0.0, 1.0)
                    pangolin.DrawLines(self.map_state.spanning_tree, 3)

            if len(self.map_state.loops) > 0:
                if self.draw_spanning_tree:
                    gl.glLineWidth(2)
                    gl.glColor3f(0.5, 0.0, 0.5)
                    pangolin.DrawLines(self.map_state.loops, 3)
                    gl.glLineWidth(1)

        # ==============================
        # draw vo
        if self.vo_state is not None:
            if self.vo_state.poses.shape[0] >= 2:
                # draw poses in green
                if self.draw_cameras:
                    gl.glColor3f(0.0, 1.0, 0.0)
                    pangolin.DrawCameras(self.vo_state.poses[:-1])

            if self.vo_state.poses.shape[0] >= 1:
                # draw current pose in blue
                gl.glColor3f(0.0, 0.0, 1.0)
                current_pose = self.vo_state.poses[-1:]
                pangolin.DrawCameras(current_pose)
                self.updateTwc(current_pose[0])

            if self.vo_state.traj3d_est.shape[0] != 0:
                # draw blue estimated trajectory
                gl.glPointSize(self.pointSize)
                gl.glColor3f(0.0, 0.0, 1.0)
                pangolin.DrawLine(self.vo_state.traj3d_est)

            if self.vo_state.traj3d_gt.shape[0] != 0:
                # draw red ground-truth trajectory
                gl.glPointSize(self.pointSize)
                gl.glColor3f(1.0, 0.0, 0.0)
                pangolin.DrawLine(self.vo_state.traj3d_gt)

        pangolin.FinishFrame()