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