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)