def visualize_3D(p3D): pangolin.CreateWindowAndBind('main', 640, 480) gl.glEnable(gl.GL_DEPTH_TEST) 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) 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) #gl.glPointSize(5); #gl.glColor3f(0.0,0.0,1.0); #pangolin.DrawPoints(p3D); # Attempt to draw camera gl.glLineWidth(2) gl.glColor3f(1.0, 0.0, 0.0) #pose = np.identity(4); pose = (np.hstack((pose1.T, np.zeros((4, 1))))).T pangolin.DrawCamera(pose, 0.5, 0.75, 0.8) pangolin.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) gl.glLineWidth(3) gl.glColor3f(0.0, 1.0, 0.0) pangolin.DrawCamera(np.identity(4)) # 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): 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 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 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 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(): pango.CreateWindowAndBind('frustrum render', 640, 480) # Projection and ModelView Matrices scam = pango.OpenGlRenderState( pango.ProjectionMatrix(640, 480, 2000, 2000, 320, 240, 0.1, 5000), pango.ModelViewLookAt(0, -50, -10, 0, 0, 0, 0, -1, 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 and append pose matrices for cameras pose = np.identity(4) poses = [] for i in range(3): poses.append(np.linalg.inv(pose)) pose[2, 3] -= 1 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) # Render Cameras gl.glLineWidth(2) gl.glColor3f(1.0, 0.0, 1.0) pango.DrawCamera(poses[0]) gl.glColor3f(0.2, 1.0, 0.2) pango.DrawCameras(poses[1:-1]) gl.glColor3f(1.0, 1.0, 1.0) pango.DrawCamera(poses[-1]) # End frame update pango.FinishFrame()
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 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 draw(self, poseList_nx4x4, drawCamera=False): while not pangolin.ShouldQuit(): gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT) gl.glClearColor(1.0, 1.0, 1.0, 1.0) self._dcam.Activate(self._scam) # pangolin.glDrawColouredCube() # points = np.random.random((10000, 3)) *10 gl.glPointSize(2) gl.glColor3f(1.0, 0.0, 0.0) pangolin.DrawLine(np.array([[0, 0, 0], [1.0, 0.0, 0.0]])) gl.glColor3f(0.0, 1.0, 0.0) pangolin.DrawLine(np.array([[0, 0, 0], [0.0, 1.0, 0.0]])) gl.glColor3f(.0, 0.0, 1.0) pangolin.DrawLine(np.array([[0, 0, 0], [0.0, 0.0, 1.0]])) origin = [[0, 0, 0]] originHomo = h**o(np.zeros((3, 1))) originT = np.eye(4) # pangolin.Draw index = 0 for T in poseList_nx4x4: originT = originT.dot(T) if drawCamera: gl.glColor3f(0.5, 0.75, 0.8) pangolin.DrawCamera(originT) pAfter = originT.dot(originHomo) p = unHomo(pAfter) origin.append([p[0][0], p[1][0], p[2][0]]) # line if np.mod(index, 2) == 0: gl.glColor3f(0.0, 0.0, 0.0) else: gl.glColor3f(0.0, 1.0, 0.0) pangolin.DrawLine(origin) index += 1 # time.sleep(0.01) pangolin.FinishFrame()
def draw_camera(pose, color=(0.0, 1.0, 0.0)): assert pose.shape == (4, 4) assert len(color) == 3 width = 0.2 height_ratio = 0.75 z_ratio = 0.8 gl.glLineWidth(1) gl.glColor3f(color[0], color[1], color[2]) # convert pose to reflect camera position with respect to world coordinate origin # TODO wtf why doesn't this work? # new_pose = util.rotation_translation_to_pose(util.pose_to_rotation(pose).transpose(), # util.pose_to_translation(pose)*-1) # do this instead of the above new_pose = np.identity(4) new_pose[:3, :3] = pose[:3, :3].transpose() new_pose[:3, 3] = pose[:3, 3] * -1 pangolin.DrawCamera(new_pose, width, height_ratio, z_ratio)
def t_draw(self): while not pangolin.ShouldQuit(): gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT) gl.glClearColor(1.0, 1.0, 1.0, 1.0) self._dcam.Activate(self._scam) pangolin.glDrawColouredCube() # points = np.random.random((10000, 3)) *10 gl.glPointSize(2) gl.glColor3f(1.0, 0.0, 0.0) # pangolin.DrawPoints(points) for theta in np.arange(0, np.pi, np.pi / 10): for psi in np.arange(0, np.pi * 2, np.pi / 10): T = makeT(Y(theta).dot(Z(psi)), np.random.random((3, 1)) * 10) pangolin.DrawCamera(T) time.sleep(0.4) # pangolin.DrawCamera(np.eye(4)) pangolin.FinishFrame()
def draw_cameras(self, cam): gl.glColor3f(0.0, 1.0, 0.0) for c in cam: pangolin.DrawCamera(c, 0.35, 0.75, 0.8)
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 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 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 viewer_refresh(self, qmap, qvo, is_paused): while not qmap.empty(): self.map_state = qmap.get() while not qvo.empty(): self.vo_state = qvo.get() # if pangolin.Pushed(self.button): # print('You Pushed a button!') self.do_follow = self.checkboxFollow.Get() self.is_grid = self.checkboxGrid.Get() self.draw_cameras = self.checkboxCams.Get() self.draw_covisibility = self.checkboxCovisibility.Get() self.draw_spanning_tree = self.checkboxSpanningTree.Get() #if pangolin.Pushed(self.checkboxPause): if self.checkboxPause.Get(): is_paused.value = 0 else: is_paused.value = 1 # self.int_slider.SetVal(int(self.float_slider)) self.pointSize = self.int_slider.Get() if self.do_follow and self.is_following: self.scam.Follow(self.Twc, True) elif self.do_follow and not self.is_following: self.scam.SetModelViewMatrix(self.look_view) self.scam.Follow(self.Twc, True) self.is_following = True elif not self.do_follow and self.is_following: self.is_following = False gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT) gl.glClearColor(1.0, 1.0, 1.0, 1.0) self.dcam.Activate(self.scam) if self.is_grid: Viewer3D.drawPlane() # ============================== # draw map if self.map_state is not None: if self.map_state.cur_pose is not None: # draw current pose in blue gl.glColor3f(0.0, 0.0, 1.0) gl.glLineWidth(2) pangolin.DrawCamera(self.map_state.cur_pose) gl.glLineWidth(1) self.updateTwc(self.map_state.cur_pose) if self.map_state.predicted_pose is not None and kDrawCameraPrediction: # draw predicted pose in red gl.glColor3f(1.0, 0.0, 0.0) pangolin.DrawCamera(self.map_state.predicted_pose) if len(self.map_state.poses) > 1: # draw keyframe poses in green if self.draw_cameras: gl.glColor3f(0.0, 1.0, 0.0) pangolin.DrawCameras(self.map_state.poses[:]) # rysowaine wektora za którym powinno wyznaczać boxy if len(self.map_state.line) > 0: # # wyznaczam nowy wektor obrócony o -90 i 90 stopni o długości 10 gl.glLineWidth(3) gl.glPointSize(self.pointSize) gl.glColor3f(0.0, 0.0, 0.0) pangolin.DrawPoints(self.map_state.line) # # pangolin.DrawLines([self.map_state.line[0], self.map_state.line[0]]) if len(self.map_state.points) > 0: # draw keypoints with their color gl.glPointSize(self.pointSize) #gl.glColor3f(1.0, 0.0, 0.0) pangolin.DrawPoints(self.map_state.points, self.map_state.colors) # TODO dopisać rozwiązanie drukujące na ekran zaznaczone obiekty w postaci obszarów if len(self.map_state.dict_pose) > 0: for k, v in self.map_state.dict_pose.items(): sizes = [] poses = [np.identity(4) for i in range(len(v))] for pose, point in zip(poses, v): pose[:3, 3] = np.array(point[0]) sizes.append(np.array(point[1])) gl.glLineWidth(2) d_color = k[1:-1].split(' ') gl.glColor3f(float(d_color[0]), float(d_color[1]), float(d_color[2])) # gl.glColor3f(1.0, 0.0, 1.0) # gl.glColor3f(self.map_state.box_color) # print("{} {}".format(poses, sizes)) pangolin.DrawBoxes(poses, sizes) # pangolin.DrawBoxes(self.map_state.box_left_botton, self.map_state.box_size) if self.map_state.reference_pose is not None and kDrawReferenceCamera: # draw predicted pose in purple gl.glColor3f(0.5, 0.0, 0.5) gl.glLineWidth(2) pangolin.DrawCamera(self.map_state.reference_pose) gl.glLineWidth(1) if len(self.map_state.covisibility_graph) > 0: if self.draw_covisibility: gl.glLineWidth(1) gl.glColor3f(0.0, 1.0, 0.0) pangolin.DrawLines(self.map_state.covisibility_graph, 3) if len(self.map_state.spanning_tree) > 0: if self.draw_spanning_tree: gl.glLineWidth(1) gl.glColor3f(0.0, 0.0, 1.0) pangolin.DrawLines(self.map_state.spanning_tree, 3) if len(self.map_state.loops) > 0: if self.draw_spanning_tree: gl.glLineWidth(2) gl.glColor3f(0.5, 0.0, 0.5) pangolin.DrawLines(self.map_state.loops, 3) gl.glLineWidth(1) # ============================== # draw vo if self.vo_state is not None: if self.vo_state.poses.shape[0] >= 2: # draw poses in green if self.draw_cameras: gl.glColor3f(0.0, 1.0, 0.0) pangolin.DrawCameras(self.vo_state.poses[:-1]) if self.vo_state.poses.shape[0] >= 1: # draw current pose in blue gl.glColor3f(0.0, 0.0, 1.0) current_pose = self.vo_state.poses[-1:] pangolin.DrawCameras(current_pose) self.updateTwc(current_pose[0]) if self.vo_state.traj3d_est.shape[0] != 0: # draw blue estimated trajectory gl.glPointSize(self.pointSize) gl.glColor3f(0.0, 0.0, 1.0) pangolin.DrawLine(self.vo_state.traj3d_est) if self.vo_state.traj3d_gt.shape[0] != 0: # draw red ground-truth trajectory gl.glPointSize(self.pointSize) gl.glColor3f(1.0, 0.0, 0.0) pangolin.DrawLine(self.vo_state.traj3d_gt) pangolin.FinishFrame()
def showing_mapping(): """ DESCRIPTION: Run a 3D mapping section and get live result. * first - only position """ # READ ARG. parser = argparse.ArgumentParser() parser.add_argument("path") parser.add_argument("x") parser.add_argument("y") parser.add_argument("z") parser.add_argument("r") parser.add_argument("p") parser.add_argument("y") args = parser.parse_args() matrice = euler_to_rotation( float(args.r), float(args.p), float(args.y), np.array([args.x, args.y, args.z], dtype=float)) # READ OBJ OBJECT. scene = pywavefront.Wavefront( '/home/thomas/Documents/rane_slam/mk3slam.0.3/Final/data/{0}.obj'. format(args.path)) points_3D = np.zeros((len(scene.vertices), 3)) for i in range(len(scene.vertices)): points_3D[i, :] = np.array( [scene.vertices[i][0], scene.vertices[i][1], scene.vertices[i][2]]) # PANGOLIN CONFIGURATION & INITIALISATION. w, h = 1280, 720 scam, dcam, dimg = pangolin_init(w, h) texture = pangolin.GlTexture( w, h, gl.GL_RGB, False, 0, gl.GL_RGB, gl.GL_UNSIGNED_BYTE, ) # ZED CONFIGURATION CAMERA. zed = sl.Camera() init_params = sl.InitParameters() init_params.camera_resolution = sl.RESOLUTION.HD720 # Use HD720 video mode init_params.coordinate_units = sl.UNIT.METER # Set coordinate units init_params.coordinate_system = sl.COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP zed.open(init_params) # ZED CALIBRATION. zed.get_camera_information( ).camera_configuration.calibration_parameters.left_cam # INITIALISATION OBJECT FOR MAPPING. pymesh = sl.Mesh() # Current incremental mesh. image = sl.Mat() # Left image from camera. pose = sl.Pose() # Pose object. # TRACKING PARAMETERS. tracking_parameters = sl.PositionalTrackingParameters() tracking_parameters.enable_area_memory = True tracking_parameters.area_file_path = '/home/thomas/Documents/rane_slam/mk3slam.0.3/Final/data/{0}.area'.format( args.path) # INITIALISATION POSITION t = sl.Transform() t[0, 0] = matrice[0, 0] t[1, 0] = matrice[1, 0] t[2, 0] = matrice[2, 0] t[0, 1] = matrice[0, 1] t[1, 1] = matrice[1, 1] t[2, 1] = matrice[2, 1] t[0, 2] = matrice[0, 2] t[1, 2] = matrice[1, 2] t[2, 2] = matrice[2, 2] t[0, 3] = matrice[0, 3] t[1, 3] = matrice[1, 3] t[2, 3] = matrice[2, 3] tracking_parameters.set_initial_world_transform(t) zed.enable_positional_tracking(tracking_parameters) # TIME PARAMETERS. last_call = time.time() runtime = sl.RuntimeParameters() while not pangolin.ShouldQuit(): # -clear all. 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 coordinate origine. pangolin_draw_coordinate() # -get image. zed.grab(runtime) zed.retrieve_image(image, sl.VIEW.LEFT) # -get position and spatial mapping state. zed.get_position(pose) # Display the translation and timestamp py_translation = sl.Translation() tx = round(pose.get_translation(py_translation).get()[0], 3) ty = round(pose.get_translation(py_translation).get()[1], 3) tz = round(pose.get_translation(py_translation).get()[2], 3) print("Translation: Tx: {0}, Ty: {1}, Tz {2}, Timestamp: {3}\n".format( tx, ty, tz, pose.timestamp.get_milliseconds())) # Display the orientation quaternion py_orientation = sl.Orientation() ox = round(pose.get_orientation(py_orientation).get()[0], 3) oy = round(pose.get_orientation(py_orientation).get()[1], 3) oz = round(pose.get_orientation(py_orientation).get()[2], 3) ow = round(pose.get_orientation(py_orientation).get()[3], 3) #print("Orientation: Ox: {0}, Oy: {1}, Oz {2}, Ow: {3}\n".format(ox, oy, oz, ow)) # DRAW ALL STATE. gl.glLineWidth(1) gl.glColor3f(0.0, 0.0, 1.0) # -transform brute data in numpy to draw pose. a = pose.pose_data() pose2 = np.array([[a[0, 0], a[0, 1], a[0, 2], a[0, 3]], [a[1, 0], a[1, 1], a[1, 2], a[1, 3]], [a[2, 0], a[2, 1], a[2, 2], a[2, 3]], [a[3, 0], a[3, 1], a[3, 2], a[3, 3]]]) print(pose2) pangolin.DrawCamera(pose2, 0.5, 0.75, 0.8) # -draw all points on the map. gl.glPointSize(2) gl.glColor3f(1.0, 0.0, 0.0) pangolin.DrawPoints(points_3D) # DISPLAY CAMERA VIDEO. img = cv2.cvtColor(image.get_data(), cv2.COLOR_BGRA2RGB) img = cv2.flip(img, 0) texture.Upload(img, gl.GL_RGB, gl.GL_UNSIGNED_BYTE) dimg.Activate() gl.glColor3f(1.0, 1.0, 1.0) texture.RenderToViewport() # END OF CYCLE. pangolin.FinishFrame() #sl.SPATIAL_MAPPING_STATE.NOT_ENABLED zed.disable_positional_tracking() zed.close()