def scenes2ares(seq_path, seq_name):
rgb_dir = os.path.join(seq_path, seq_name, 'rgb')
depth_dir = os.path.join(seq_path, seq_name, 'depth')
poses_dir = os.path.join(seq_path, seq_name, 'poses')
frames = sorted(glob.glob(os.path.join(rgb_dir, '*.jpg')))
frame_names = [seq.split('/')[-1].split('.jpg')[0] for seq in frames]
default_intrinsic = np.asarray([525, 525, 320, 240, 0, 0], dtype=np.float32)
frame_seq = FrameSeqData()
# Read the pose
for frame_idx, frame_name in enumerate(frame_names):
pose_file = os.path.join(poses_dir, frame_name + '.txt')
rgb_file = os.path.join(poses_dir, frame_name + '.jpg')
depth_file = os.path.join(poses_dir, frame_name + '.png')
# Read the pose
pose = np.loadtxt(pose_file).astype(np.float32).reshape(4, 4)
Tcw = cam_opt.camera_pose_inv(pose[:3, :3], pose[:3, 3])
timestamp = float(frame_idx)
frame_seq.append_frame(frame_idx=frame_idx,
img_file_name=os.path.join(seq_name, 'rgb', frame_name + '.jpg'),
Tcw=Tcw,
camera_intrinsic=default_intrinsic,
frame_dim=(480, 640),
time_stamp=timestamp,
depth_file_name=os.path.join(seq_name, 'depth', frame_name + '.png'))
return frame_seq
def convert_rel_vo(seq: FrameSeqData, ref_T):
for frame_idx in range(0, len(seq)):
frame = seq.get_frame(frame_idx)
Tcw = seq.get_Tcw(frame)
rel_T = cam_opt.relateive_pose(ref_T[:3, :3], ref_T[:3, 3],
Tcw[:3, :3], Tcw[:3, 3])
frame['extrinsic_Tcw'] = rel_T
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)
def read_tum_seq(tum_rgbd_base_dir, seq_name):
"""
Read the SUN3D sequence to the frames collection
:param sun3d_seq_dir: input sun3d sequence directory
:return: uniform frames collection.
"""
frames = FrameSeqData()
abs_seq_dir = os.path.join(tum_rgbd_base_dir, seq_name)
# Read intrinsic mat
fx = 525.0 # focal length x
fy = 525.0 # focal length y
cx = 319.5 # optical center x
cy = 239.5 # optical center y
K_param = np.asarray([fx, fy, cx, cy, 0.0, 0.0], dtype=np.float32)
default_img_dim = (480, 640)
if os.path.exists(os.path.join(abs_seq_dir, 'rdpose_associate.txt')):
gt_file = 'rdpose_associate.txt'
else:
gt_file = 'rd_associate.txt'
frame_idx = 0
with open(os.path.join(abs_seq_dir, gt_file), 'r') as f:
for line in f:
# Load frame data
if gt_file.startswith('rdpose_associate'):
timestamp, img_file_name, _, depth_file_name, _, tx, ty, tz, qx, qy, qz, qw = line.strip(
).split(' ')
tx = float(tx)
ty = float(ty)
tz = float(tz)
qx = float(qx)
qy = float(qy)
qz = float(qz)
qw = float(qw)
R_mat = trans.quaternion_matrix([qw, qx, qy,
qz]).astype(np.float32)
t = np.array([tx, ty, tz]).astype(np.float32)
Twc_mat = R_mat
Twc_mat[:3, 3] = t
Tcw = np.linalg.inv(Twc_mat)[:3, :]
else:
timestamp, img_file_name, _, depth_file_name = line.strip(
).split(' ')
Tcw = np.eye(4)[:3, :]
frames.append_frame(
frame_idx=frame_idx,
img_file_name=os.path.join(seq_name, img_file_name),
Tcw=Tcw[:3, :],
camera_intrinsic=K_param,
frame_dim=default_img_dim,
time_stamp=float(timestamp),
depth_file_name=os.path.join(seq_name, depth_file_name))
frame_idx += 1
return frames
def read_scannet_seq(scannet_base_dir, seq_name):
"""
Read SCANNET sequences to the frames collection
:param input_seq_dir: directory single SCANNET sequence.
:return: uniform frames collection.
"""
frames = FrameSeqData()
abs_seq_dir = os.path.join(scannet_base_dir, seq_name)
# Read camera intrinsic info.
intrinsic_txt = os.path.join(abs_seq_dir, 'info.txt')
if not os.path.exists(intrinsic_txt):
raise Exception("No camera intrinsic mat.")
with open(intrinsic_txt, "r") as intrinsic_file:
for line in intrinsic_file:
tokens = line.split(' = ')
if tokens[0].startswith("m_depthWidth"):
frame_w = int(tokens[1].strip())
elif tokens[0].startswith("m_depthHeight"):
frame_h = int(tokens[1].strip())
elif tokens[0].startswith("m_depthShift"):
shift_factor = float(tokens[1].strip())
if shift_factor != 1000:
raise Exception("Depth shift error")
elif tokens[0].startswith("m_calibrationDepthIntrinsic"):
k_tokens = tokens[1].split(' ')[:16]
K = np.asarray(k_tokens, dtype=np.float32).reshape(4, 4)
K_param = np.asarray(
[K[0, 0], K[1, 1], K[0, 2], K[1, 2], 0.0, 0.0],
dtype=np.float32)
samples_txt = os.path.join(abs_seq_dir, 'samples.txt')
if not os.path.exists(samples_txt):
raise Exception("No seq samples info.")
with open(samples_txt, "r") as sample_file:
for line in sample_file:
tokens = line.split(' ')
tokens[-1] = tokens[-1].strip()
frame_idx = int(tokens[0])
frame_name = tokens[1].split('/')[1].split('.')[0]
img_name = os.path.join(seq_name, 'rgb', frame_name + '.color.jpg')
depth_name = os.path.join(seq_name, 'depth',
frame_name + '.depth.png')
Twc = np.asarray(tokens[3:19], dtype=np.float32).reshape((4, 4))
Tcw = np.linalg.inv(Twc)[:3, :]
frames.append_frame(frame_idx,
img_name,
Tcw,
K_param, (frame_h, frame_w),
depth_file_name=depth_name)
return frames
def read_sun3d_seq(sun3d_base_dir, seq_name):
"""
Read the SUN3D sequence to the frames collection
:param sun3d_seq_dir: input sun3d sequence directory
:return: uniform frames collection.
"""
frames = FrameSeqData()
abs_seq_dir = os.path.join(sun3d_base_dir, seq_name)
# Read intrinsic mat
intrinsic_file_path = os.path.join(abs_seq_dir, 'intrinsics.txt')
if not os.path.exists(intrinsic_file_path):
raise Exception("DIR: %s ----" % abs_seq_dir)
K = np.loadtxt(intrinsic_file_path, dtype=np.float32).reshape((3, 3))
K_param = np.asarray([K[0, 0], K[1, 1], K[0, 2], K[1, 2], 0.0, 0.0], dtype=np.float32)
default_img_dim = (480, 640)
# Read extrinsic poses
ext_pose_file_path = sorted(glob.glob(os.path.join(abs_seq_dir, 'extrinsics', '*.txt')))[-1]
ext_poses = np.loadtxt(ext_pose_file_path)
n_frames = int(ext_poses.shape[0] / 3)
ext_poses = ext_poses.reshape((n_frames, 3, 4))
# Synchronize the image and depth with timestamp
depth_list = sorted(glob.glob(os.path.join(abs_seq_dir, 'depth', '*.png')))
depth_timestamps = []
for depth_path in depth_list:
depth_name = depth_path.split('/')[-1].strip()
depth_tokens = depth_name.split('-')[1].split('.')[0]
depth_timestamps.append(int(depth_tokens))
depth_timestamps = np.asarray(depth_timestamps)
img_list = sorted(glob.glob(os.path.join(abs_seq_dir, 'image', '*.jpg')))
assert len(img_list) == n_frames
for frame_idx, img_path in enumerate(img_list):
img_name = img_path.split('/')[-1].strip()
img_tokens = img_name.split('-')
frame_timestamp = int(img_tokens[1].split('.')[0])
# Find the closest depth frame
depth_frame_idx = np.argmin(np.abs(depth_timestamps - frame_timestamp))
depth_frame_path = depth_list[depth_frame_idx].split('/')[-1].strip()
Twc = ext_poses[frame_idx]
frames.append_frame(frame_idx=frame_idx,
img_file_name=os.path.join(seq_name, 'image', img_name),
Tcw=camera_pose_inv(Twc[:3, :3], Twc[:3, 3]),
camera_intrinsic=K_param,
frame_dim=default_img_dim,
time_stamp=float(frame_timestamp),
depth_file_name=os.path.join(seq_name, 'depth', depth_frame_path))
return frames
def gen_frame_list(seq_dir, seq_lists, sel_params):
total_sub_seq_list = []
for seq_name in seq_lists:
in_frame_path = os.path.join(seq_dir, seq_name, 'seq.json')
if os.path.exists(in_frame_path):
frames = FrameSeqData(in_frame_path)
sub_frames_list = sel_pairs_with_overlap_range_7scene(
frames,
scene_lmdb=None,
trans_thres=sel_params.trans_thres,
rot_thres=sel_params.rotation_threshold,
dataset_base_dir=seq_dir,
overlap_thres=sel_params.overlap_threshold,
scene_dist_thres=sel_params.scene_dist_threshold,
max_subseq_num=sel_params.max_sub_seq_per_scene,
frames_per_subseq_num=sel_params.frames_per_sub_seq,
frames_range=(0.02, 0.8),
interval_skip_frames=sel_params.skip_frames,
train_anchor_num=sel_params.train_anchor_num,
test_anchor_num=sel_params.test_anchor_num)
total_sub_seq_list += sub_frames_list
return total_sub_seq_list
def scenes2ares(base_dir, seq_name):
rgb_dir = os.path.join(base_dir, seq_name, 'rgb')
depth_dir = os.path.join(base_dir, seq_name, 'depth')
poses_dir = os.path.join(base_dir, seq_name, 'poses')
frames = sorted(glob.glob(os.path.join(rgb_dir, '*.color.jpg')))
frame_names = [seq.split('/')[-1].split('.color.jpg')[0] for seq in frames]
default_intrinsic = np.asarray([572, 572, 320, 240, 0, 0],
dtype=np.float32)
default_rgb_intrinsic = np.asarray([1158.3, 1153.53, 649, 483.5, 0, 0],
dtype=np.float32)
frame_seq = FrameSeqData()
# Read the pose
frame_idx = 0
for i, frame_name in enumerate(frame_names):
pose_file = os.path.join(poses_dir, frame_name + '.pose.txt')
INF_flag = False
with open(pose_file, 'r') as f:
lines = f.readlines()
if 'INF' in lines[0]:
INF_flag = True
if INF_flag:
continue
# Read the pose
pose = np.loadtxt(pose_file).astype(np.float32).reshape(4, 4)
Tcw = cam_opt.camera_pose_inv(pose[:3, :3], pose[:3, 3])
timestamp = float(i)
frame_seq.append_frame(
frame_idx=frame_idx,
img_file_name=os.path.join(seq_name, 'rgb',
frame_name + '.color.jpg'),
Tcw=Tcw,
camera_intrinsic=default_intrinsic,
frame_dim=(480, 640),
time_stamp=timestamp,
depth_file_name=os.path.join(seq_name, 'depth',
frame_name + '.depth.png'),
rgb_intrinsic=default_rgb_intrinsic)
frame_idx += 1
return frame_seq
if __name__ == '__main__':
# Read list
list_file = os.path.join(dataset_dir, seq_name_path)
seq_name_list = []
with open(list_file, 'r') as list_f:
for seq_name in list_f:
seq_name = seq_name.strip()
seq_name_list.append(seq_name)
for seq_name in tqdm(seq_name_list[-1:],
desc='generating lmdbs for sequences'):
seq_file_path = os.path.join(dataset_dir, seq_name, 'seq.json')
if not os.path.exists(seq_file_path):
continue
seq = FrameSeqData(seq_file_path)
seq_lmdb = LMDBSeqModel(
os.path.join(dataset_dir, seq_name, 'rgbd.lmdb'))
for frame_idx in range(0, 80, 20):
frame = seq.frames[frame_idx]
img_path = os.path.join(dataset_dir, seq.get_image_name(frame))
img2 = cv2.imread(img_path)
depth_path = os.path.join(dataset_dir, seq.get_depth_name(frame))
depth = read_sun3d_depth(depth_path)
depth = cv2.resize(depth, (320, 240),
interpolation=cv2.INTER_NEAREST)
img_key = seq.get_image_name(frame)
depth_key = seq.get_depth_name(frame)
alpha = alpha + delta
return R, t
''' Pipeline -----------------------------------------------------------------------------------------------------------
'''
torch.set_default_tensor_type('torch.cuda.FloatTensor')
dataset_dir = '/home/ziqianb/Documents/data/RGBD-SLAM/rgbd_dataset_freiburg1_xyz/ares_output'
img_dir = os.path.join(dataset_dir, 'img')
depth_dir = os.path.join(dataset_dir, 'depth')
# vis = Visualizer(1024, 720)
# Load the frames
frames = FrameSeqData()
frames.load_json(os.path.join(dataset_dir, 'keyframe.json'))
# Test on frames
pairs = [(235, 237), (128, 131)]
I_a_set = []
d_a_set = []
I_b_set = []
K_set = []
sel_a_idx_set = []
T_gt_set = []
for pair in pairs:
frame_a = frames.frames[pair[0]]
file_name = frame_a['file_name']
img = cv2.imread(os.path.join(img_dir,
def rand_sel_subseq_sun3d(scene_frames,
max_subseq_num,
frames_per_subseq_num=10,
dataset_base_dir=None,
trans_thres=0.15,
rot_thres=15,
frames_range=(0, 0.7),
overlap_thres=0.6,
interval_skip_frames=1):
"""
Random select sub set of sequences from scene
:param scene_frames: scene frames to extract subset
:param trans_thres_range: translation threshold, based on the center of different frames
:param max_subseq_num: maximum number of sub sequences
:param frames_per_subseq_num: for each sub sequences, how many frames in the subset
:param frames_range: range of start and end within original scene sequences, from (0, 1)
:param interval_skip_frames: skip interval in original scene frames, used in iteration
:return: list of selected sub sequences
"""
assert dataset_base_dir is not None
n_frames = len(scene_frames)
if interval_skip_frames < 1:
interval_skip_frames = 2
# Simple selection based on trans threshold
if frames_per_subseq_num * interval_skip_frames > n_frames:
raise Exception('Not enough frames to be selected')
rand_start_frame = np.random.randint(int(frames_range[0] * len(scene_frames)),
int(frames_range[1] * len(scene_frames)),
size=max_subseq_num)
sub_seq_list = []
dim = scene_frames.get_frame_dim(scene_frames.frames[0])
K = scene_frames.get_K_mat(scene_frames.frames[0])
pre_cache_x2d = x_2d_coords(dim[0], dim[1])
for start_frame_idx in rand_start_frame:
# print('F:', start_frame_idx)
# Push start keyframe into frames
sub_frames = FrameSeqData()
pre_frame = scene_frames.frames[start_frame_idx]
sub_frames.frames.append(copy.deepcopy(pre_frame))
# Iterate the remaining keyframes into subset
cur_frame_idx = start_frame_idx
no_found_flag = False
while cur_frame_idx < n_frames:
pre_Tcw = sub_frames.get_Tcw(pre_frame)
pre_depth_path = sub_frames.get_depth_name(pre_frame)
pre_depth = read_sun3d_depth(os.path.join(dataset_base_dir, pre_depth_path))
# [Deprecated]
# pre_img_name = sub_frames.get_image_name(pre_frame)
# pre_img = cv2.imread(os.path.join(dataset_base_dir, pre_img_name)).astype(np.float32) / 255.0
# pre_center = camera_center_from_Tcw(pre_Tcw[:3, :3], pre_Tcw[:3, 3])
pre_search_frame = scene_frames.frames[cur_frame_idx + interval_skip_frames - 1]
for search_idx in range(cur_frame_idx + interval_skip_frames, n_frames, 1):
cur_frame = scene_frames.frames[search_idx]
cur_Tcw = sub_frames.get_Tcw(cur_frame)
# [Deprecated]
# cur_center = camera_center_from_Tcw(cur_Tcw[:3, :3], cur_Tcw[:3, 3])
# cur_img_name = sub_frames.get_image_name(cur_frame)
# cur_img = cv2.imread(os.path.join(dataset_base_dir, cur_img_name)).astype(np.float32) / 255.0
rel_angle = rel_rot_angle(pre_Tcw, cur_Tcw)
rel_dist = rel_distance(pre_Tcw, cur_Tcw)
overlap = photometric_overlap(pre_depth, K, Ta=pre_Tcw, Tb=cur_Tcw, pre_cache_x2d=pre_cache_x2d)
# [Deprecated]
# overlap_map, x_2d = cam_opt.gen_overlap_mask_img(pre_depth, K, Ta=pre_Tcw, Tb=cur_Tcw, pre_cache_x2d=pre_cache_x2d)
# rel_T = relateive_pose(pre_Tcw[:3, :3], pre_Tcw[:3, 3], cur_Tcw[:3, :3], cur_Tcw[:3, 3])
# wrap_img, _ = cam_opt.wrapping(pre_img, cur_img, pre_depth, K, rel_T[:3, :3], rel_T[:3, 3])
# img_list = [
# {'img': pre_img},
# {'img': cur_img},
# {'img': wrap_img},
# {'img': overlap_map},
# {'img': x_2d[:, :, 0], 'cmap':'gray'},
# {'img': x_2d[:, :, 1], 'cmap': 'gray'}
# ]
# show_multiple_img(img_list, num_cols=4)
# plt.show()
if rel_dist > trans_thres or overlap < overlap_thres or rel_angle > rot_thres:
# Select the new keyframe that larger than the trans threshold and add the previous frame as keyframe
sub_frames.frames.append(copy.deepcopy(pre_search_frame))
pre_frame = pre_search_frame
cur_frame_idx = search_idx + 1
break
else:
pre_search_frame = cur_frame
if search_idx == n_frames - 1:
no_found_flag = True
if no_found_flag:
break
if len(sub_frames) > frames_per_subseq_num - 1:
break
# If the subset is less than setting, ignore
if len(sub_frames) >= frames_per_subseq_num:
sub_seq_list.append(sub_frames)
print('sel: %d', len(sub_seq_list))
return sub_seq_list
out_bin_file = sys.argv[2] if len(
sys.argv) > 2 else '/mnt/Exp_2/SUN3D/relocal_train.bin'
# Read list
list_file = os.path.join(base_dir, 'SUN3Dv1_train.txt')
seq_lists = []
with open(list_file, 'r') as list_f:
for seq_name in list_f:
seq_name = seq_name.strip()
seq_lists.append(seq_name)
total_sub_seq_list = []
for seq_name in tqdm(seq_lists):
in_frame_path = os.path.join(base_dir, seq_name, 'seq.json')
if os.path.exists(in_frame_path):
frames = FrameSeqData(in_frame_path)
if check_file_exist(base_dir, frames) is False:
print('None Exist on %s', seq_name)
else:
sub_frames_list = sel_triple_sun3d(
base_dir=base_dir,
scene_frames=frames,
max_triple_num=max_triple_num,
num_sample_per_triple=num_sample_per_triple,
trans_thres=trans_thres,
overlap_thres=overlap_threshold)
total_sub_seq_list += sub_frames_list
with open(out_bin_file, 'wb') as out_f:
if shuffle_list:
random.shuffle(total_sub_seq_list)
import numpy as np
import os, sys, glob, cv2
from tqdm import tqdm
from frame_seq_data import FrameSeqData
from relocal_data.cambridge.read_util import get_frame_scene_coord
base_dir = '/mnt/Tango/pg/Ambi'
seq_name = ['cup', 'books', 'cereal', 'desk', 'oats', 'street']
frame_list_filename = "ImageList.txt"
train_seq = FrameSeqData(os.path.join(base_dir, seq_name, 'seq.json'))
test_seq = FrameSeqData(os.path.join(base_dir, seq_name, 'seq_test.json'))
from relocal.vlad_encoder import VLADEncoder
vlad_db = VLADEncoder(checkpoint_path='data/netvlad_vgg16.tar', dev_id=0)
# for train_frame in tqdm(train_seq.frames, desc='add train samples'):
# img, depth, _, _, _ = get_frame_scene_coord(base_dir, train_frame, resize_hw=(192, 256))
# vlad_db.add_sample(img, train_frame)
# vlad_db.load(os.path.join(base_dir, 'train_vlad_feats.bin'))
# vlad_db.dump(os.path.join(base_dir, 'train_vlad_feats.bin'))
"""
for test_frame in test_seq.frames:
img, depth, _, _, _ = get_frame_scene_coord(base_dir, test_frame, resize_hw=(192, 256))
res = vlad_db.find_close_samples(img)
print(res)
"""
for dataset in seq_name:
image_list = read_image_list(
with open(
'/mnt/Exp_2/SUN3D_Valid/reloc_pairs_Overlap0.5to0.55_Rot0to10_valid.bin',
'rb') as f:
data_list = pickle.load(f)
reversed_data_list = []
for sample_dict in tqdm(data_list):
sub_frames = sample_dict['sub_frames']
train_anchor_frames = sample_dict['train_anchor_frames']
test_anchor_frames = sample_dict['test_anchor_frames']
assert len(test_anchor_frames) == 0
assert len(sub_frames) == 1
assert len(train_anchor_frames) == 1
res_sub_frames = FrameSeqData()
res_sub_frames.frames.append(copy.deepcopy(train_anchor_frames[0]))
res_train_anchor_frames = [copy.deepcopy(sub_frames.frames[0])]
reversed_data_list.append({
'sub_frames': res_sub_frames,
'train_anchor_frames': res_train_anchor_frames,
'test_anchor_frames': []
})
with open(out_bin_file, 'wb') as out_f:
if shuffle_list:
random.shuffle(reversed_data_list)
pickle.dump(reversed_data_list, out_f)
print('Total:', len(reversed_data_list))
print('Done, saved to %s' % out_bin_file)
# generate train and test frames information (e.g. extrinsic, intrinsic)
# load train and test split txt
with open(os.path.join(seq_dir, 'TestSplit.txt')) as f:
test_seqs_l = f.readlines()
test_seqs_l = [int(l.split('sequence')[1].strip()) for l in test_seqs_l]
with open(os.path.join(seq_dir, 'TrainSplit.txt')) as f:
train_seqs_l = f.readlines()
train_seqs_l = [int(l.split('sequence')[1].strip()) for l in train_seqs_l]
# collect all test and train frames from all sequences
test_frames = []
for test_seq in test_seqs_l:
json_path = os.path.join(seq_dir, 'seq-%02d' % test_seq, 'seq.json')
seq = FrameSeqData(json_path)
test_frames += seq.frames
train_frames = []
for train_seq in train_seqs_l:
json_path = os.path.join(seq_dir, 'seq-%02d' % test_seq, 'seq.json')
seq = FrameSeqData(json_path)
train_frames += seq.frames
# dump
with open(os.path.join(seq_dir, 'test_frames.bin')) as f:
pickle.dump(test_frames, f)
with open(os.path.join(seq_dir, 'train_frames.bin')) as f:
pickle.dump(train_frames, f)
from frame_seq_data import FrameSeqData, K_from_frame
import core_3dv.camera_operator as cam_opt
import torch
import banet_track.ba_module as module
from core_io.depth_io import load_depth_from_png
import core_math.transfom as trans
from visualizer.visualizer_2d import show_multiple_img
from visualizer.visualizer_3d import Visualizer
from seq_data.sun3d.read_util import read_sun3d_depth
''' Configuration ------------------------------------------------------------------------------------------------------
'''
base_dir = '/mnt/Exp_3/scannet'
seq_name = 'scene0105_02'
frames = FrameSeqData(os.path.join(base_dir, seq_name, 'seq.json'))
''' Script -------------------------------------------------------------------------------------------------------------
'''
x_2d = cam_opt.x_2d_coords(240, 320)
for frame_idx in range(10, len(frames), 5):
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 + 10]
next_Tcw = next_frame['extrinsic_Tcw']
next_name = next_frame['file_name']
K = K_from_frame(cur_frame)
def sel_pairs_with_overlap_range_7scene(scene_frames,
scene_lmdb: LMDBSeqModel,
max_subseq_num,
frames_per_subseq_num=10,
dataset_base_dir=None,
trans_thres=0.15,
rot_thres=15,
frames_range=(0, 0.7),
overlap_thres=0.5,
scene_dist_thres=(0.0, 1.0),
interval_skip_frames=1,
train_anchor_num=100,
test_anchor_num=100):
"""
Random select sub set of sequences from scene
:param scene_frames: scene frames to extract subset
:param trans_thres_range: translation threshold, based on the center of different frames
:param max_subseq_num: maximum number of sub sequences
:param frames_per_subseq_num: for each sub sequences, how many frames in the subset
:param frames_range: range of start and end within original scene sequences, from (0, 1)
:param interval_skip_frames: skip interval in original scene frames, used in iteration
:return: list of selected sub sequences
"""
use_lmdb_cache = True if scene_lmdb is not None else False
assert dataset_base_dir is not None
n_frames = len(scene_frames)
if interval_skip_frames < 1:
interval_skip_frames = 2
max_subseq_num = int(n_frames * max_subseq_num)
# Simple selection based on trans threshold
# if frames_per_subseq_num * interval_skip_frames > n_frames:
# # raise Exception('Not enough frames to be selected')
# return []
rand_start_frame = np.random.randint(
int(frames_range[0] * len(scene_frames)),
int(frames_range[1] * len(scene_frames)),
size=max_subseq_num)
sub_seq_list = []
dim = scene_frames.get_frame_dim(scene_frames.frames[0])
dim = list(dim)
dim[0] = int(dim[0] // 4)
dim[1] = int(dim[1] // 4)
K = scene_frames.get_K_mat(scene_frames.frames[0])
K /= 4.0
K[2, 2] = 1.0
pre_cache_x2d = cam_opt.x_2d_coords(dim[0], dim[1])
for start_frame_idx in rand_start_frame:
# print('F:', start_frame_idx)
# Push start keyframe into frames
sub_frames = FrameSeqData()
pre_frame = scene_frames.frames[start_frame_idx]
sub_frames.frames.append(copy.deepcopy(pre_frame))
sub_frames_idx = [start_frame_idx]
# Iterate the remaining keyframes into subset
cur_frame_idx = start_frame_idx
no_found_flag = False
while cur_frame_idx + interval_skip_frames < n_frames:
pre_Tcw = sub_frames.get_Tcw(pre_frame)
pre_depth_path = sub_frames.get_depth_name(pre_frame)
# pre_depth = read_sun3d_depth(os.path.join(dataset_base_dir, pre_depth_path))
pre_depth = scene_lmdb.read_depth(pre_depth_path) if use_lmdb_cache else \
read_7scenese_depth(os.path.join(dataset_base_dir, pre_depth_path))
pre_depth = cv2.resize(pre_depth, (dim[1], dim[0]),
interpolation=cv2.INTER_NEAREST)
# H, W = pre_depth.shape
# if float(np.sum(pre_depth <= 1e-5)) / float(H*W) > 0.2:
# continue
# pre_depth = torch.from_numpy(pre_depth).cuda()
# pre_Tcw_gpu = torch.from_numpy(pre_Tcw).cuda()
# pre_img_name = sub_frames.get_image_name(pre_frame)
# pre_img = cv2.imread(os.path.join(dataset_base_dir, pre_img_name))
# pre_depth = fill_depth_cross_bf(pre_img, pre_depth)
# [Deprecated]
# import cv2
# pre_img_name = sub_frames.get_image_name(pre_frame)
# pre_img = cv2.imread(os.path.join(dataset_base_dir, pre_img_name)).astype(np.float32) / 255.0
# pre_center = cam_opt.camera_center_from_Tcw(pre_Tcw[:3, :3], pre_Tcw[:3, 3])
pre_search_frame = scene_frames.frames[cur_frame_idx +
interval_skip_frames - 1]
for search_idx in range(cur_frame_idx + interval_skip_frames,
n_frames, 1):
cur_frame = scene_frames.frames[search_idx]
cur_Tcw = sub_frames.get_Tcw(cur_frame)
# cur_Tcw_gpu = torch.from_numpy(cur_Tcw).cuda()
# cur_depth_path = sub_frames.get_depth_name(cur_frame)
# cur_depth = read_sun3d_depth(os.path.join(dataset_base_dir, cur_depth_path))
# H, W = cur_depth.shape
# [Deprecated]
# cur_center = cam_opt.camera_center_from_Tcw(cur_Tcw[:3, :3], cur_Tcw[:3, 3])
# cur_img_name = sub_frames.get_image_name(cur_frame)
# cur_img = cv2.imread(os.path.join(dataset_base_dir, cur_img_name)).astype(np.float32) / 255.0
rel_angle = rel_rot_angle(pre_Tcw, cur_Tcw)
rel_dist = rel_distance(pre_Tcw, cur_Tcw)
overlap = cam_opt.photometric_overlap(
pre_depth,
K,
Ta=pre_Tcw,
Tb=cur_Tcw,
pre_cache_x2d=pre_cache_x2d)
# mean scene coordinate dist
# pre_Twc = cam_opt.camera_pose_inv(R=pre_Tcw[:3, :3], t=pre_Tcw[:3, 3])
# d_a = pre_depth.reshape((H * W, 1))
# x_a_2d = pre_cache_x2d.reshape((H * W, 2))
# X_3d = cam_opt.pi_inv(K, x_a_2d, d_a)
# pre_X_3d = cam_opt.transpose(pre_Twc[:3, :3], pre_Twc[:3, 3], X_3d).reshape((H, W, 3))
# pre_mean = np.empty((3,), dtype=np.float)
# pre_mean[0] = np.mean(pre_X_3d[pre_depth > 1e-5, 0])
# pre_mean[1] = np.mean(pre_X_3d[pre_depth > 1e-5, 1])
# pre_mean[2] = np.mean(pre_X_3d[pre_depth > 1e-5, 2])
#
# cur_Twc = cam_opt.camera_pose_inv(R=cur_Tcw[:3, :3], t=cur_Tcw[:3, 3])
# d_a = cur_depth.reshape((H * W, 1))
# x_a_2d = pre_cache_x2d.reshape((H * W, 2))
# X_3d = cam_opt.pi_inv(K, x_a_2d, d_a)
# cur_X_3d = cam_opt.transpose(cur_Twc[:3, :3], cur_Twc[:3, 3], X_3d).reshape((H, W, 3))
# cur_mean = np.empty((3,), dtype=np.float)
# cur_mean[0] = np.mean(cur_X_3d[cur_depth > 1e-5, 0])
# cur_mean[1] = np.mean(cur_X_3d[cur_depth > 1e-5, 1])
# cur_mean[2] = np.mean(cur_X_3d[cur_depth > 1e-5, 2])
#
# scene_dist = np.linalg.norm(pre_mean - cur_mean)
# def keyPressEvent(obj, event):
# key = obj.GetKeySym()
# if key == 'Left':
# tmp_img = pre_img
# X_3d = pre_X_3d.reshape((H * W, 3))
# vis.set_point_cloud(X_3d, tmp_img.reshape((H * W, 3)))
# # vis.add_frame_pose(cur_Tcw[:3, :3], cur_Tcw[:3, 3])
#
# if key == 'Right':
# tmp_img = cur_img
# X_3d = cur_X_3d.reshape((H * W, 3))
# vis.set_point_cloud(X_3d, tmp_img.reshape((H * W, 3)))
# # vis.add_frame_pose(cur_Tcw[:3, :3], cur_Tcw[:3, 3])
#
# if key == 'Up':
# vis.set_point_cloud(pre_mean.reshape((1, 3)), pt_size=10)
#
# if key == 'Down':
# vis.set_point_cloud(cur_mean.reshape((1, 3)), pt_size=10)
# return
# vis = Visualizer(1280, 720)
# vis.bind_keyboard_event(keyPressEvent)
# vis.show()
# vis.close()
# [Deprecated]
# overlap_map, x_2d = cam_opt.gen_overlap_mask_img(pre_depth, K, Ta=pre_Tcw, Tb=cur_Tcw, pre_cache_x2d=pre_cache_x2d)
# rel_T = relateive_pose(pre_Tcw[:3, :3], pre_Tcw[:3, 3], cur_Tcw[:3, :3], cur_Tcw[:3, 3])
# wrap_img, _ = cam_opt.wrapping(pre_img, cur_img, pre_depth, K, rel_T[:3, :3], rel_T[:3, 3])
# img_list = [
# {'img': pre_img},
# {'img': cur_img},
# {'img': wrap_img},
# {'img': overlap_map},
# {'img': x_2d[:, :, 0], 'cmap':'gray'},
# {'img': x_2d[:, :, 1], 'cmap': 'gray'}
# ]
# show_multiple_img(img_list, num_cols=4)
# plt.show()
# if rel_dist > trans_thres:
# print('exceed trans_thres')
# elif overlap < overlap_thres:
# print('exceed overlap_thres')
# elif rel_angle > rot_thres:
# print('exceed rot_thres')
# if overlap_thres[0] <= overlap <= overlap_thres[1] and \
# rot_thres[0] <= rel_angle <= rot_thres[1]: #and \
# # scene_dist_thres[0] <= scene_dist <= scene_dist_thres[1]:
# sub_frames.frames.append(copy.deepcopy(cur_frame))
if overlap < overlap_thres or rel_dist > trans_thres: #or scene_dist > scene_dist_thres[1]:
# Select the new keyframe that larger than the trans threshold and add the previous frame as keyframe
sub_frames.frames.append(copy.deepcopy(pre_search_frame))
pre_frame = pre_search_frame
cur_frame_idx = search_idx + 1
sub_frames_idx.append(search_idx - 1)
break
else:
pre_search_frame = cur_frame
if search_idx + 1 >= n_frames:
no_found_flag = True
if no_found_flag:
break
if len(sub_frames) > frames_per_subseq_num - 1:
break
# If the subset is less than setting, ignore
if len(sub_frames) >= frames_per_subseq_num:
min_idx = min(sub_frames_idx)
max_idx = max(sub_frames_idx)
print(min_idx, max_idx, n_frames)
# factor = (max_idx - min_idx) // 3
#
# min_Tcw = sub_frames.get_Tcw(sub_frames.frames[0])
# max_Tcw = sub_frames.get_Tcw(sub_frames.frames[-1])
potential_anchor_idces = []
# for i in range(min_idx + factor, max_idx - factor, 1):
# cur_frame = scene_frames.frames[i]
# cur_Tcw = scene_frames.get_Tcw(cur_frame)
# cur_depth_path = sub_frames.get_depth_name(cur_frame)
# cur_depth = scene_lmdb.read_depth(cur_depth_path)
# cur_depth = cv2.resize(cur_depth, (dim[1], dim[0]), interpolation=cv2.INTER_NEAREST)
# H, W = cur_depth.shape
# if float(np.sum(cur_depth <= 1e-5)) / float(H*W) > 0.2:
# continue
# min_overlap = cam_opt.photometric_overlap(cur_depth, K, Ta=cur_Tcw, Tb=min_Tcw,
# pre_cache_x2d=pre_cache_x2d)
# max_overlap = cam_opt.photometric_overlap(cur_depth, K, Ta=cur_Tcw, Tb=max_Tcw,
# pre_cache_x2d=pre_cache_x2d)
# min_rel_angle = rel_rot_angle(cur_Tcw, min_Tcw)
# max_rel_angle = rel_rot_angle(cur_Tcw, max_Tcw)
# if min_overlap < 0.65 and max_overlap < 0.65 and \
# ((0.5 < min_overlap and min_rel_angle < 20.0) or \
# (0.5 < max_overlap and max_rel_angle < 20.0)):
# potential_anchor_idces.append(i)
for i in range(min_idx, max_idx):
if i not in sub_frames_idx:
potential_anchor_idces.append(i)
if len(potential_anchor_idces
) >= train_anchor_num + test_anchor_num:
anchor_idces = np.random.choice(
range(len(potential_anchor_idces)),
size=train_anchor_num + test_anchor_num,
replace=False)
train_anchor_frames = []
for i in anchor_idces[:train_anchor_num]:
train_anchor_frames.append(
scene_frames.frames[potential_anchor_idces[i]])
test_anchor_frames = []
for i in anchor_idces[train_anchor_num:]:
test_anchor_frames.append(
scene_frames.frames[potential_anchor_idces[i]])
sub_seq_list.append({
'sub_frames': sub_frames,
'train_anchor_frames': train_anchor_frames,
'test_anchor_frames': test_anchor_frames
})
print('selected', len(potential_anchor_idces), len(sub_frames))
print('sel: %d', len(sub_seq_list))
return sub_seq_list
import banet_track.ba_module as module from core_io.depth_io import load_depth_from_png import core_math.transfom as trans from visualizer.visualizer_2d import show_multiple_img from visualizer.visualizer_3d import Visualizer from seq_data.seven_scenes.read_util import read_7scenese_depth from img_proc.img_dim import crop_by_intrinsic from seq_data.plot_seq_2d import plot_frames_seq_2d from vo_core.track_preprocess import convert_rel_vo ''' Configuration ------------------------------------------------------------------------------------------------------ ''' base_dir = '/home/ziqianb/Desktop/intel' seq_name = 'apartment' frames = FrameSeqData(os.path.join(base_dir, seq_name, 'seq.json')) refer_T = frames.get_Tcw(frames.frames[0]) convert_rel_vo(frames, refer_T) in_intrinsic = np.asarray([525.0, 525.0, 320.0, 240.0], dtype=np.float32) in_K = cam_opt.K_from_intrinsic(in_intrinsic) ''' Scripts ------------------------------------------------------------------------------------------------------------ ''' plot_frames_seq_2d(frames, show_view_direction=True) plt.show() vis = Visualizer() frame_idx = 0 x_2d = cam_opt.x_2d_coords(int(in_intrinsic[3] * 2), int(in_intrinsic[2] * 2)) # x_2d = cam_opt.x_2d_coords(480, 640)
break
if len(sub_frames) > frames_per_subseq_num - 1:
break
# If the subset is less than setting, ignore
if len(sub_frames) >= frames_per_subseq_num:
sub_seq_list.append(sub_frames)
print('sel: %d', len(sub_seq_list))
return sub_seq_list
if __name__ == '__main__':
ori_seq_json_path = '/home/luwei/mnt/Tango/ziqianb/SUN3D/seq.json'
ori_seq = FrameSeqData(ori_seq_json_path)
sub_seq_list = rand_sel_subseq_sun3d(ori_seq, trans_thres_range=0.2, rot_thres=15.0, max_subseq_num=20, frames_per_subseq_num=5)
# out_sub_seq_list = []
# for seq in sub_seq_list:
# frame_instances = copy.deepcopy(seq.frames)
# for frame in frame_instances:
# frame['extrinsic_Tcw'] = frame['extrinsic_Tcw'].ravel().tolist()
# frame['camera_intrinsic'] = frame['camera_intrinsic'].ravel().tolist()
# out_sub_seq_list.append(frame_instances)
# with open('/home/luwei/mnt/Tango/ziqianb/SUN3D/seq_list.json', 'w') as out_json_file:
# json.dump(out_sub_seq_list, out_json_file, indent=2)
# Show 2D seq
plt.figure()
ax = plt.gca()
import os
import cv2
import numpy as np
from frame_seq_data import FrameSeqData
from seq_data.sun3d.read_util import read_sun3d_depth
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)),
pnp_pose[:3, 3] = t_res.ravel()
poses.append(pnp_pose)
poses = torch.cat([torch.from_numpy(pose) for pose in poses])
if keep_dim_n is True:
poses.squeeze(0)
return poses
if __name__ == '__main__':
vis = Visualizer()
seq_dir = '/home/ziqianb/Desktop/datasets/tgz_target/'
seq = FrameSeqData(
os.path.join(seq_dir, 'rgbd_dataset_freiburg1_desk', 'seq.json'))
frame_a = seq.frames[0]
frame_b = seq.frames[12]
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)
