def keyPressEvent(obj, event): global frame_idx global refer_T key = obj.GetKeySym() if key == 'Right': # vis.clear_frame_poses() if frame_idx > 305: return cur_frame = frames.frames[frame_idx] cur_Tcw = cur_frame['extrinsic_Tcw'] cur_name = cur_frame['file_name'] cur_depth_name = cur_frame['depth_file_name'] print(cur_name) K = K_from_frame(cur_frame) # Read image cur_img = cv2.imread(os.path.join(base_dir, cur_name)).astype( np.float32) / 255.0 cur_depth = load_depth_from_png(os.path.join(base_dir, cur_depth_name), div_factor=5000) # Crop with new intrinsic cur_img = crop_by_intrinsic(cur_img, K, in_K, interp_method='nearest') # next_img = crop_by_intrinsic(next_img, K, in_K) cur_depth = crop_by_intrinsic(cur_depth, K, in_K, interp_method='nearest') h, w, c = cur_img.shape # rel_T = cam_opt.relateive_pose(cur_Tcw[:3, :3], cur_Tcw[:3, 3], next_Tcw[:3, :3], next_Tcw[:3, 3]) X_3d = cam_opt.pi_inv(K, x_2d.reshape((h * w, 2)), cur_depth.reshape((h * w, 1))) cur_Twc = cam_opt.camera_pose_inv(cur_Tcw[:3, :3], cur_Tcw[:3, 3]) X_3d = cam_opt.transpose(cur_Twc[:3, :3], cur_Twc[:3, 3], X_3d) vis.set_point_cloud(X_3d, cur_img.reshape((h * w, 3))) vis.add_frame_pose(cur_Tcw[:3, :3], cur_Tcw[:3, 3], camera_obj_scale=0.01) frame_idx += 1 return
def load_frame_2_tensors(self, frame, out_frame_dim): C, H, W = out_frame_dim K = self.depth_k.copy() Tcw = np.asarray(frame['extrinsic_Tcw'], dtype=np.float32).reshape( (3, 4)) Rcw, tcw = Tcw[:3, :3], Tcw[:3, 3] img_file_name = frame['file_name'] depth_file_name = frame['depth_file_name'] # Load image and depth img = cv2.imread(os.path.join(self.base_dir, img_file_name)) ori_H, ori_W, _ = img.shape img = crop_by_intrinsic(img, self.img_k, self.depth_k) img = cv2.resize(img, (ori_W, ori_H)) depth = read_7scenese_depth( os.path.join(self.base_dir, depth_file_name)) # Post-process image and depth (fill the holes with cross bilateral filter) resize_ratio = max(H / ori_H, W / ori_W) img = cv2.resize(img, dsize=(int(resize_ratio * ori_W), int(resize_ratio * ori_H))) depth = cv2.resize(depth, dsize=(int(resize_ratio * ori_W), int(resize_ratio * ori_H)), interpolation=cv2.INTER_NEAREST) if self.fill_depth_holes: depth = fill_depth_cross_bf(img, depth) depth[depth < 1e-5] = 1e-5 img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB).astype(np.float32) / 255.0 # camera intrinsic parameters: K[0, 0] *= resize_ratio K[0, 2] = (resize_ratio * ori_W) / 2 K[1, 1] *= resize_ratio K[1, 2] = (resize_ratio * ori_H) / 2 new_K = K.copy() new_K[0, 2] = W / 2 new_K[1, 2] = H / 2 # crop and resize with new K img = crop_by_intrinsic(img, K, new_K) depth = crop_by_intrinsic(depth, K, new_K, interp_method='nearest') # camera motion representation: (center, rotation_center2world) c = camera_center_from_Tcw(Rcw, tcw) Rwc = np.eye(4) Rwc[:3, :3] = Rcw.T q = quaternion_from_matrix(Rwc) log_q = log_quat(q) pose_vector = np.concatenate((c, log_q)).astype(np.float32) # convert to torch.tensor ori_img_tensor = torch.from_numpy(img.transpose( (2, 0, 1))) # (C, H, W) img_tensor = ori_img_tensor.clone() if self.transform_func: img_tensor = self.transform_func(img_tensor) depth_tensor = torch.from_numpy(depth).view(1, H, W) # (1, H, W) pose_vector = torch.from_numpy(pose_vector) # (1, 3) Tcw_tensor = torch.from_numpy(Tcw) # (3, 4) K_tensor = torch.from_numpy(new_K) # (3, 3) return pose_vector, img_tensor, depth_tensor, K_tensor, Tcw_tensor, ori_img_tensor