def init(self): w, h = (1024, 768) f = 2000 #420 pango.CreateWindowAndBind("g2o_stuff", w, h) gl.glEnable(gl.GL_DEPTH_TEST) # Projection and ModelView Matrices self.scam = pango.OpenGlRenderState( pango.ProjectionMatrix(w, h, f, f, w // 2, h // 2, 0.1, 100000), pango.ModelViewLookAt(0, -50.0, -10.0, 0.0, 0.0, 0.0, 0.0, -1.0, 0.0)) #pango.AxisDirection.AxisY)) self.handler = pango.Handler3D(self.scam) # Interactive View in Window self.dcam = pango.CreateDisplay() self.dcam.SetBounds(0.0, 1.0, 0.0, 1.0, -w / h) self.dcam.SetHandler(self.handler) self.dcam.Activate() pango.RegisterKeyPressCallback(ord('r'), self.optimize_callback) pango.RegisterKeyPressCallback(ord('t'), self.switch_callback)
def setup(): global dcam, scam pangolin.CreateWindowAndBind('Main', 640, 480) gl.glEnable(gl.GL_DEPTH_TEST) # Define Projection and initial ModelView matrix width = 640 height = 480 z_near = 0.2 z_far = 1000 scam = pangolin.OpenGlRenderState( pangolin.ProjectionMatrix(width, height, 420, 420, 320, 240, z_near, z_far), # Negative Y direction is up since top left corner of images is (0,0) pangolin.ModelViewLookAt(-2, -2, -4, 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, -640.0 / 480.0) dcam.SetHandler(handler)
def init(self, width, height): """Initialize the 3D display (pangolin) :width: TODO :height: TODO :returns: TODO """ pangolin.CreateWindowAndBind('Map viewer', width, height) gl.glEnable(gl.GL_DEPTH_TEST) # Define Projection and initial ModelView matrix self.scam = pangolin.OpenGlRenderState( pangolin.ProjectionMatrix(width, height, 420, 420, width // 2, height // 2, 0.2, 10000), pangolin.ModelViewLookAt(0, -10, -8, 0, 0, 0, 0, -1, 0)) self.handler = pangolin.Handler3D(self.scam) # Create Interactive View in window self.dcam = pangolin.CreateDisplay() self.dcam.SetBounds(0.0, 1.0, 0.0, 1.0, -width / height) self.dcam.SetHandler(self.handler)
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) dcam.Resize(pangolin.Viewport(0, 0, 640 * 2, 480 * 2)) 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 __init__(self, name): super().__init__() self.name = name # Window pango.CreateWindowAndBind(self.name, int(640*(3/2)), int(480*(3/2))) gl.glEnable(gl.GL_DEPTH_TEST) # Define Projection and initial ModelView matrix self.scam = pango.OpenGlRenderState( pango.ProjectionMatrix(640, 480, 420, 420, 320, 240, 0.2, 200), pango.ModelViewLookAt(0, -0.5, -0.5, 0, 0, 1, 0, -1, 0)) self.handler = pango.Handler3D(self.scam) # Create Interactive View in window self.dcam = pango.CreateDisplay() self.dcam.SetBounds( pango.Attach(0), pango.Attach(1), pango.Attach(0), pango.Attach(1), -640.0/480.0) self.dcam.SetHandler(self.handler)
def main(): # Parse arguments args = options.get_args(description="Debug a network", options=[ "batch", "workers", "device", "load", "net", "loss", ]) if args.load == "": print("No model file specified to load!") exit() # Construct datasets random.seed(1337) _, _, test_loader = utils.get_kitti_split(args.batch, args.workers) # The model model = networks.architectures.get_net(args) loss_fn = sfm_loss.get_loss_fn(args) # Load checkpoint = torch.load(args.load, map_location=torch.device(args.device)) model.load_state_dict(checkpoint["model"]) # Window pango.CreateWindowAndBind('Main', int(640 * (3 / 2)), int(480 * (3 / 2))) gl.glEnable(gl.GL_DEPTH_TEST) # Define Projection and initial ModelView matrix scam = pango.OpenGlRenderState( pango.ProjectionMatrix(640, 480, 420, 420, 320, 240, 0.2, 200), pango.ModelViewLookAt(0, -0.5, -0.5, 0, 0, 1, 0, -1, 0)) handler = pango.Handler3D(scam) # Create Interactive View in window dcam = pango.CreateDisplay() dcam.SetBounds(pango.Attach(0), pango.Attach(1), pango.Attach(0), pango.Attach(1), -640.0 / 480.0) dcam.SetHandler(handler) # Test model.train() model = model.to(args.device) def step_fn(step, inputs): # Forward pass and loss with torch.no_grad(): loss, data = utils.forward_pass(model, loss_fn, inputs) print("loss %f" % loss.item()) print(data.keys()) print(data["pose"].shape) for i in range(args.batch): print(list(data["pose"][i, 0, :].cpu().detach().numpy())) print(list(data["pose"][i, 1, :].cpu().detach().numpy())) print("--") depth_img = viz.tensor2depthimg( torch.cat((*data["depth"][0][:, 0], ), dim=0)) tgt_img = viz.tensor2img(torch.cat((*data["tgt"], ), dim=1)) img = np.concatenate((tgt_img, depth_img), axis=1) warp_imgs = [] #diff_imgs = [] for warp, diff in zip(data["warp"], data["diff"]): warp = restack(restack(warp, 1, -1), 0, -2) diff = restack(restack(diff, 1, -1), 0, -2) warp_imgs.append(viz.tensor2img(warp)) #diff_imgs.append(viz.tensor2diffimg(diff)) world = reconstruction.depth_to_3d_points(data["depth"][0], data["K"]) points = world[0, :].view(3, -1).transpose( 1, 0).cpu().detach().numpy().astype(np.float64) colors = (data["tgt"][0, :].view(3, -1).transpose( 1, 0).cpu().detach().numpy().astype(np.float64) + 1) / 2 loop = True while loop: key = cv2.waitKey(10) if key == 27 or pango.ShouldQuit(): exit() elif key != -1: loop = False cv2.imshow("target and depth", img) #for i, (warp, diff) in enumerate(zip(warp_imgs, diff_imgs)): for i, warp in enumerate(warp_imgs): cv2.imshow("warp scale: %d" % i, warp) #cv2.imshow("diff scale: %d" % i, diff) 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) gl.glPointSize(5) pango.DrawPoints(points, colors) pose = np.identity(4) pose[:3, 3] = 0 gl.glLineWidth(1) gl.glColor3f(0.0, 0.0, 1.0) pango.DrawCamera(pose, 0.5, 0.75, 0.8) pango.FinishFrame() utils.iterate_loader(args, test_loader, step_fn)
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 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) panel = pangolin.CreatePanel('menu') panel.SetBounds(0.5, 1.0, 0.0, 175 / 1024.) # checkbox m_follow_camera = pangolin.VarBool('menu.Follow Camera', value=True, toggle=True) m_show_points = pangolin.VarBool('menu.Show Points', True, True) m_show_lines = pangolin.VarBool('menu.Show Lines', True, True) m_show_keyframes = pangolin.VarBool('menu.Show KeyFrames', True, True) m_show_graph = pangolin.VarBool('menu.Show Graph', True, True) m_show_image = pangolin.VarBool('menu.Show Image', True, True) # button m_replay = pangolin.VarBool('menu.Replay', value=False, toggle=False) m_refresh = pangolin.VarBool('menu.Refresh', False, False) m_reset = pangolin.VarBool('menu.Reset', False, False) if self.config is None: width, height = 400, 250 viewpoint_x = 0 viewpoint_y = -500 # -10 viewpoint_z = -100 # -0.1 viewpoint_f = 2000 camera_width = 1. else: width = self.config.view_image_width height = self.config.view_image_height viewpoint_x = self.config.view_viewpoint_x viewpoint_y = self.config.view_viewpoint_y viewpoint_z = self.config.view_viewpoint_z viewpoint_f = self.config.view_viewpoint_f camera_width = self.config.view_camera_width 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) # 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.0, 1.0, 175 / 1024., 1.0, -1024 / 768.) dcam.SetHandler(pangolin.Handler3D(scam)) # image # width, height = 400, 130 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') pose = pangolin.OpenGlMatrix() # identity matrix following = True active = [] replays = [] graph = [] loops = [] mappoints = DynamicArray(shape=(3, )) maplines = DynamicArray(shape=(6, )) colors = DynamicArray(shape=(3, )) cameras = DynamicArray(shape=(4, 4)) active_lines = [] line_colors = [] gnd_mesh = None while not pangolin.ShouldQuit(): if not self.q_pose.empty(): pose.m = self.q_pose.get() follow = m_follow_camera.Get() if follow and following: scam.Follow(pose, True) elif follow and not following: scam.SetModelViewMatrix(look_view) scam.Follow(pose, True) following = True elif not follow and following: following = False 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) # show graph if not self.q_graph.empty(): graph = self.q_graph.get() loops = np.array([_[0] for _ in graph if _[1] == 2]) graph = np.array([_[0] for _ in graph if _[1] < 2]) if m_show_graph.Get(): if len(graph) > 0: gl.glLineWidth(1) gl.glColor3f(0.0, 1.0, 0.0) pangolin.DrawLines(graph, 3) if len(loops) > 0: gl.glLineWidth(2) gl.glColor3f(0.0, 0.0, 0.0) pangolin.DrawLines(loops, 4) gl.glPointSize(4) gl.glColor3f(1.0, 0.0, 0.0) gl.glBegin(gl.GL_POINTS) gl.glVertex3d(pose[0, 3], pose[1, 3], pose[2, 3]) gl.glEnd() # if not self.q_ground.empty(): # gnd_mesh = self.q_ground.get() if gnd_mesh is not None: gl.glLineWidth(2) gl.glColor3f(0.5, 0.25, 0.5) pangolin.DrawLines(gnd_mesh, 2) # Show mappoints if not self.q_points.empty(): pts, code = self.q_points.get() cls, code = self.q_colors.get() if code == 1: # append new points mappoints.extend(pts) colors.extend(cls) elif code == 0: # refresh all points mappoints.clear() mappoints.extend(pts) colors.clear() colors.extend(cls) if m_show_points.Get(): gl.glPointSize(4) # easily draw millions of points pangolin.DrawPoints(mappoints.array(), colors.array()) if not self.q_active.empty(): active = self.q_active.get() gl.glPointSize(5) gl.glBegin(gl.GL_POINTS) gl.glColor3f(1.0, 0.0, 0.0) for point in active: gl.glVertex3f(*point) gl.glEnd() if not self.q_lines.empty(): lines, code = self.q_lines.get() maplines.extend(lines) if m_show_lines.Get(): gl.glLineWidth(1) gl.glColor3f(1.0, 0.0, 0.5) pangolin.DrawLines(maplines.array(), 2) if not self.q_active_lines.empty(): active_lines, line_colors = np.array( self.q_active_lines.get()) if active_lines is not None: for act_line, color in zip(active_lines, line_colors): c = (color[2] / 255, color[1] / 255, color[0] / 255) act_line = act_line.reshape((1, 6)) gl.glLineWidth(5) gl.glColor3f(*c) pangolin.DrawLines(act_line, 2) if len(replays) > 0: n = 300 gl.glPointSize(4) gl.glColor3f(1.0, 0.0, 0.0) gl.glBegin(gl.GL_POINTS) for point in replays[:n]: gl.glVertex3f(*point) gl.glEnd() replays = replays[n:] # show cameras if not self.q_camera.empty(): cams = self.q_camera.get() if len(cams) > 20: cameras.clear() cameras.extend(cams) if m_show_keyframes.Get(): gl.glLineWidth(1) gl.glColor3f(0.0, 0.0, 1.0) pangolin.DrawCameras(cameras.array(), camera_width) # show image if not self.q_image.empty(): image = self.q_image.get() if image.ndim == 3: image = image[::-1, :, ::-1] else: image = np.repeat(image[::-1, :, np.newaxis], 3, axis=2) image = cv2.resize(image, (width, height)) if m_show_image.Get(): texture.Upload(image, gl.GL_RGB, gl.GL_UNSIGNED_BYTE) dimg.Activate() gl.glColor3f(1.0, 1.0, 1.0) texture.RenderToViewport() if pangolin.Pushed(m_replay): replays = mappoints.array() if pangolin.Pushed(m_reset): m_show_graph.SetVal(True) m_show_keyframes.SetVal(True) m_show_points.SetVal(True) m_show_image.SetVal(True) m_follow_camera.SetVal(True) follow_camera = True if pangolin.Pushed(m_refresh): self.q_refresh.put(True) pangolin.FinishFrame()
def visualize(R1, t1, R2, t2, R3, t3, R4, t4, name): pangolin.CreateWindowAndBind(name, 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(0, -10, 0.1, 0, 0, 0, pangolin.AxisNegY)) 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) w = 0.3 h = w * 0.75 z = w * 0.6 t1r = np.matmul(R1, t1) t2r = np.matmul(R2, t2) t3r = np.matmul(R3, t3) t4r = np.matmul(R4, t4) bottom = np.array([[0, 0, 0, 1.0]]) T1 = np.concatenate([R1, t1], axis=1) T1 = np.concatenate([T1, bottom], axis=0) T2 = np.concatenate([R2, t2], axis=1) T2 = np.concatenate([T2, bottom], axis=0) T3 = np.concatenate([R3, t3], axis=1) T3 = np.concatenate([T3, bottom], axis=0) T4 = np.concatenate([R4, t4], axis=1) T4 = np.concatenate([T4, bottom], axis=0) 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) gl.glLineWidth(2) gl.glBegin(gl.GL_LINES) gl.glColor3f(0.0, 0.0, 0.0) gl.glVertex3f(0, 0, 0) gl.glVertex3f(w, h, z) gl.glVertex3f(0, 0, 0) gl.glVertex3f(w, -h, z) gl.glVertex3f(0, 0, 0) gl.glVertex3f(-w, -h, z) gl.glVertex3f(0, 0, 0) gl.glVertex3f(-w, h, z) gl.glVertex3f(w, h, z) gl.glVertex3f(w, -h, z) gl.glVertex3f(-w, h, z) gl.glVertex3f(-w, -h, z) gl.glVertex3f(-w, h, z) gl.glVertex3f(w, h, z) gl.glVertex3f(-w, -h, z) gl.glVertex3f(w, -h, z) gl.glEnd() gl.glPushMatrix() gl.glMultMatrixd(T1.T) gl.glLineWidth(2) gl.glBegin(gl.GL_LINES) gl.glColor3f(0., 1.0, 1.0) gl.glVertex3f(0, 0, 0) gl.glVertex3f(w, h, z) gl.glVertex3f(0, 0, 0) gl.glVertex3f(w, -h, z) gl.glVertex3f(0, 0, 0) gl.glVertex3f(-w, -h, z) gl.glVertex3f(0, 0, 0) gl.glVertex3f(-w, h, z) gl.glVertex3f(w, h, z) gl.glVertex3f(w, -h, z) gl.glVertex3f(-w, h, z) gl.glVertex3f(-w, -h, z) gl.glVertex3f(-w, h, z) gl.glVertex3f(w, h, z) gl.glVertex3f(-w, -h, z) gl.glVertex3f(w, -h, z) gl.glEnd() gl.glPopMatrix() gl.glPushMatrix() gl.glMultMatrixd(T2.T) gl.glLineWidth(2) gl.glBegin(gl.GL_LINES) gl.glColor3f(1.0, 0.7, 1.0) gl.glVertex3f(0, 0, 0) gl.glVertex3f(w, h, z) gl.glVertex3f(0, 0, 0) gl.glVertex3f(w, -h, z) gl.glVertex3f(0, 0, 0) gl.glVertex3f(-w, -h, z) gl.glVertex3f(0, 0, 0) gl.glVertex3f(-w, h, z) gl.glVertex3f(w, h, z) gl.glVertex3f(w, -h, z) gl.glVertex3f(-w, h, z) gl.glVertex3f(-w, -h, z) gl.glVertex3f(-w, h, z) gl.glVertex3f(w, h, z) gl.glVertex3f(-w, -h, z) gl.glVertex3f(w, -h, z) gl.glEnd() gl.glPopMatrix() gl.glPushMatrix() gl.glMultMatrixd(T3.T) gl.glLineWidth(2) gl.glBegin(gl.GL_LINES) gl.glColor3f(0.8, 0.8, 0.) gl.glVertex3f(0, 0, 0) gl.glVertex3f(w, h, z) gl.glVertex3f(0, 0, 0) gl.glVertex3f(w, -h, z) gl.glVertex3f(0, 0, 0) gl.glVertex3f(-w, -h, z) gl.glVertex3f(0, 0, 0) gl.glVertex3f(-w, h, z) gl.glVertex3f(w, h, z) gl.glVertex3f(w, -h, z) gl.glVertex3f(-w, h, z) gl.glVertex3f(-w, -h, z) gl.glVertex3f(-w, h, z) gl.glVertex3f(w, h, z) gl.glVertex3f(-w, -h, z) gl.glVertex3f(w, -h, z) gl.glEnd() gl.glPopMatrix() gl.glPushMatrix() gl.glMultMatrixd(T4.T) gl.glLineWidth(2) gl.glBegin(gl.GL_LINES) gl.glColor3f(0.7, 0.7, 0.7) gl.glVertex3f(0, 0, 0) gl.glVertex3f(w, h, z) gl.glVertex3f(0, 0, 0) gl.glVertex3f(w, -h, z) gl.glVertex3f(0, 0, 0) gl.glVertex3f(-w, -h, z) gl.glVertex3f(0, 0, 0) gl.glVertex3f(-w, h, z) gl.glVertex3f(w, h, z) gl.glVertex3f(w, -h, z) gl.glVertex3f(-w, h, z) gl.glVertex3f(-w, -h, z) gl.glVertex3f(-w, h, z) gl.glVertex3f(w, h, z) gl.glVertex3f(-w, -h, z) gl.glVertex3f(w, -h, z) gl.glEnd() gl.glPopMatrix() # gl.glLineWidth(2) # gl.glBegin(gl.GL_LINES) # gl.glColor3f(0., 1., 0.) # gl.glVertex3f(0, 0, 0) # # gl.glVertex3f(t1r[0], t1r[1], t1r[2]) # gl.glVertex3f(t1r[0], t1r[1], t1r[2]) # gl.glVertex3f(t2r[0], t2r[1], t3r[2]) # gl.glVertex3f(t2r[0], t2r[1], t3r[2]) # gl.glVertex3f(t3r[0], t3r[1], t4r[2]) # gl.glVertex3f(t3r[0], t3r[1], t4r[2]) # gl.glVertex3f(t4r[0], t4r[1], t4r[2]) # # gl.glEnd() # pangolin.FinishFrame()
def UpdateProjectionMat(self): self.projectionMat = pangolin.ProjectionMatrix(1024, 768, 420, 420, 320, 240, self.near, self.far) return self.projectionMat
def glMainLoop(self): self.logger.info("Running gl viewer ...") flag = False pangolin.CreateWindowAndBind('Main', 640, 480) gl.glEnable(gl.GL_DEPTH_TEST) gl.glEnable(gl.GL_BLEND) gl.glBlendFunc(gl.GL_SRC_ALPHA, gl.GL_ONE_MINUS_SRC_ALPHA) panel = pangolin.CreatePanel('menu') panel.SetBounds(1.0, 1.0, 0.0, 100 / 640.) # Does not work with initialization of window # self.Init() camera_pose = pangolin.OpenGlMatrix() # create scene graph # scene = pangolin.Renderable() # # x : R # # y : G # # z : B # scene.Add(pangolin.Axis()) # rendered_cam = pangolin.OpenGlRenderState( pangolin.ProjectionMatrix(640, 480, 420, 420, 320, 240, 0.01, 2000), pangolin.ModelViewLookAt(1, 0, 1, 0, 0, 0, 0, 0, 1) ) handler = pangolin.Handler3D(rendered_cam) # handler = pangolin.SceneHandler(scene, rendered_cam) # add drawing callback # viewport = pangolin.CreateDisplay() viewport.SetBounds(0.0, 1.0, 0.0, 1.0, -640.0 / 480.0) viewport.SetHandler(handler) image_viewport = pangolin.Display('image') w = 160 h = 120 image_viewport.SetBounds(0, h / 480., 0.0, w / 640., 640. / 480.) image_viewport.SetLock(pangolin.Lock.LockLeft, pangolin.Lock.LockTop) texture = pangolin.GlTexture(w, h, gl.GL_RGB, False, 0, gl.GL_RGB, gl.GL_UNSIGNED_BYTE) # geometries self.lines = [] self.points = {} self.colors = {} self.poses = [] self.redundant_points = [] self.redundant_colors = [] self.landmarks = {} # img = np.ones((h, w, 3), 'uint8') while not pangolin.ShouldQuit(): datum = None if not self.attributes.empty(): self.logger.info("[GL Process] attributes is not empty, fetching datum ...") datum = self.attributes.get() self.logger.info("[GL Process] get a datum from the main thread of the parent process") if datum is not None: # dispatch instructions if datum.attrs.get('pose', None) is not None: pose = datum.attrs['pose'] # self.camera.pose.m = pose # set Twc camera_pose.m = pose # np.linalg.inv(pose) # camera_pose.m = pose rendered_cam.Follow(camera_pose, True) self.logger.info("[GL Process] update camera pose matrix got from datum: \n%s" % pose) pass pass # self.Clear() # self.control.Update(self.camera.pose) gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT) gl.glClearColor(1.0, 1.0, 1.0, 1.0) viewport.Activate(rendered_cam) # scene.Render() # Just for test # pangolin.glDrawColouredCube(0.1) # render graph if datum is not None: # dispatch data # update graph if datum.attrs.get('lines', None) is not None: lines = datum.attrs['lines'] self.lines.extend(lines) self.logger.info("[GL Process] drawing %d lines ..." % len(lines)) if datum.attrs.get('pose', None) is not None: pose = datum.attrs['pose'] self.poses.append(pose) self.logger.info("[GL Process] drawing a camera with new pose matrix ...") pass # update image if datum.attrs.get('img', None) is not None: img = datum.attrs['img'] # see pangolin issue #180 # change cv BGR channels to norm one # img = img[::-1, : ,::-1].astype(np.uint8) img = img.astype(np.uint8) img = cv2.resize(img, (w, h)) self.logger.info("[GL Process] drawing image to image viewport ...") # show mappoints if datum.attrs.get('points', None) is not None: points = datum.attrs['points'] if len(points) > 0: # self.points.extend(points) for point in points: self.points[point.seq] = point # colors = np.array([p.color if p.color is not None else np.array([1.0, 0.0, 0.0]) for p in points]).astype(np.float64) colors = [p.color / 255. if p.color is not None else np.array([1.0, 1.0, 0.0]) for p in points] # print("colors: \n%s" % np.array(colors)) # colors = [ [1., 1., 0.] for p in points] # self.colors.extend(colors) for i, color in enumerate(colors): point = points[i] self.colors[point.seq] = color self.logger.info("[GL Process] drawing %d points" % len(points)) # print("new mappoints: \n%s" % np.array([ p.data for p in points]).astype(np.float64)) # print("new colors (default): \n%s" % np.array(colors)) else: self.logger.info("[GL Process] no points to be drawn.") # redundant points if datum.attrs.get('points-redundant', None) is not None: points = datum.attrs['points-redundant'] colors = datum.attrs['colors-redundant'] for i, p in enumerate(points): self.redundant_points.append(p) self.redundant_colors.append(colors[i] / 255.) # show landmarks if datum.attrs.get('landmarks', None) is not None: landmarks = datum.attrs['landmarks'] for landmark in landmarks: self.landmarks[landmark.seq] = landmark self.logger.info("[GL Process] drawing %d landmarks" % len(landmarks)) self.attributes.task_done() self.logger.info("[GL Process] datum has been processed.") ############ # draw graph ############ line_geometries = np.array([ [*line[0], *line[1]] for line in self.lines ]) if len(line_geometries) > 0: gl.glLineWidth(1) gl.glColor3f(0.0, 1.0, 0.0) pangolin.DrawLines(line_geometries, 3) # pose = self.camera.pose pose = camera_pose # GL 2.0 API gl.glPointSize(4) gl.glColor3f(1.0, 0.0, 0.0) gl.glBegin(gl.GL_POINTS) gl.glVertex3d(pose[0, 1], pose[1, 3], pose[2, 3]) gl.glEnd() ############ # draw poses ############ if len(self.poses) > 0: gl.glLineWidth(1) gl.glColor3f(0.0, 0.0, 1.0) # poses: numpy.ndarray[float64], w: float=1.0, h_ratio: float=0.75, z_ratio: float=0.6 pangolin.DrawCameras(np.array(self.poses)) gl.glPointSize(4) gl.glColor3f(0.0, 0.0, 1.0) gl.glBegin(gl.GL_POINTS) for pose in self.poses: gl.glVertex3d(pose[0, 1], pose[1, 3], pose[2, 3]) gl.glEnd() ################ # draw mappoints ################ if len(self.points) > 0: # points_geometries = np.array([p.data for p in self.points]).astype(np.float64) points_geometries = [] colors = [] for point_key, p in self.points.items(): points_geometries.append(p.data) colors.append(self.colors[point_key]) points_geometries = np.array(points_geometries).astype(np.float64) colors = np.array(colors).astype(np.float64) gl.glPointSize(6) # pangolin.DrawPoints(points_geometries, np.array(self.colors).astype(np.float64) ) pangolin.DrawPoints(points_geometries, colors) # gl.glPointSize(4) # gl.glColor3f(1.0, 0.0, 0.0) # # gl.glBegin(gl.GL_POINTS) # for point in points_geometries: # gl.glVertex3d(point[0], point[1], point[2]) # gl.glEnd() #################### # redundant points # #################### if len(self.redundant_points) > 0: points_geometries = [] colors = [] for i, p in enumerate(self.redundant_points): points_geometries.append(p.data) colors.append(self.redundant_colors[i]) points_geometries = np.array(points_geometries).astype(np.float64) colors = np.array(colors).astype(np.float64) gl.glPointSize(3) pangolin.DrawPoints(points_geometries, colors) ################ # draw landmarks ################ for key, landmarkDatum in self.landmarks.items(): # abox = landmark.computeAABB() # drawABox(abox.toArray()) drawABox(landmarkDatum.abox.toArray(), color=landmarkDatum.color) pass ############# # draw images ############# # self.camera.RenderImg(img) texture.Upload(img, gl.GL_RGB, gl.GL_UNSIGNED_BYTE) image_viewport.Activate() gl.glColor3f(1.0, 1.0, 1.0) texture.RenderToViewport() pangolin.FinishFrame() print("gl program loop stopped") with self._state_locker: self._stopped = True
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) panel = pangolin.CreatePanel('menu') panel.SetBounds(0.5, 1.0, 0.0, 175 / 1024.) # checkbox if self.view_point_cloud: m_show_points = pangolin.VarBool('menu.Show Points', True, True) m_show_keyframes = pangolin.VarBool('menu.Show KeyFrames', True, True) m_show_graph = pangolin.VarBool('menu.Show Graph', True, True) m_show_image = pangolin.VarBool('menu.Show Image', True, True) # button m_next_frame = pangolin.VarBool('menu.Next', False, False) if self.config is None: viewpoint_x = 0 viewpoint_y = -500 # -10 viewpoint_z = -100 # -0.1 viewpoint_f = 2000 camera_width = 1. width, height = 350, 250 else: viewpoint_x = self.config.view_viewpoint_x viewpoint_y = self.config.view_viewpoint_y viewpoint_z = self.config.view_viewpoint_z viewpoint_f = self.config.view_viewpoint_f camera_width = self.config.view_camera_width width = self.config.view_image_width * 2 height = self.config.view_image_height 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) # 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.0, 1.0, 175 / 1024., 1.0, -1024 / 768.) dcam.SetHandler(pangolin.Handler3D(scam)) # Dilay image 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') pose = pangolin.OpenGlMatrix() # identity matrix active = [] graph = [] loops = [] loops_local = [] mappoints = DynamicArray(shape=(3, )) colors = DynamicArray(shape=(3, )) cameras = DynamicArray(shape=(4, 4)) while not pangolin.ShouldQuit(): if not self.q_pose.empty(): pose.m = self.q_pose.get() 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) # Show graph if not self.q_graph.empty(): graph = self.q_graph.get() loops = np.array([_[0] for _ in graph if _[1] == 2]) loops_local = np.array([_[0] for _ in graph if _[1] == 1]) graph = np.array([_[0] for _ in graph if _[1] == 0]) if m_show_graph.Get(): if len(graph) > 0: gl.glLineWidth(1) gl.glColor3f(0.0, 1.0, 0.0) pangolin.DrawLines(graph, 3) if len(loops) > 0: gl.glLineWidth(2) gl.glColor3f(1.0, 0.0, 1.0) pangolin.DrawLines(loops, 4) if len(loops_local) > 0: gl.glLineWidth(2) gl.glColor3f(1.0, 1.0, 0.0) pangolin.DrawLines(loops_local, 4) if self.view_point_cloud: # Show mappoints if not self.q_points.empty(): pts, code = self.q_points.get() cls, code = self.q_colors.get() if code == 1: # append new points mappoints.extend(pts) colors.extend(cls) elif code == 0: # refresh all points mappoints.clear() mappoints.extend(pts) colors.clear() colors.extend(cls) if m_show_points.Get(): gl.glPointSize(2) # easily draw millions of points pangolin.DrawPoints(mappoints.array(), colors.array()) if not self.q_active.empty(): active = self.q_active.get() gl.glPointSize(3) gl.glBegin(gl.GL_POINTS) gl.glColor3f(1.0, 0.0, 0.0) for point in active: gl.glVertex3f(*point) gl.glEnd() # Show cameras if not self.q_camera.empty(): cams = self.q_camera.get() if len(cams) > 20: cameras.clear() cameras.extend(cams) if m_show_keyframes.Get(): if cameras.array().shape[0] > 0: gl.glLineWidth(1) gl.glColor3f(0.0, 0.0, 1.0) pangolin.DrawCameras(cameras.array()[:-1], camera_width) gl.glLineWidth(1) gl.glColor3f(1.0, 0.0, 0.0) pangolin.DrawCameras( np.expand_dims(cameras.array()[-1], axis=0), camera_width) # Show image if not self.q_image.empty(): image = self.q_image.get() if image.ndim == 3: image = image[::-1, :, ::-1] else: image = np.repeat(image[::-1, :, np.newaxis], 3, axis=2) image = cv2.resize(image, (width, height)) if m_show_image.Get(): texture.Upload(image, gl.GL_RGB, gl.GL_UNSIGNED_BYTE) dimg.Activate() gl.glColor3f(1.0, 1.0, 1.0) texture.RenderToViewport() if pangolin.Pushed(m_next_frame): self.q_next.put(True) pangolin.FinishFrame()
# print("******** : ") # print(np.linalg.inv(pose)) # print(pose) # Ow = pose[:3, 3] # Xw = 0.1*pose[:3, :3].dot(np.array([1, 0, 0]).T) + Ow # Yw = 0.1*pose[:3, :3].dot(np.array([0, 1, 0]).T) + Ow # Zw = 0.1*pose[:3, :3].dot(np.array([0, 0, 1]).T) + Ow # print(Ow, Xw, Yw, Zw) pangolin.CreateWindowAndBind("Trajectory 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) scam = pangolin.OpenGlRenderState( pangolin.ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000), pangolin.ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)) handler = pangolin.Handler3D(scam) dcam = pangolin.CreateDisplay() dcam.SetBounds(0.0, 1.0, 0.0, 1.0, -1024.0 / 768.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) gl.glLineWidth(2) Ow = np.array([0.0, 0.0, 0.0]).T for pose in poses: Ow = pose[:3, 3]
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()
def display(self): points = None W = 990 H = 540 pangolin.CreateWindowAndBind('JSLAM', W, H) gl.glEnable(gl.GL_DEPTH_TEST) # Define Projection and initial ModelView matrix scam = pangolin.OpenGlRenderState( pangolin.ProjectionMatrix(W, H, 420, 420, 320, 240, 0.2, 100), pangolin.ModelViewLookAt(0, 2, 15, 2, -3, -5, 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) x = 0 y = 0 z = 0 # Perspective coordinates xc = 0 yc = 0 zc = 0 animation_counter = 0 while not pangolin.ShouldQuit(): failed_to_load = False try: points = pickle.load(open("data/points.p", "rb")) except Exception: failed_to_load = True pass if not failed_to_load: self.realPoints = [] for i, (xp, yp) in enumerate(points): self.realPoints.append([xp, yp, z]) gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT) gl.glClearColor(0, 0, 0, 0) dcam.Activate(scam) # Draw Point Cloud if not failed_to_load: points = np.random.random((100000, 3)) * 10 gl.glPointSize(2) gl.glColor3f(1.0, 0.0, 0.0) pangolin.DrawPoints(points) # Load the camera print("Animation counter: {}".format(animation_counter)) pose = np.identity(4) pose[:3, 3] = [x, y, z] gl.glLineWidth(2) gl.glColor3f(0.0, 1.0, 0.0) pangolin.DrawCamera(pose, 0.5, 0.75, 0.8) if not self.creative_mode or animation_counter > 100: zc += 0.1 scam = pangolin.OpenGlRenderState( pangolin.ProjectionMatrix(W, H, 420, 420, 320, 240, 0.2, 100), pangolin.ModelViewLookAt(0, 2, 15 + zc, 2, -3, -5, pangolin.AxisDirection.AxisY)) handler = pangolin.Handler3D(scam) dcam.SetBounds(0.0, 1.0, 0.0, 1.0, -640.0 / 480.0) dcam.SetHandler(handler) z += 0.1 animation_counter += 1 pangolin.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_init(self, w, h): # pangolin.ParseVarsFile('app.cfg') pangolin.CreateWindowAndBind('Map Viewer', w, h) gl.glEnable(gl.GL_DEPTH_TEST) viewpoint_x = 0 viewpoint_y = -40 viewpoint_z = -80 viewpoint_f = 1000 self.proj = pangolin.ProjectionMatrix(w, h, viewpoint_f, viewpoint_f, w // 2, h // 2, 0.1, 5000) self.look_view = pangolin.ModelViewLookAt(viewpoint_x, viewpoint_y, viewpoint_z, 0, 0, 0, 0, -1, 0) self.scam = pangolin.OpenGlRenderState(self.proj, self.look_view) self.handler = pangolin.Handler3D(self.scam) # Create Interactive View in window self.dcam = pangolin.CreateDisplay() self.dcam.SetBounds(0.0, 1.0, kUiWidth / w, 1.0, -w / h) self.dcam.SetHandler(pangolin.Handler3D(self.scam)) self.panel = pangolin.CreatePanel('ui') self.panel.SetBounds(0.0, 1.0, 0.0, kUiWidth / w) self.do_follow = True self.is_following = True self.draw_cameras = True self.draw_covisibility = True self.draw_spanning_tree = True self.draw_loops = True #self.button = pangolin.VarBool('ui.Button', value=False, toggle=False) self.checkboxFollow = pangolin.VarBool('ui.Follow', value=True, toggle=True) self.checkboxCams = pangolin.VarBool('ui.Draw Cameras', value=True, toggle=True) self.checkboxCovisibility = pangolin.VarBool('ui.Draw Covisibility', value=True, toggle=True) self.checkboxSpanningTree = pangolin.VarBool('ui.Draw Tree', value=True, toggle=True) self.checkboxGrid = pangolin.VarBool('ui.Grid', value=True, toggle=True) self.checkboxPause = pangolin.VarBool('ui.Pause', value=False, toggle=True) #self.float_slider = pangolin.VarFloat('ui.Float', value=3, min=0, max=5) #self.float_log_slider = pangolin.VarFloat('ui.Log_scale var', value=3, min=1, max=1e4, logscale=True) self.int_slider = pangolin.VarInt('ui.Point Size', value=kDefaultPointSize, min=1, max=10) self.pointSize = self.int_slider.Get() self.Twc = pangolin.OpenGlMatrix() self.Twc.SetIdentity()
import numpy as np import OpenGL.GL as gl import pangolin state = np.load("traj_full.npy", allow_pickle=True) w = 1024 h = 768 pangolin.CreateWindowAndBind('Map Viewer', w, h) gl.glEnable(gl.GL_DEPTH_TEST) scam = pangolin.OpenGlRenderState( pangolin.ProjectionMatrix(w, h, 420, 420, w // 2, h // 2, 0.2, 10000), pangolin.ModelViewLookAt(0, -10, -8, 0, 0, 0, 0, -1, 0)) 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) # hack to avoid small Pangolin, no idea why it's *2 dcam.Resize(pangolin.Viewport(0, 0, w * 2, h * 2)) dcam.Activate() while not pangolin.ShouldQuit(): 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) if state is not None: if state[0].shape[0] >= 2:
import numpy as np import OpenGL.GL as gl import pangolin 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 gl.glPointSize(2) gl.glColor3f(1.0, 0.0, 0.0) pangolin.DrawPoints(points)
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()
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.0, 1.0, 175 / 1024., 1.0, -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 UpdatePointsThread(self, PointsQueue, PoseQueue): import OpenGL.GL as gl import pangolin 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)) scam = pangolin.OpenGlRenderState( pangolin.ProjectionMatrix(640, 480, 420, 420, 320, 240, 0.2, 10000), 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) points = None cameras = None while not pangolin.ShouldQuit(): if points is None: try: points = PointsQueue.get() except: continue else: points = np.vstack((points, PointsQueue.get())) if cameras is None: try: cameras = [PoseQueue.get()] except: continue else: cameras.append(PoseQueue.get()) 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() gl.glPointSize(2) gl.glColor3f(0.0, 1.0, 0.0) pangolin.DrawPoints(points) gl.glColor3f(1.0, 0.0, 0.0) pangolin.DrawCameras(cameras) # gl.glPointSize(5) # gl.glColor3f(1.0, 0.0, 0.0) # pangolin.DrawPoints(self.state[1], self.state[2]) #assert 1==0 pangolin.FinishFrame()