from SlamUtils.transformation import pos_quats2SEs, pos_quats2SE_matrices, pose2motion, SEs2ses, line2mat, tartan2kitti from SlamUtils.visualization import getVisualizationBB, getKeyframe from SlamUtils.Loader.TartanAir import getRootDir, getDataSequences, getDataLists, tartan_camExtr rootDIR = getRootDir() path = getDataSequences(root=rootDIR, scenario='office', level='Easy', seq_num=4) # path = getDataSequences(root=rootDIR, scenario='seasidetown', level='Easy', seq_num=0) # path = getDataSequences(root=rootDIR, scenario='neighborhood', level='Easy', seq_num=0) files_rgb_left, files_rgb_right, files_depth_left, poselist = getDataLists( dir=path, skip=5) poses_mat44 = pos_quats2SE_matrices(poselist) config_cam = { 'width': 640, 'height': 480, 'fx': 320, 'fy': 320, 'cx': 320, 'cy': 240 } camIntr = camera.PinholeCameraIntrinsic(**config_cam) #open3d if need camExtr = tartan_camExtr K = Cal3_S2(320, 320, 0.0, 320, 240) def getFrameInfo(id):
data_ids = np.loadtxt(fname=save_dir + 'data_id.txt').astype(np.int32) # Trace start id in estimation base_time = datetime.datetime(year=2022, month=2, day=22, hour=22, minute=22, second=22).timestamp() est_time = pose_est[0][0] start_id = np.round((est_time - base_time) * 1000 / 50).astype(np.int32) # remove time_stamp pose_est = pose_est[:, 1:] pose_gt = pose_gt[:, 1:] pose_gt_mat44 = pos_quats2SE_matrices(pose_gt) # remap to global pose pose_ref = pose_gt_mat44[start_id - 1] pose_est_mat44 = pos_quats2SE_matrices(pose_est) for i, pose in enumerate(pose_est_mat44): pose_est_mat44[i] = pose_ref.dot(pose) # remap to estimation to gt est_ids = [*range(start_id, start_id + pose_est.shape[0])] pose_gt = pose_gt[est_ids] pose_gt_mat44 = pos_quats2SE_matrices(pose_gt) # remap to tartanAIR data_ids = data_ids[est_ids] files_rgb_left = [files_rgb_left[i] for i in data_ids]