def main(self, pts=[], colors=None, camera=None): if True: gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT) gl.glClearColor(1.0, 0.5, 0.5, 1.0) self.dcam.Activate(self.scam) # Render OpenGL Cube # pangolin.glDrawColouredCube(0.1) # Draw Point Cloud gl.glColor3f(1.0, 1.0, 0.0) # pangolin.DrawPoints(points) if len(pts) > 0: gl.glPointSize(3) if isinstance(colors, np.ndarray): pangolin.DrawPoints(pts, colors) else: pangolin.DrawPoints(pts) if isinstance(camera, list): self.draw_cameras(camera) # gl.glColor3f(0.0, 0.0, 0.0) # gl.glPointSize(1) # pangolin.DrawPoints(pts2) pangolin.FinishFrame()
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()
def draw(self): q = self.q self.draw_init() while not pangolin.ShouldQuit(): try: while not q.empty(): state = q.get() if state is not None: gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT) gl.glClearColor(0.0, 0.0, 0.0, 0.0) self.dcam.Activate(self.scam) # Draw Point Cloud gl.glPointSize(2) gl.glColor3f(0.0, 1.0, 0.0) if self.history: self.points.append(state[0]) pangolin.DrawPoints( np.reshape(np.array(self.points), (-1, 3))) else: pangolin.DrawPoints(state[0]) pangolin.FinishFrame() except: continue
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()
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) 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) # 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(5) pangolin.DrawPoints(points, colors) pangolin.FinishFrame()
def refresh(self, q): """Refresh the display if there is new data :q: TODO :returns: TODO """ while not q.empty(): self.message = q.get() if self.message is not None: map_points, poses, colors = self.message 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 poses is not None: if poses.shape[0] >= 2: gl.glColor3f(0.0, 1.0, 0.0) pangolin.DrawCameras(poses[0:-1, :]) gl.glColor3f(1.0, 1.0, 0.0) pangolin.DrawCameras(poses[-1:, :]) if map_points is not None and colors is not None: if map_points.shape[0] != 0: gl.glPointSize(3) gl.glColor3f(1.0, 0.0, 0.0) pangolin.DrawPoints(map_points, colors) pangolin.FinishFrame()
def viewer_refresh(self, q): while not q.empty(): self.points_to_draw, self.point_colors, self.poses_to_draw = 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) #pangolin.glDrawColouredCube(1) # Draw feature points with red dots. gl.glPointSize(4) gl.glColor3f(1.0, 0.0, 0.0) if self.points_to_draw is not None: pangolin.DrawPoints(self.points_to_draw, self.point_colors) # Test of drawing manual points. #gl.glColor3f(1.0, 1.0, 1.0) #pangolin.DrawPoints(np.array([[0, 0, 0], [10, 0, 0], [0, 20, 0], [0, 0, 40]])) gl.glColor3f(1.0, 1.0, 1.0) if self.poses_to_draw is not None: for pose in self.poses_to_draw: pangolin.DrawCamera(pose, 1, 0.5, 0.8) pangolin.FinishFrame() return
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) pangolin.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) 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]) pangolin.FinishFrame()
def viewer_refresh(self, q): while not q.empty(): self.data = 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) # drawing begins here # self.pose[2, 3] = self.pose[2, 3] - 0.1 # self.poses.append(np.linalg.inv(self.pose)) if self.data is not None: gl.glLineWidth(3) gl.glColor3f(0.0, 1.0, 0.0) pangolin.DrawCameras(self.data[0]) if len(self.data) > 1: gl.glPointSize(5) points = self.data[1] colors = self.data[2] pangolin.DrawPoints(points, colors) pangolin.FinishFrame()
def DISPLAY(): 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) pangolin.FinishFrame()
def main(showSemSegmentation=False): # create window 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, 50), pangolin.ModelViewLookAt(0, 0, -.01, 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, colors, sem = getPointsAndColors("000020.png") # If we want to show the semantic segmentations if showSemSegmentation: colors = colors * 0.5 + sem * 0.5 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.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 main(): pango.CreateWindowAndBind('point cloud cube render', 640, 480) # Projection and ModelView Matrices scam = pango.OpenGlRenderState( pango.ProjectionMatrix(640, 480, 420, 420, 320, 240, 0.2, 100), pango.ModelViewLookAt(-1, -1, -1, 0, 0, 0, pango.AxisDirection.AxisY)) handler = pango.Handler3D(scam) # Interactive View in Window disp_cam = pango.CreateDisplay() disp_cam.SetBounds(0.0, 1.0, 0.0, 1.0, -640.0 / 480.0) disp_cam.SetHandler(handler) # Create a random point cloud pts = np.random.random((100, 3)) * 10 # Color matrix based on point location colors = np.zeros((len(pts), 3)) colors[:, :] = 1. - pts[:, :] / 10 #colors[:] = [1.0, 0.0, 0.0] while not pango.ShouldQuit(): # Clear screen gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT) gl.glClearColor(0.15, 0.15, 0.15, 0.0) disp_cam.Activate(scam) # Draw Points gl.glPointSize(5) gl.glColor3f(0.0, 1.0, 0.0) pango.DrawPoints(pts, colors) # Finish Drawing pango.FinishFrame()
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()
def run(self): pangolin.CreateWindowAndBind(self.win_name, 640, 480) gl.glEnable(gl.GL_DEPTH_TEST) self.scam = pangolin.OpenGlRenderState( pangolin.ProjectionMatrix(640, 480, 420, 420, 320, 240, 0.2, 1000), pangolin.ModelViewLookAt(2, -10, -30, 0, 0, 0, pangolin.AxisNegY)) self.tree = pangolin.Renderable() self.tree.Add(pangolin.Axis()) self.handler = pangolin.SceneHandler(self.tree, self.scam) self.dcam = pangolin.CreateDisplay() self.dcam.SetBounds(0.0, 1.0, 0.0, 1.0, -640.0 / 480.0) self.dcam.SetHandler(self.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) self.dcam.Activate(self.scam) self.tree.Render() self.data_lock.acquire() if self.poses: gl.glPointSize(5) gl.glColor3f(*self.cam_color) for pose in self.poses: pangolin.DrawPoints(pose[1].T) if self.map_pts is not None: gl.glPointSize(2) gl.glColor3f(*self.map_pts_color) pangolin.DrawPoints(self.map_pts) if self.new_pts_obs is not None: cur_pos = self.poses[-1] cam_centers = np.repeat(cur_pos[1].T, self.new_pts_obs.shape[0], axis=0) gl.glLineWidth(1) gl.glColor3f(0.0, 0.0, 1.0) pangolin.DrawLines(cam_centers, self.new_pts_obs) self.data_lock.release() time.sleep(0.02) pangolin.FinishFrame()
def drawPoints(self, pointCloud): ''' Function to allow chosen point cloud to be drawn ''' gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT) SampleToggling.dcam.Activate(SampleToggling.scam) pangolin.DrawPoints(pointCloud) pangolin.FinishFrame()
def handler_pts(pts, attrs): if pts is None or len(pts) == 0: return if attrs: gl.glColor3f(*attrs['Color']) gl.glPointSize(attrs['PointSize']) else: gl.glColor3f(0.0, 1.0, 0.0) gl.glPointSize(2) pangolin.DrawPoints(pts)
def viewer_refresh(self, q): 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(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 draw_axes(): gl.glPointSize(1) points = np.zeros((300, 3)) points[:100, 0] = np.arange(100) points[100:200, 1] = np.arange(100) points[200:300, 2] = np.arange(100) colors = np.zeros((300, 3)) colors[:100, 0] = 1 colors[100:200, 1] = 1 colors[200:300, 2] = 1 pangolin.DrawPoints(points, colors)
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()
def viewer_refresh(self, q): if self.state is None or not q.empty(): self.state = q.get() 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.glLineWidth(1) gl.glColor3f(0.0, 1.0, 0.0) pangolin.DrawCameras(self.state[0]) gl.glPointSize(2) gl.glColor3f(1.0, 0.0, 0.0) pangolin.DrawPoints(self.state[1]) pangolin.FinishFrame()
def draw3d(self, out, cam_loc, vis_pose_check): # object points gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT) gl.glClearColor(1, 1, 1, 0) self.dcam.Activate(self.scam) # WORLD gl.glPointSize(3) gl.glColor3f(1.0, 0, 0) pangolin.DrawPoints(out) #POSE if vis_pose_check: for cam_loc1 in cam_loc: # cam pose gl.glColor3f(0, 1.0, 0) pose = np.identity(4) pose[:3, 3] = cam_loc1 #loc pangolin.DrawCamera(pose, 0.6, 0.4, 0.8) pangolin.FinishFrame()
def viewer_refresh(self, q): if self.state is None or not q.empty(): self.state = q.get() gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT) # gl.glClearColor(1.0, 1.0, 1.0, 1.0) gl.glClearColor(0.0, 0.0, 0.0, 1.0) self.dcam.Activate(self.scam) # draw poses gl.glColor3f(0.0, 1.0, 0.0) pangolin.DrawCameras(self.state[0]) # draw keypoints gl.glPointSize(5) gl.glColor3f(1.0, 0.0, 0.0) pangolin.DrawPoints(self.state[1], self.state[2]) pangolin.FinishFrame()
def render_map(self, q): if (self.currState is None) or (not q.empty()): self.currState = q.get() # Extract points and poses 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 poses #gl.glPointSize(10) gl.glColor3f(1.0, 0.0, 0.0) pangolin.DrawCameras(self.currState[0]) # Draw points gl.glPointSize(2) gl.glColor3f(0.0, 1.0, 0.0) pangolin.DrawPoints(self.currState[1]) pangolin.FinishFrame()
def cloud_for_vis(img,depth): index_matrix_2[:,2] = np.reshape(depth,(480*640)) points_in_cam_frame = index_matrix_2 #point_in_cam_frame = np.transpose(depth*index_matrix_2) # 3x480*640 points_in_world = np.matmul(camera_matrix_inv,points_in_cam_frame.T) # 3x480*640 points_in_world = np.transpose(points_in_world) # 480*640x3 points_colours = np.reshape(img,(480*640,3)) # RGB values for each point """fig = plt.figure() ax = Axes3D(fig) #ax.scatter([2,4,5,2],[3,4,2,1],[6,3,4,1]) ax.scatter(points_in_world[:,0],points_in_world[:,1],points_in_world[:,2]) plt.show()""" 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 = points_in_world#np.random.random((100000, 3)) * 10 gl.glPointSize(2) gl.glColor3f(1.0, 0.0, 0.0) pangolin.DrawPoints(points) pangolin.FinishFrame()
def viewer_refresh(self, q): if self.state == None or not q.empty(): self.state = q.get() # turn state into points, np.asarray() keep all 'd' as array, not matrix # ppts = np.array([np.asarray(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(1.0, 1.0, 1.0, 1.0) self.dcam.Activate(self.scam) # draw pose gl.glColor3f(0.0, 1.0, 0.0) pangolin.DrawCameras(self.state[0]) # draw key points gl.glPointSize(2) gl.glColor3f(1.0, 0.0, 0.0) pangolin.DrawPoints(self.state[1]) 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) 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 viewer_refresh(self, q): if self.state is None or 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: gl.glColor3f(0.0, 1.0, 0.0) pangolin.DrawCameras(self.state[0][:-1]) if self.state[0].shape[0] >= 1: gl.glColor3f(1.0, 1.0, 0.0) pangolin.DrawCameras(self.state[0][-1:]) if self.state[1].shape[0] != 0: gl.glPointSize(2) gl.glColor3f(1.0, 0.0, 0.0) pangolin.DrawPoints(self.state[1], self.state[2]) 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 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 draw_points(self, points, colors): self.dcam.Activate(self.scam) gl.glPointSize(5) pango.DrawPoints(points, colors)