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 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(4): 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 = inverse_warp.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 point_cloud.points = o3d.open3d.Vector3dVector(points) point_cloud.colors = o3d.open3d.Vector3dVector(colors) vis.add_geometry(point_cloud) loop = True while loop: key = cv2.waitKey(10) if key == 27: exit() elif key != -1: loop = False vis.update_geometry() vis.poll_events() vis.update_renderer() cv2.imshow("target and depth", img) for i, (warp, diff) in enumerate(zip(warp_imgs, diff_imgs)): cv2.imshow("warp scale: %d" % i, warp) cv2.imshow("diff scale: %d" % i, diff)
def main(): dataset = SequenceDataset(sys.argv[1]) vis = o3d.Visualizer() vis.create_window() point_cloud = o3d.geometry.PointCloud() vis.add_geometry(point_cloud) for i in range(0, len(dataset)): tgt, refs, K, Kinv, sparse, dense, tgt_pose, ref_pose = dataset[i] print(tgt_pose) exit() pose_b = pose.unsqueeze(0) dense_b = dense.unsqueeze(0).unsqueeze(0) ref_b = refs[1].unsqueeze(0) K_b = K.unsqueeze(0) warp, valid, world, projpixel, grid = inverse_warp.inverse_warp( ref_b, dense_b, pose_b, K_b) # Vizualisation colors = (ref_b[0, :].view(3, -1).transpose(1, 0) + 1) / 2 points = world[0, :3].view(3, -1).transpose(1, 0) point_cloud.points = o3d.open3d.Vector3dVector(points) point_cloud.colors = o3d.open3d.Vector3dVector(colors) vis.add_geometry(point_cloud) img = torch.cat((refs[0], tgt, refs[1]), dim=1) while cv2.waitKey(10) != 27: vis.update_geometry() vis.poll_events() vis.update_renderer() cv2.imshow("img", viz.tensor2img(img)) cv2.imshow("dense", viz.tensor2depthimg(dense)) cv2.imshow("warp", viz.tensor2img(warp.squeeze(0)))
def sfm_inspector(data): global K K = utils.torch_to_numpy(data["K"]) img = torch.cat((data["refs"][0], data["tgt"], data["refs"][1]), dim=1) cv2.imshow("img", viz.tensor2img(img)) cv2.imshow("gt_sparse", viz.tensor2depthimg(data["gt_sparse"]))
def _step_fn(self, step, inputs): # Forward pass and loss with torch.no_grad(): loss, data = utils.forward_pass(self.model, self.loss_fn, inputs) print(f"loss {loss.item():.3f}") for i in range(data["pose"].shape[1]): pose = list(data["pose"][0, i, :].cpu().detach().numpy()) #print("pose %d -> x: %.6f, y: %.6f, z: %.6f, rx: %.6f, ry: %.6f, rz: %.6f" % (i, *pose)) poses = data["pose"] T0 = utils.torch_to_numpy( geometry.to_homog_matrix(geometry.pose_vec2mat( poses[:, 1])).squeeze(0)) T1 = np.identity(4) T1[:3, 3] = 0 T2 = utils.torch_to_numpy( geometry.to_homog_matrix(geometry.pose_vec2mat( poses[:, 0])).squeeze(0)) T_gt = utils.torch_to_numpy(data["T"].squeeze(0)) T0_gt = T_gt[0] T1_gt = np.identity(4) T1_gt[:3, 3] = 0 T2_gt = T_gt[1] Ta, Tb, Tc = T0.copy(), T1.copy(), T2.copy() Ta_gt, Tb_gt, Tc_gt = T0_gt.copy(), T1_gt.copy(), T2_gt.copy() # Trajectory if self.prev_tgt_i != data["tgt_i"] - 1 or self.scale is None: self.positions = [] # New sequence! self.positions_gt = [] self.scale = np.linalg.norm(Tc_gt[:3, -1] - Ta_gt[:3, -1] ) / np.linalg.norm(Tc[:3, -1] - Ta[:3, -1]) self.prev_tgt_i = data["tgt_i"] Ta_gt[:3, -1] /= self.scale Tc_gt[:3, -1] /= self.scale print(Tc_gt) print(Tc) if len(self.positions) == 0: self.positions = [Ta, Tb, Tc] self.positions_gt = [Ta_gt, Tb_gt, Tc_gt] else: inv = np.linalg.pinv(self.positions[-1]) self.positions = [inv @ T for T in self.positions] self.positions.append(Tc) inv_gt = np.linalg.pinv(self.positions_gt[-1]) self.positions_gt = [inv_gt @ T_gt for T_gt in self.positions_gt] self.positions_gt.append(Tc_gt) # Debug images depth_img = viz.tensor2depthimg(data["depth"][0][0, 0]) tgt_img = viz.tensor2img(data["tgt"][0]) img = np.concatenate((tgt_img, depth_img), axis=1) tgtrefs = viz.tensor2img( torch.cat((data["refs"][0, 0], data["tgt"][0], data["refs"][0, 1]), dim=1)) points, colors = to_points_3d(data["tgt"][0], data["depth"][0], data["K"]) loop = True while loop: key = cv2.waitKey(10) if key == 27 or self.renderer.should_quit(): exit() elif key != -1: loop = False cv2.imshow("target and depth", img) cv2.imshow("target and refs", tgtrefs) self.renderer.clear_screen() self.renderer.draw_points(points, colors) line = [T[:3, 3] for T in self.positions] line_gt = [T[:3, 3] for T in self.positions_gt] self.renderer.draw_line(line, color=(1., 0., 0.)) self.renderer.draw_line(line_gt, color=(0., 1., 0.)) #self.renderer.draw_cameras([T0], color=(1.,0.,0.)) #self.renderer.draw_cameras([T1], color=(0.,1.,0.)) #self.renderer.draw_cameras([T2], color=(0.,0.,1.)) self.renderer.finish_frame()
if self.load_gt: sparse = np.load(paths[0][:-4] + ".npy").astype(np.float32) dense = np.load(paths[0][:-4] + "_dense.npy").astype( np.float32) tgt_pose = self.poses[i][tgt] ref_pose = [self.poses[i][tgt - 1], self.poses[i][tgt + 1]] data = { **data, "gt_sparse": torch.tensor(sparse).unsqueeze(0), "gt_dense": torch.tensor(dense).unsqueeze(0), "gt_pose": torch.tensor(0) } return data if __name__ == '__main__': dataset = SequenceDataset(sys.argv[1]) for i in range(0, len(dataset)): data = dataset[i] print(data["gt_sparse"].shape) #print(relative_transform(tgt_pose, ref_pose[0])) #print(relative_transform(tgt_pose, ref_pose[1])) #print("---") img = torch.cat((data["refs"][0], data["tgt"], data["refs"][1]), dim=1) cv2.imshow("img", viz.tensor2img(img)) cv2.imshow("gt_sparse", viz.tensor2depthimg(data["gt_sparse"])) cv2.imshow("gt_dense", viz.tensor2depthimg(data["gt_dense"])) key = cv2.waitKey(0) if key == 27: break