def get_pt_from_img(img, depth, K, Twc):
H, W = depth.shape
x_2d = x_2d_coords(H, W).reshape((H * W, 2))
X_3d = pi_inv(K, x_2d, depth.reshape((H * W, 1)))
X_3d = transpose(Twc[:3, :3], Twc[:3, 3], X_3d)
colors = img.reshape((H * W, 3)) / 255.0
return X_3d, colors
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 keyPressEvent(obj, event):
global frame_idx
key = obj.GetKeySym()
if key == 'Right':
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']
next_frame = frames.frames[frame_idx + 1]
next_Tcw = next_frame['extrinsic_Tcw']
next_name = next_frame['file_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
next_img = cv2.imread(os.path.join(base_dir, next_name)).astype(
np.float32) / 255.0
cur_depth = load_depth_from_png(os.path.join(base_dir, cur_depth_name))
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])
frame_idx += 20
return
def get_pt_from_img(img, depth, K):
H, W = depth.shape
x_2d = x_2d_coords(H, W).reshape((H * W, 2))
X_3d = pi_inv(K, x_2d, depth.reshape((H * W, 1)))
colors = img.reshape((H * W, 3)) / 255.0
return X_3d, colors