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]