kitti Odometry数据集对齐raw的oxts数据，包含

kitti Odometry数据集，带位置真值的数据，包含11个场景，但可惜只对齐了图像和激光数据，没有oxts数据，也就是没有rtk、imu等数据，对于做vio、lio等任务就没法搞了，好在数据说明里有写到这11个场景对应的raw数据，如下：

seq_map = {
    "00": {"name": "2011_10_03_drive_0027", "start": 0, "end": 4540},
    "01": {"name": "2011_10_03_drive_0042", "start": 0, "end": 1100},
    "02": {"name": "2011_10_03_drive_0034", "start": 0, "end": 4660},
    "03": {"name": "2011_09_26_drive_0067", "start": 0, "end": 800},
    "04": {"name": "2011_09_30_drive_0016", "start": 0, "end": 270},
    "05": {"name": "2011_09_30_drive_0018", "start": 0, "end": 2760},
    "06": {"name": "2011_09_30_drive_0020", "start": 0, "end": 1100},
    "07": {"name": "2011_09_30_drive_0027", "start": 0, "end": 1100},
    "08": {"name": "2011_09_30_drive_0028", "start": 1100, "end": 5170},
    "09": {"name": "2011_09_30_drive_0033", "start": 0, "end": 1590},
    "10": {"name": "2011_09_30_drive_0034", "start": 0, "end": 1200}
}

有了raw数据，就有了oxts数据了，只要基于时间戳做个对齐就行了，由于想要100hz的oxts数据，所以选择unsync的raw数据，sync的raw数据只有10hz
还有大神支出raw里的oxts数据时间戳存在跳变情况，需要做些处理，详见：https://zhuanlan.zhihu.com/p/75672946
https://zhuanlan.zhihu.com/p/115562083

我实现的一个处理oxts时间戳跳变情况函数，可以参考下

def load_timestamps(timestamps_file):
    timestamps = []
    with open(timestamps_file, 'r') as f:
        for line in f.readlines():
            # NB: datetime only supports microseconds, but KITTI timestamps
            # give nanoseconds, so need to truncate last 4 characters to
            # get rid of \n (counts as 1) and extra 3 digits
            t = dt.datetime.strptime(line[:-4], '%Y-%m-%d %H:%M:%S.%f')
            t = dt.datetime.timestamp(t)
            timestamps.append(t)
    return timestamps


def load_sync_timestamps(timestamps_file, std_dt=0.01):
    max_dt = std_dt * 1.5
    timestamps = np.array(load_timestamps(timestamps_file))
    x = np.arange(0, len(timestamps))

    last_timestamp = timestamps[:-1]
    curr_timestamp = timestamps[1:]
    dt = np.array(curr_timestamp - last_timestamp)  # 计算前后帧时间差

    print(timestamps_file)
    print("dt > {}: \n{}".format(max_dt, dt[dt > max_dt]))
    dt = dt.tolist()
    dt.append(std_dt)
    dt = np.array(dt)
    unsync_x = x[dt > max_dt]
    print("dt > {}: \n{}".format(max_dt, unsync_x))
    for i in unsync_x:
        succ = False
        # 向前对齐
        while (not succ):
            ii = i-1
            if ii < 0:
                break
            if ii not in unsync_x:
                for j in range(ii+1, i+1):
                    timestamps[j] = timestamps[j-1] + std_dt
                succ = True
                break
        # 向后对齐
        while (not succ):
            ii = i+1
            if ii >= len(timestamps):
                break
            if ii not in unsync_x:
                for j in range(ii, i, -1):
                    timestamps[j-1] = timestamps[j] - std_dt
                succ = True
                break
        if not succ:
            print("cannot sync timestamp", i, timestamps[i], dt[i])
    return timestamps

对齐逻辑也简单粗暴

def concat_oxts(oxts_timestamps, extract_raw_dir, start_time, end_time, out_file):
    oxts_data = []
    for idx, oxts_t in enumerate(oxts_timestamps):
        if oxts_t < start_time:
            continue
        if oxts_t > end_time:
            break
        oxts_file = os.path.join(
            extract_raw_dir, "oxts", "data", "{:0>10d}.txt".format(idx))
        if not os.path.exists(oxts_file):
            print("oxts file not exists", oxts_file)
            continue
        with open(oxts_file, "r") as f:
            for line in f.readlines():
                oxts_data.append(line)
    os.makedirs(os.path.dirname(out_file), exist_ok=True)
    with open(out_file, "w") as f:
        f.writelines(oxts_data)


if __name__ == "__main__":
    os.makedirs(oxts_out_dir, exist_ok=True)
    # 对齐时间戳找到oxts数据
    for seq in seq_map:
        print(seq)
        post_raw_dir = seq_map[seq]["name"][:10]
        extract_raw_dir = os.path.join(
            raw_dir, post_raw_dir, seq_map[seq]["name"] + "_extract")
        if not os.path.exists(extract_raw_dir):
            print("raw seq data not exists", extract_raw_dir)
            continue

        start_idx = seq_map[seq]["start"]
        end_idx = seq_map[seq]["end"]
        image_timestamps_file = os.path.join(
            extract_raw_dir, "image_02", 'timestamps.txt')
        image_timestamps = load_sync_timestamps(image_timestamps_file, 0.1)

        # 扣出对应时间戳内的oxts数据
        oxts_timestamps_file = os.path.join(
            extract_raw_dir, "oxts", 'timestamps.txt')
        oxts_timestamps = load_sync_timestamps(oxts_timestamps_file, 0.01)
        # 上一帧到当前帧之间的oxts数据
        for i in range(start_idx, end_idx + 1):
            if i == 0:
                start_time = image_timestamps[i] - 0.1
            else:
                start_time = image_timestamps[i-1]
            end_time = image_timestamps[i]
            oxts_file = os.path.join(
                oxts_out_dir, seq, "{:0>10d}.txt".format(i-start_idx))
            concat_oxts(oxts_timestamps, extract_raw_dir,
                        start_time, end_time, oxts_file)

提供一个我对齐好的oxts数据，由于03对应的2011_09_26_drive_0067数据官网无法下载，所以没有03对应的oxts数据

链接: https://pan.baidu.com/s/1CR8aaNB83cWC0nrIatag5w 提取码: gi9c