def main(): # Create OpenGL window in single line 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, 100), 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(pangolin.Attach(0.0), pangolin.Attach(1.0), pangolin.Attach(0.0), pangolin.Attach(1.0), -640.0 / 480.0) dcam.SetHandler(handler) # Data logger object log = pangolin.DataLog() # Optionally add named labels labels = ['sin(t)', 'cos(t)', 'sin(t)+cos(t)'] log.SetLabels(labels) # OpenGL 'view' of data. We might have many views of the same data. tinc = 0.03 plotter = pangolin.Plotter(log, 0.0, 6.0 * np.pi / tinc, -2.0, 2.0, np.pi / (6 * tinc), 0.5) plotter.SetBounds(pangolin.Attach(0.05), pangolin.Attach(0.3), pangolin.Attach(0.0), pangolin.Attach(0.4)) plotter.Track('$i') # Add some sample annotations to the plot plotter.AddMarker(pangolin.Marker.Vertical, -1000, pangolin.Marker.LessThan, pangolin.Colour.Blue().WithAlpha(0.2)) plotter.AddMarker(pangolin.Marker.Horizontal, 100, pangolin.Marker.GreaterThan, pangolin.Colour.Red().WithAlpha(0.2)) plotter.AddMarker(pangolin.Marker.Horizontal, 10, pangolin.Marker.Equal, pangolin.Colour.Green().WithAlpha(0.2)) pangolin.DisplayBase().AddDisplay(plotter) t = 0 while not pangolin.ShouldQuit(): gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT) # Plot line log.Log(np.sin(t), np.cos(t), np.sin(t) + np.cos(t)) t += tinc gl.glClearColor(1.0, 1.0, 1.0, 1.0) dcam.Activate(scam) # Render OpenGL 3D Cube pangolin.glDrawColouredCube() pangolin.FinishFrame()
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, 100), pangolin.ModelViewLookAt(-2, 2, -2, 0, 0, 0, pangolin.AxisY)) tree = pangolin.Renderable() tree.Add(pangolin.Axis()) # Create Interactive View in window handler = pangolin.SceneHandler(tree, scam) dcam = pangolin.CreateDisplay() dcam.SetBounds(0.0, 1.0, 0.0, 1.0, -640.0 / 480.0) dcam.SetHandler(handler) def draw(view): view.Activate(scam) tree.Render() dcam.SetDrawFunction(draw) while not pangolin.ShouldQuit(): gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT) # or # dcam.Activate(scam) # tree.Render() pangolin.FinishFrame()
def main(): win = pango.CreateWindowAndBind("pySimpleDisplay", 640, 480) glEnable(GL_DEPTH_TEST) pm = pango.ProjectionMatrix(640,480,420,420,320,240,0.1,1000); mv = pango.ModelViewLookAt(-0, 0.5, -3, 0, 0, 0, pango.AxisY) s_cam = pango.OpenGlRenderState(pm, mv) ui_width = 180 handler=pango.Handler3D(s_cam) d_cam = pango.CreateDisplay().SetBounds(pango.Attach(0), pango.Attach(1), pango.Attach.Pix(ui_width), pango.Attach(1), -640.0/480.0).SetHandler(handler) pango.CreatePanel("ui").SetBounds( pango.Attach(0), pango.Attach(1), pango.Attach(0), pango.Attach.Pix(ui_width)) var_ui=pango.Var("ui") var_ui.A_Button=False var_ui.B_Button=True var_ui.B_Double=1 var_ui.B_Str="sss" ctrl=-96 pango.RegisterKeyPressCallback(ctrl+ord('a'), a_callback) while not pango.ShouldQuit(): glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT) d_cam.Activate(s_cam) pango.glDrawColouredCube() pango.FinishFrame()
def main(): win = pango.CreateWindowAndBind("main py_pangolin", 640, 480) log = pango.DataLog() log.SetLabels(["sin(t)", "cos(t)", "sin(t)+cos(t)"]) t=0; tinc=0.01 plotter = pango.Plotter(log,0,4*math.pi/tinc,-2,2,math.pi/(4*tinc),0.5); plotter.Track("$i") plotter.AddMarker(pango.Marker.Vertical, -1000, pango.Marker.LessThan, pango.Colour.Blue().WithAlpha(0.2)) plotter.AddMarker(pango.Marker.Horizontal, 100, pango.Marker.GreaterThan, pango.Colour.Red().WithAlpha(0.2)) plotter.AddMarker(pango.Marker.Horizontal, 10, pango.Marker.Equal, pango.Colour.Green().WithAlpha(0.2)) plotter.SetBounds(pango.Attach(0), pango.Attach(1), pango.Attach(0), pango.Attach(1)) pango.DisplayBase().AddDisplay(plotter) while not pango.ShouldQuit(): glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT) log.Log(math.sin(t), math.cos(t), math.sin(t)+math.cos(t)) t+=tinc pango.FinishFrame()
def viewer_refresh(self, q): while not q.empty(): self.state = q.get() gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT) gl.glClearColor(0.0, 0.0, 0.0, 1.0) self.dcam.Activate(self.scam) if self.state is not None: if self.state[0].shape[0] >= 2: # draw poses gl.glColor3f(0.0, 1.0, 0.0) drawCameras(self.state[0][:-1]) if self.state[0].shape[0] >= 1: # draw current pose as yellow gl.glColor3f(1.0, 1.0, 0.0) drawCameras(self.state[0][-1:]) if self.state[1].shape[0] != 0: # draw keypoints gl.glPointSize(5) gl.glColor3f(1.0, 0.0, 0.0) drawPoints(self.state[1], self.state[2]) pangolin.FinishFrame()
def viewer_refresh(self, q): if self.state is None or not q.empty(): # while not q.empty(): self.state = q.get() # turn state into points ppts = np.array([d[:3, 3] for d in self.state[0]]) # spts = np.array(self.state[1]) gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT) gl.glClearColor(0.0, 0.0, 0.0, 1.0) self.dcam.Activate(self.scam) if self.state is not None: if self.state[0].shape[0] >= 2: # draw poses gl.glColor3f(0.0, 1.0, 0.0) # pangolin.DrawCameras(self.state[0][:-1]) drawPoints(ppts[:-1]) # drawCameras(self.state[0]) if self.state[0].shape[0] >= 1: # draw current pose as yellow gl.glColor3f(1.0, 1.0, 0.0) drawPoints(ppts[-1:]) # pangolin.DrawCameras(self.state[0][-1:]) if self.state[1].shape[0] != 0: # draw keypoints gl.glPointSize(5) gl.glColor3f(1.0, 0.0, 0.0) # pangolin.DrawPoints(self.state[1], self.state[2]) drawPoints(self.state[1]) #, self.state[2]) pangolin.FinishFrame()
def showPointCloud(pointcloud): if len(pointcloud) == 0: raise Exception("point cloud could not be empty!") pango.CreateWindowAndBind("pointcloud viewer", 1024, 768) glEnable(GL_DEPTH_TEST) glEnable(GL_BLEND) glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA) pm = pango.ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000) mv = pango.ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0) s_cam = pango.OpenGlRenderState(pm, mv) handler = pango.Handler3D(s_cam) d_cam = pango.CreateDisplay() d_cam.SetBounds(pango.Attach(0.0), pango.Attach(1.0), pango.Attach(0.0), pango.Attach(1.0), -1024 / 768) d_cam.SetHandler(handler) while not pango.ShouldQuit(): glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT) d_cam.Activate(s_cam) glClearColor(1.0, 1.0, 1.0, 1.0) glPointSize(2) glBegin(GL_POINTS) for p in pointcloud: glColor3d(p[3], p[3], p[3]) glVertex3d(p[0], p[1], p[2]) glEnd() pango.FinishFrame() time.sleep(0.005)
def viewer_refresh(self, q): #turn state into points if self.state is None or not q.empty(): self.state = q.get() # ppts = np.array([d[:3, 3] for d in self.state[0]]) # spts = np.array(self.state[1]) gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT) gl.glClearColor(0.0, 0.0, 0.0, 1.0) self.d_cam.Activate(self.scam) # draw poses # colors = np.zeros((len(ppts), 3)) # colors[:, 0] = 1 # colors[:, 1] = 1 # colors[:, 2] = 0 gl.glPointSize(10) gl.glColor3f(0.0, 0.0, 1.0) # pango.DrawPoints(ppts, colors) # pango.DrawPoints(self.state[0], self.state[2]) pango.DrawCameras(self.state[0]) # # draw keypoints # colors = np.zeros((len(spts), 3)) # colors[:, 0] = 0 # colors[:, 1] = 1 # colors[:, 2] = 0 gl.glPointSize(2) gl.glColor3f(0.0, 1.0, 0.0) pango.DrawPoints(self.state[1], self.state[2]) pango.FinishFrame()
def DrawTrajectory(poses): pango.CreateWindowAndBind("trajectory viewer", 1024, 768) glEnable(GL_DEPTH_TEST) glEnable(GL_BLEND) glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA) pm = pango.ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000) mv = pango.ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0) s_cam = pango.OpenGlRenderState(pm, mv) handler = pango.Handler3D(s_cam) d_cam = pango.CreateDisplay() d_cam.SetBounds(pango.Attach(0.0), pango.Attach(1.0), pango.Attach(0.0), pango.Attach(1.0), -1024 / 768) d_cam.SetHandler(handler) poses.append(poses[0]) while not pango.ShouldQuit(): glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT) d_cam.Activate(s_cam) glClearColor(1.0, 1.0, 1.0, 1.0) glLineWidth(2) for i, pose in enumerate(poses[:-1]): _, t = mat.T2Rt(pose) Ow = t Xw = mat.transform_homogeneous( pose, list(map(lambda x: x * 0.1, [1, 0, 0]))) Yw = mat.transform_homogeneous( pose, list(map(lambda x: x * 0.1, [0, 1, 0]))) Zw = mat.transform_homogeneous( pose, list(map(lambda x: x * 0.1, [0, 0, 1]))) glBegin(GL_LINES) glColor3f(1.0, 0.0, 0.0) glVertex3d(Ow[0], Ow[1], Ow[2]) glVertex3d(Xw[0], Xw[1], Xw[2]) glColor3f(0.0, 1.0, 0.0) glVertex3d(Ow[0], Ow[1], Ow[2]) glVertex3d(Yw[0], Yw[1], Yw[2]) glColor3f(0.0, 0.0, 1.0) glVertex3d(Ow[0], Ow[1], Ow[2]) glVertex3d(Zw[0], Zw[1], Zw[2]) p1 = poses[i] p2 = poses[i + 1] _, t1 = mat.T2Rt(p1) _, t2 = mat.T2Rt(p2) glColor3f(0.0, 0.0, 0.0) glVertex3d(t1[0], t1[1], t1[2]) glVertex3d(t2[0], t2[1], t2[2]) glEnd() pango.FinishFrame() time.sleep(0.005)
def main(): win = pango.CreateWindowAndBind("pySimpleDisplay", 640, 480) glEnable(GL_DEPTH_TEST) pm = pango.ProjectionMatrix(640, 480, 420, 420, 320, 240, 0.1, 1000) mv = pango.ModelViewLookAt(-0, 0.5, -3, 0, 0, 0, pango.AxisY) s_cam = pango.OpenGlRenderState(pm, mv) ui_width = 180 handler = pango.Handler3D(s_cam) d_cam = ( pango.CreateDisplay() .SetBounds( pango.Attach(0), pango.Attach(1), pango.Attach.Pix(ui_width), pango.Attach(1), -640.0 / 480.0, ) .SetHandler(handler) ) pango.CreatePanel("ui").SetBounds( pango.Attach(0), pango.Attach(1), pango.Attach(0), pango.Attach.Pix(ui_width) ) var_ui = pango.Var("ui") var_ui.a_Button = False var_ui.a_double = (0.0, pango.VarMeta(0, 5)) var_ui.an_int = (2, pango.VarMeta(0, 5)) var_ui.a_double_log = (3.0, pango.VarMeta(1, 1e4, logscale=True)) var_ui.a_checkbox = (False, pango.VarMeta(toggle=True)) var_ui.an_int_no_input = 2 var_ui.a_str = "sss" ctrl = -96 pango.RegisterKeyPressCallback(ctrl + ord("a"), a_callback) while not pango.ShouldQuit(): glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT) if var_ui.a_checkbox: var_ui.an_int = var_ui.a_double var_ui.an_int_no_input = var_ui.an_int d_cam.Activate(s_cam) pango.glDrawColouredCube() pango.FinishFrame()
def main(): # Create OpenGL window in single line pangolin.CreateWindowAndBind('Main', 640, 480) # 3D Mouse handler requires depth testing to be enabled gl.glEnable(gl.GL_DEPTH_TEST) scam = pangolin.OpenGlRenderState( pangolin.ProjectionMatrix(640, 480, 420, 420, 320, 240, 0.1, 1000), pangolin.ModelViewLookAt(-1, 1, -1, 0, 0, 0, pangolin.AxisDirection.AxisY)) # Aspect ratio allows us to constrain width and height whilst fitting within specified # bounds. A positive aspect ratio makes a view 'shrink to fit' (introducing empty bars), # whilst a negative ratio makes the view 'grow to fit' (cropping the view). dcam = pangolin.CreateDisplay() dcam.SetBounds(0.0, 1.0, 0.0, 1.0, -640.0 / 480.0) dcam.SetHandler(pangolin.Handler3D(scam)) # This view will take up no more than a third of the windows width or height, and it # will have a fixed aspect ratio to match the image that it will display. When fitting # within the specified bounds, push to the top-left (as specified by SetLock). dimg = pangolin.Display('image') dimg.SetBounds(2. / 3, 1.0, 0.0, 1. / 3, 640. / 480) dimg.SetLock(pangolin.Lock.LockLeft, pangolin.Lock.LockTop) w, h = 64, 48 texture = pangolin.GlTexture(w, h, gl.GL_RGB, False, 0, gl.GL_RGB, gl.GL_UNSIGNED_BYTE) # Default hooks for exiting (Esc) and fullscreen (tab). while not pangolin.ShouldQuit(): gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT) gl.glClearColor(0.95, 0.95, 0.95, 1.0) dcam.Activate(scam) gl.glColor3f(1.0, 1.0, 1.0) pangolin.glDrawColouredCube() # Set some random image data and upload to GPU image = random_image(w, h) texture.Upload(image, gl.GL_RGB, gl.GL_UNSIGNED_BYTE) # display the image dimg.Activate() gl.glColor3f(1.0, 1.0, 1.0) texture.RenderToViewport() pangolin.FinishFrame()
def DrawTrajectory(groundtruth, estimated): pango.CreateWindowAndBind("trajectory viewer", 1024, 768) glEnable(GL_DEPTH_TEST) glEnable(GL_BLEND) glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA) pm = pango.ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000) mv = pango.ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0) s_cam = pango.OpenGlRenderState(pm, mv) handler = pango.Handler3D(s_cam) d_cam = pango.CreateDisplay() d_cam.SetBounds(pango.Attach(0.0), pango.Attach(1.0), pango.Attach(0.0), pango.Attach(1.0), -1024/768) d_cam.SetHandler(handler) while not pango.ShouldQuit(): glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT) d_cam.Activate(s_cam) glClearColor(1.0, 1.0, 1.0, 1.0) glLineWidth(2) for i in range(len(groundtruth)-1): glColor3f(0.0, 1.0, 0.0) glBegin(GL_LINES) SE3_p1 = groundtruth[i] SE3_p2 = groundtruth[i+1] glVertex3d(SE3_p1[0], SE3_p1[1], SE3_p1[2]) glVertex3d(SE3_p2[0], SE3_p2[1], SE3_p2[2]) glEnd() for i in range(len(estimated)-1): glColor3f(1.0, 0.0, 0.0) glBegin(GL_LINES) SE3_p1 = estimated[i] SE3_p2 = estimated[i + 1] glVertex3d(SE3_p1[0], SE3_p1[1], SE3_p1[2]) glVertex3d(SE3_p2[0], SE3_p2[1], SE3_p2[2]) glEnd() pango.FinishFrame() time.sleep(0.005)
def viewer_refresh(self): if self.state == None: return # turn state into points ppts = np.array([d[:3, 3] for d in self.state[0]]) spts = np.array(self.state[1]) print(ppts) 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) gl.glPointSize(10) gl.glColor3f(0.0, 1.0, 0.0) pangolin.DrawPoints(ppts) gl.glPointSize(2) gl.glColor3f(0.0, 1.0, 0.0) pangolin.DrawPoints(spts) pangolin.FinishFrame()
def viewer_refresh(self, q): if self.state is None or not q.empty(): self.state = q.get() # turn state into points ppts = np.array([d[:3, 3] for d in self.state[0]]) spts = np.array(self.state[1]) gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT) gl.glClearColor(0, 0, 0, 0) self.dcam.Activate(self.scam) gl.glPointSize(10) gl.glColor3f(0.0, 1.0, 0.0) pangolin.DrawPoints(ppts) gl.glPointSize(2) gl.glColor3f(1.0, 1.0, 1.0) pangolin.DrawPoints(spts) pangolin.FinishFrame()
def main(): # Create OpenGL window in single line pangolin.CreateWindowAndBind('Main', 640, 480) # Data logger object log = pangolin.DataLog() # Optionally add named labels labels = ['sin(t)', 'cos(t)', 'sin(t)+cos(t)'] log.SetLabels(labels) # OpenGL 'view' of data. We might have many views of the same data. tinc = 0.03 plotter = pangolin.Plotter(log, 0.0, 4.0 * np.pi / tinc, -2.0, 2.0, np.pi / (4 * tinc), 0.5) plotter.SetBounds(0.0, 1.0, 0.0, 1.0) plotter.Track('$i') # Add some sample annotations to the plot plotter.AddMarker(pangolin.Marker.Vertical, -1000, pangolin.Marker.LessThan, pangolin.Colour.Blue().WithAlpha(0.2)) plotter.AddMarker(pangolin.Marker.Horizontal, 100, pangolin.Marker.GreaterThan, pangolin.Colour.Red().WithAlpha(0.2)) plotter.AddMarker(pangolin.Marker.Horizontal, 10, pangolin.Marker.Equal, pangolin.Colour.Green().WithAlpha(0.2)) pangolin.DisplayBase().AddDisplay(plotter) t = 0 while not pangolin.ShouldQuit(): gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT) log.Log(np.sin(t), np.cos(t), np.sin(t) + np.cos(t)) t += tinc pangolin.FinishFrame()
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, 100), 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) 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() # Draw Point Cloud points = np.random.random((100000, 3)) * 10 colors = np.zeros((len(points), 3)) colors[:, 1] = 1 - points[:, 0] / 10. colors[:, 2] = 1 - points[:, 1] / 10. colors[:, 0] = 1 - points[:, 2] / 10. gl.glPointSize(2) gl.glColor3f(1.0, 0.0, 0.0) # access numpy array directly(without copying data), array should be contiguous. pangolin.DrawPoints(points, colors) pangolin.FinishFrame()
def update(self, obs, ego_all_states, ego_picked_state, ego_xy_grid): # clear buffer gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT) gl.glClearColor(37 / 255, 37 / 255, 38 / 255, 1.0) self.dcam.Activate(self.scam) # grab observations # ignoring scans for now # car poses ego_x = obs['poses_x'][0] ego_y = obs['poses_y'][0] ego_theta = obs['poses_theta'][0] # print('ego', ego_x, ego_y, ego_theta) # print('opp', op_x, op_y, op_theta) # Draw boxes for agents # ego_pose = np.identity(4) ego_pose = transformations.rotation_matrix(ego_theta, (0, 0, 1)) ego_pose[0, 3] = ego_x ego_pose[1, 3] = ego_y # ego_pose[2, 3] = 0.1 # op_pose = np.identity(4) ego_size = np.array([0.58, 0.31, 0.1]) gl.glLineWidth(1) # ego is blue-ish gl.glColor3f(0.0, 0.5, 1.0) pangolin.DrawBoxes([ego_pose], [ego_size]) # Draw map gl.glPointSize(2) gl.glColor3f(0.2, 0.2, 0.2) pangolin.DrawPoints(self.map_points) # draw waypoints gl.glPointSize(2) gl.glColor3f(0.3, 0.3, 0.3) pangolin.DrawPoints(self.waypoints_plot) # draw flow samples # ego gl.glPointSize(2) gl.glColor3f(0.0, 0.5, 1.0) # print('hello \n') # print('st', ego_grid[0:5]) if ego_xy_grid is None: pangolin.FinishFrame() return gl.glPointSize(2) gl.glColor3f(0.0, 0.5, 1.0) # print('xytheta', ego_xythetas[0:5]) rot = np.array([[np.cos(ego_theta), np.sin(ego_theta)], [-np.sin(ego_theta), np.cos(ego_theta)]]) xy_grid = np.dot(ego_xy_grid[:, :2], rot) temp = np.hstack([xy_grid, np.zeros((xy_grid.shape[0], 1))]) # print('xyz', ego_xythetas[0:5]) pangolin.DrawPoints(temp + np.array([ego_x, ego_y, 0.0])[None, :]) # opp # Draw laser scans # Red for ego, Blue for op # Could be turned off if self.show_laser: rot_mat = transformations.rotation_matrix(ego_theta, (0, 0, 1)) ego_scan = obs['scans'][0] ego_scan = np.asarray(ego_scan) ego_scan_x = np.multiply(ego_scan, np.sin(self.scan_angles)) ego_scan_y = np.multiply(ego_scan, np.cos(self.scan_angles)) ego_scan_arr = np.zeros((ego_scan_x.shape[0], 3)) ego_scan_arr[:, 0] = ego_scan_y ego_scan_arr[:, 1] = ego_scan_x ego_scan_arr = np.dot(rot_mat[0:3, 0:3], ego_scan_arr.T) ego_scan_arr = ego_scan_arr + np.array([[ego_x], [ego_y], [0]]) gl.glPointSize(1) gl.glColor3f(1.0, 0.0, 0.0) pangolin.DrawPoints(ego_scan_arr.T) # Draw splines if ego_all_states is not None: gl.glPointSize(1) gl.glColor3f(0.8, 0.0, 0.5) # print('num traj', ego_all_states.shape[0]/100) pangolin.DrawPoints( np.hstack([ ego_all_states[:, 0:2], np.zeros((ego_all_states.shape[0], 1)) ])) if ego_picked_state is not None: gl.glPointSize(3) if ego_all_states is None: gl.glColor3f(1., 0., 0.) else: gl.glColor3f(1., 1., 1.) pangolin.DrawPoints( np.hstack([ ego_picked_state[:, 0:2], np.zeros((ego_picked_state.shape[0], 1)) ])) # render pangolin.FinishFrame()
pango.Attach(0), pango.Attach(1), pango.Attach(0), pango.Attach(0) ) while not pango.ShouldQuit(): gl.glClear(gl.GL_COLOR_BUFFER_BIT + gl.GL_DEPTH_BUFFER_BIT) d_cam.Activate(s_cam) gl.glEnableClientState(gl.GL_VERTEX_ARRAY) gl.glEnableClientState(gl.GL_COLOR_ARRAY) gl.glVertexPointer( 3, gl.GL_FLOAT, 0, xyz_neg.ctypes.data_as(ctypes.POINTER(ctypes.c_float)) ) gl.glColorPointer( 3, gl.GL_FLOAT, 0, rgb_neg.ctypes.data_as(ctypes.POINTER(ctypes.c_float)) ) gl.glDrawArrays(gl.GL_POINTS, 0, xyz_neg.shape[0]) gl.glVertexPointer( 3, gl.GL_FLOAT, 0, xyz_pos.ctypes.data_as(ctypes.POINTER(ctypes.c_float)) ) gl.glColorPointer( 3, gl.GL_FLOAT, 0, rgb_pos.ctypes.data_as(ctypes.POINTER(ctypes.c_float)) ) gl.glDrawArrays(gl.GL_POINTS, 0, xyz_pos.shape[0]) gl.glDisableClientState(gl.GL_COLOR_ARRAY) gl.glDisableClientState(gl.GL_VERTEX_ARRAY) pango.FinishFrame()
def main(): pangolin.ParseVarsFile('app.cfg') pangolin.CreateWindowAndBind('Main', 640, 480) gl.glEnable(gl.GL_DEPTH_TEST) scam = pangolin.OpenGlRenderState( pangolin.ProjectionMatrix(640, 480, 420, 420, 320, 240, 0.1, 1000), pangolin.ModelViewLookAt(0, 0.5, -3, 0, 0, 0, pangolin.AxisDirection.AxisY)) handler3d = pangolin.Handler3D(scam) dcam = pangolin.CreateDisplay() dcam.SetBounds(0.0, 1.0, 180 / 640., 1.0, -640.0 / 480.0) # dcam.SetBounds(pangolin.Attach(0.0), pangolin.Attach(1.0), # pangolin.Attach.Pix(180), pangolin.Attach(1.0), -640.0/480.0) dcam.SetHandler(pangolin.Handler3D(scam)) panel = pangolin.CreatePanel('ui') panel.SetBounds(0.0, 1.0, 0.0, 180 / 640.) button = pangolin.VarBool('ui.Button', value=False, toggle=False) checkbox = pangolin.VarBool('ui.Checkbox', value=False, toggle=True) float_slider = pangolin.VarFloat('ui.Float', value=3, min=0, max=5) float_log_slider = pangolin.VarFloat('ui.Log_scale var', value=3, min=1, max=1e4, logscale=True) int_slider = pangolin.VarInt('ui.Int', value=2, min=0, max=5) int_slave_slider = pangolin.VarInt('ui.Int_slave', value=2, toggle=False) save_window = pangolin.VarBool('ui.Save_Window', value=False, toggle=False) save_cube = pangolin.VarBool('ui.Save_Cube', value=False, toggle=False) record_cube = pangolin.VarBool('ui.Record_Cube', value=False, toggle=False) def reset(): #float_slider.SetVal(0.5) print('You typed ctrl-r or pushed reset') # Reset = SetVarFunctor(float_slider, 0.5) # reset = pangolin.VarFunc('ui.Reset', reset) # pangolin.RegisterKeyPressCallback(int(pangolin.PANGO_CTRL) + ord('r'), reset) # segfault # pangolin.RegisterKeyPressCallback(int(pangolin.PANGO_CTRL) + ord('b'), pangolin.SetVarFunctorFloat('ui.Float', 4.5)) # segfault # pangolin.RegisterKeyPressCallback(int(pangolin.PANGO_CTRL) + ord('b'), SetVarFunctor(float_slider, 4.5)) # segfault while not pangolin.ShouldQuit(): gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT) if pangolin.Pushed(button): print('You Pushed a button!') if checkbox.Get(): int_slider.SetVal(int(float_slider)) int_slave_slider.SetVal(int_slider) if pangolin.Pushed(save_window): pangolin.SaveWindowOnRender("window") if pangolin.Pushed(save_cube): pangolin.SaveWindowOnRender("cube") if pangolin.Pushed(record_cube): pangolin.DisplayBase().RecordOnRender( "ffmpeg:[fps=50,bps=8388608,unique_filename]//screencap.avi") dcam.Activate(scam) gl.glColor3f(1.0, 1.0, 1.0) pangolin.glDrawColouredCube() pangolin.FinishFrame()
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()
def main(): # Create OpenGL window in single line pangolin.CreateWindowAndBind('Main', 640, 480) # 3D Mouse handler requires depth testing to be enabled gl.glEnable(gl.GL_DEPTH_TEST) # Issue specific OpenGl we might need gl.glEnable(gl.GL_BLEND) gl.glBlendFunc(gl.GL_SRC_ALPHA, gl.GL_ONE_MINUS_SRC_ALPHA) # Define Camera Render Object (for view / scene browsing) proj = pangolin.ProjectionMatrix(640, 480, 420, 420, 320, 240, 0.1, 1000) scam = pangolin.OpenGlRenderState( proj, pangolin.ModelViewLookAt(1, 0.5, -2, 0, 0, 0, pangolin.AxisY)) scam2 = pangolin.OpenGlRenderState( proj, pangolin.ModelViewLookAt(0, 0, -2, 0, 0, 0, pangolin.AxisY)) # Add named OpenGL viewport to window and provide 3D Handler dcam1 = pangolin.Display('cam1') dcam1.SetAspect(640 / 480.) dcam1.SetHandler(pangolin.Handler3D(scam)) dcam2 = pangolin.Display('cam2') dcam2.SetAspect(640 / 480.) dcam2.SetHandler(pangolin.Handler3D(scam2)) dcam3 = pangolin.Display('cam3') dcam3.SetAspect(640 / 480.) dcam3.SetHandler(pangolin.Handler3D(scam)) dcam4 = pangolin.Display('cam4') dcam4.SetAspect(640 / 480.) dcam4.SetHandler(pangolin.Handler3D(scam2)) dimg1 = pangolin.Display('img1') dimg1.SetAspect(640 / 480.) dimg2 = pangolin.Display('img2') dimg2.SetAspect(640 / 480.) # LayoutEqual is an EXPERIMENTAL feature - it requires that all sub-displays # share the same aspect ratio, placing them in a raster fasion in the # viewport so as to maximise display size. view = pangolin.Display('multi') view.SetBounds(0.0, 1.0, 0.0, 1.0) view.SetLayout(pangolin.LayoutEqual) view.AddDisplay(dcam1) view.AddDisplay(dimg1) view.AddDisplay(dcam2) view.AddDisplay(dimg2) view.AddDisplay(dcam3) view.AddDisplay(dcam4) w, h = 64, 48 image_texture = pangolin.GlTexture(w, h, gl.GL_RGB, False, 0, gl.GL_RGB, gl.GL_UNSIGNED_BYTE) # Default hooks for exiting (Esc) and fullscreen (tab) while not pangolin.ShouldQuit(): gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT) gl.glColor3f(1.0, 1.0, 1.0) dcam1.Activate(scam) pangolin.glDrawColouredCube() dcam2.Activate(scam2) pangolin.glDrawColouredCube() dcam3.Activate(scam) pangolin.glDrawColouredCube() dcam4.Activate(scam2) pangolin.glDrawColouredCube() dimg1.Activate() gl.glColor4f(1.0, 1.0, 1.0, 1.0) image_texture.Upload(random_image(w, h), gl.GL_RGB, gl.GL_UNSIGNED_BYTE) image_texture.RenderToViewport() dimg2.Activate() gl.glColor4f(1.0, 1.0, 1.0, 1.0) # image_texture.Upload(random_image(w, h), gl.GL_RGB, gl.GL_UNSIGNED_BYTE) image_texture.RenderToViewport() pangolin.FinishFrame()
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) print(trajectory.shape) 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) drawPlane() # 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()
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 = -7 viewpoint_z = -18 viewpoint_f = 1000 proj = pangolin.ProjectionMatrix(1024, 768, viewpoint_f, viewpoint_f, 512, 389, 0.1, 300) look_view = pangolin.ModelViewLookAt(viewpoint_x, viewpoint_y, viewpoint_z, 0, 0, 0, 0, -1, 0) # Camera Render Object (for view / scene browsing) scam = pangolin.OpenGlRenderState(proj, look_view) # Add named OpenGL viewport to window and provide 3D Handler dcam = pangolin.CreateDisplay() # dcam.SetBounds(0, 1, 0, 1, 1024 / 768.) dcam.SetHandler(pangolin.Handler3D(scam)) # image width, height = 376, 240 dimg = pangolin.Display('image') # dimg.SetBounds(0, height / 768., 0.0, width / 1024., 1024 / 768.) dimg.SetLock(pangolin.Lock.LockLeft, pangolin.Lock.LockTop) texture = pangolin.GlTexture(width, height, gl.GL_RGB, False, 0, gl.GL_RGB, gl.GL_UNSIGNED_BYTE) image = np.ones((height, width, 3), 'uint8') # axis axis = pangolin.Renderable() axis.Add(pangolin.Axis()) trajectory = DynamicArray() camera = None image = None while not pangolin.ShouldQuit(): if not self.pose_queue.empty(): while not self.pose_queue.empty(): pose = self.pose_queue.get() trajectory.append(pose[:3, 3]) camera = pose if not self.image_queue.empty(): while not self.image_queue.empty(): img = self.image_queue.get() img = img[::-1, :, ::-1] img = cv2.resize(img, (width, height)) image = img.copy() 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) # draw axis axis.Render() # draw current camera if camera is not None: gl.glLineWidth(1) gl.glColor3f(0.0, 0.0, 1.0) pangolin.DrawCameras(np.array([camera]), 0.5) # show trajectory if len(trajectory) > 0: gl.glPointSize(2) gl.glColor3f(0.0, 0.0, 0.0) pangolin.DrawPoints(trajectory.array()) # show image if image is not None: texture.Upload(image, gl.GL_RGB, gl.GL_UNSIGNED_BYTE) dimg.Activate() gl.glColor3f(1.0, 1.0, 1.0) texture.RenderToViewport() pangolin.FinishFrame()
def update(self,obs,ego_grid,opp_grid,op_all_states,op_picked_state,ego_all_states,ego_picked_state,is_corner,ego_xy_grid): gl.glClear(gl.GL_COLOR_BUFFER_BIT|gl.GL_DEPTH_BUFFER_BIT) gl.glClearColor(37/255,37/255,38/255,1.0) self.dcam.Activate(self.scam) ego_x=obs['poses_x'][0] ego_y=obs['poses_y'][0] ego_theta=obs['poses_theta'][0] op_x=obs['poses_x'][1] op_y=obs['poses_y'][1] op_theta=obs['poses_theta'][1] ego_pose=transformations.rotation_matrix(ego_theta,(0,0,1)) ego_pose[0,3]=ego_x ego_pose[1,3]=ego_y op_pose=transformations.rotation_matrix(op_theta,(0,0,1)) op_pose[0,3]=op_x op_pose[1,3]=op_y ego_size=np.array([0.58,0.31,0.1]) op_size=np.array([0.58,0.31,0.1]) gl.glLineWidth(1) gl.glColor3f(1.0,1.0,1.0) pangolin.DrawBoxes([ego_pose],[ego_size]) gl.glColor(231/256.,34/256.,46/256.) pangolin.DrawBoxes([op_pose],[op_size]) gl.glPointSize(2) gl.glColor3f(0.2,0.2,0.2) pangolin.DrawPoints(self.map_points) gl.glPointSize(2) gl.glColor3f(0.0,0.5,1.0) if ego_xy_grid is None: pangolin.FinishFrame() return gl.glPointSize(2) gl.glColor3f(0.0,0.5,1.0) rot=np.array([[np.cos(ego_theta),np.sin(ego_theta)],[-np.sin(ego_theta),np.cos(ego_theta)]]) xy_grid=np.dot(ego_xy_grid[:,:2],rot) temp=np.hstack([xy_grid,np.zeros((xy_grid.shape[0],1))]) if self.show_laser: rot_mat=transformations.rotation_matrix(ego_theta,(0,0,1)) ego_scan=obs['scans'][0] ego_scan=np.asarray(ego_scan) ego_scan_x=np.multiply(ego_scan,np.sin(self.scan_angles)) ego_scan_y=np.multiply(ego_scan,np.cos(self.scan_angles)) ego_scan_arr=np.zeros((ego_scan_x.shape[0],3)) ego_scan_arr[:,0]=ego_scan_y ego_scan_arr[:,1]=ego_scan_x ego_scan_arr=np.dot(rot_mat[0:3,0:3],ego_scan_arr.T) ego_scan_arr=ego_scan_arr+np.array([[ego_x],[ego_y],[0]]) gl.glPointSize(1) gl.glColor3f(1.0,0.0,0.0) pangolin.DrawPoints(ego_scan_arr.T) if ego_all_states is not None: gl.glPointSize(2) gl.glColor3f(0.8,0.0,0.5) if op_picked_state is not None: gl.glPointSize(3) if op_all_states is None: gl.glColor3f(231/256.,34/256.,46/256.) else: gl.glColor3f(231/256.,34/256.,46/256.) pangolin.DrawPoints(np.hstack([op_picked_state[:,0:2],np.zeros((op_picked_state.shape[0],1))])) if ego_picked_state is not None: gl.glPointSize(5) if ego_all_states is None: gl.glColor3f(1.,1.,1.) else: gl.glColor3f(1.,1.,1.) pangolin.DrawPoints(np.hstack([ego_picked_state[:,0:2],np.zeros((ego_picked_state.shape[0],1))])) gl.glPointSize(2) gl.glColor3f(22/256.,88/256.,142/256.) pangolin.DrawPoints(self.waypoints_plot) pangolin.FinishFrame()