def filter_seq3(seq_list: FrameSeqData, base_dir):
for seq in seq_list.frames:
pre_frame = seq[0]
center_frame = seq[1]
next_frame = seq[2]
pre_Tcw = seq_list.get_Tcw(pre_frame)
center_Tcw = seq_list.get_Tcw(center_frame)
next_Tcw = seq_list.get_Tcw(next_frame)
K_mat = seq_list.get_K_mat(center_frame)
# Read Image
pre_img_name = seq_list.get_image_name(pre_frame)
center_img_name = seq_list.get_image_name(center_frame)
next_img_name = seq_list.get_image_name(pre_frame)
pre_img = cv2.imread(os.path.join(base_dir, pre_img_name)).astype(
np.float32) / 255.0
center_img = cv2.imread(os.path.join(
base_dir, center_img_name)).astype(np.float32) / 255.0
next_img = cv2.imread(os.path.join(base_dir, next_img_name)).astype(
np.float32) / 255.0
# Read depth
pre_depth_name = seq_list.get_depth_name(pre_frame)
center_depth_name = seq_list.get_depth_name(center_frame)
next_depth_name = seq_list.get_depth_name(next_frame)
pre_depth = read_sun3d_depth(pre_depth_name)
center_depth = read_sun3d_depth(center_depth_name)
next_depth = read_sun3d_depth(next_depth_name)
import core_3dv.camera_operator as cam_opt
import core_3dv.camera_operator_gpu as cam_opt_gpu
from visualizer.visualizer_2d import show_multiple_img
from core_io.depth_io import load_depth_from_png
valid_set_dir = '/home/ziqianb/Desktop/datasets/tgz_target/'
valid_seq_name = 'rgbd_dataset_freiburg1_desk'
seq = FrameSeqData(os.path.join(valid_set_dir, valid_seq_name, 'seq.json'))
frame_a = seq.frames[5]
frame_b = seq.frames[20]
Tcw_a = seq.get_Tcw(frame_a)
Tcw_b = seq.get_Tcw(frame_b)
K = seq.get_K_mat(frame_a)
img_a = cv2.imread(os.path.join(
valid_set_dir, seq.get_image_name(frame_a))).astype(np.float32) / 255.0
img_b = cv2.imread(os.path.join(
valid_set_dir, seq.get_image_name(frame_b))).astype(np.float32) / 255.0
depth_a = load_depth_from_png(os.path.join(valid_set_dir,
seq.get_depth_name(frame_a)),
div_factor=5000.0)
depth_b = load_depth_from_png(os.path.join(valid_set_dir,
seq.get_depth_name(frame_b)),
div_factor=5000.0)
rel_T = cam_opt.relateive_pose(Tcw_a[:3, :3], Tcw_a[:3, 3], Tcw_b[:3, :3],
Tcw_b[:3, 3])
wrap_b2a, _ = cam_opt.wrapping(img_a, img_b, depth_a, K, rel_T[:3, :3],
depth_a = load_depth_from_png(os.path.join(seq_dir,
seq.get_depth_name(frame_a)),
div_factor=5000)
depth_b = load_depth_from_png(os.path.join(seq_dir,
seq.get_depth_name(frame_b)),
div_factor=5000)
img_a = cv2.imread(os.path.join(
seq_dir, seq.get_image_name(frame_a))).astype(np.float32) / 255.0
img_a = cv2.cvtColor(img_a, cv2.COLOR_BGR2RGB)
img_b = cv2.imread(os.path.join(
seq_dir, seq.get_image_name(frame_b))).astype(np.float32) / 255.0
img_b = cv2.cvtColor(img_b, cv2.COLOR_BGR2RGB)
depth_a = torch.from_numpy(depth_a).unsqueeze(0)
K = torch.from_numpy(seq.get_K_mat(frame_a)).unsqueeze(0)
H, W = depth_a.shape[1], depth_a.shape[2]
Twc_a = torch.from_numpy(seq.get_Twc(frame_a)).unsqueeze(0)
R_a, t_a = cam_opt.Rt(Twc_a)
x_2d = cam_opt.x_2d_coords(H, W, n=1)
X_3d_a = cam_opt.pi_inv(K=K, d=depth_a, x=x_2d)
X_3d_a = cam_opt.transpose(R_a, t_a, X_3d_a)
""" PnP By OpenCV
"""
# X_3d_a = X_3d_a.view(H, W, 3).cpu().numpy()
# x_2d = x_2d.view(H, W, 2).cpu().numpy()
# K = K.view(3, 3).cpu().numpy()
# dist = np.zeros(4)
# _, R_res, t_res, _ = cv2.solvePnPRansac(X_3d_a.reshape(1, H*W, 3), x_2d.reshape(1, H*W, 2), K, dist)
# R_res, _ = cv2.Rodrigues(R_res)