Kitti数据集跑vins-mono

突发奇想打算用kitti数据集跑vins-mono,找了好多资源终于跑了起来，中间遇到很多坑，不过一一解决了，下面看怎么做得吧。

参考：

VINS-Mono KITT00 测试 - 知乎

如何在kitti raw data上跑起vins-mono - 知乎

KITTI里程计数据集 | 跑VINS-Fusion计算外参_vins kitti-CSDN博客KITTI数据集下载以及转化为rosbag包，kitti2bag使用教程（完整版）-CSDN博客

下载地址

百度网盘 请输入提取码提取码：tsdp

数据集制作

1.下载数据集，我下载的是2011_09_30_drive_0033_extract.zip中的视觉和激光数据 和2011_09_30_drive_0033_sync.zip中的IMU数据。为什么需要下载两个数据集，因为*_extract.zip　包含的IMU数据是100Hz, 但是视觉的数据没有去畸变，此外激光数据是以txt格式存储的，在转换为bag格式的时候非常耗时．虽然*_sync.zip数据中的IMU是10Hz, 但是视觉数据已经去畸变了，并且视觉和激光的时间戳已经同步好了，激光数据的存储格式是二进制格式bin存储的。（虽然没用到激光）

还需要下载标定文件2011_09_30_calib.zip。

2.将下载的_extract.zip　和　_sync.zip 解压，然后用*_extract/oxts文件夹把*_sync/oxt的文件夹

替换掉。

3.解决IMU时间戳问题

不改正会出现bag跑起来以后，vins总是报警告，什么throw image之类的一大堆，轨迹也各种重置。imu时间戳在oxts/timestamp.txt中。

原因KITTI提供的的原始的IMU数据的时间戳存在断续和逆序的情况，我们只能解决逆序情况，断续问题无法解决, 通过下面的程序查看断续的和逆序的IMU时间戳，并对逆序的IMU数据的时间戳进行修改

随便在任何路径建立一个python文件，我命名为imu.py！！！注意正确输入*_sync/路径！！！！

import datetime as dt
import glob
import os
import matplotlib.pyplot as plt
import numpy as np

data_path = "/media/gl/gl-U/kittidata/2011_09_30_drive_0033_sync/2011_09_30/2011_09_30_drive_0033_sync"
def load_timestamps(data='oxts'):
    """Load timestamps from file."""
    timestamp_file = os.path.join(
        data_path, data, 'timestamps.txt')

    # Read and parse the timestamps
    timestamps = []
    with open(timestamp_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)

    # Subselect the chosen range of frames, if any
    return timestamps
timestamps = np.array(load_timestamps())
x = np.arange(0, len(timestamps))



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

print("dt > 0.015: \n{}".format(dt[dt> 0.015])) # 打印前后帧时间差大于0.015的IMU index
dt = dt.tolist()
dt.append(0.01)
dt = np.array(dt)
print("dt > 0.015: \n{}".format(x[dt> 0.015])) # 打印时间差大于0.015的具体时间差
plt.plot(x, timestamps, 'r', label='imu')# 可视化ＩＭＵ的时间戳
plt.show()

然后在imu.py所在目录打开终端执行python3 imu.py命令

gl@gl-pc:~$ python3 imu.py 

终端会打印时间戳有问题的地方和弹出一个表（自行放大，可以看到跳变的地方）

打开oxtx文件夹下timstamps.txt的找到这些地方，手动把时间戳改正

可以明显的看到500处的时间戳有问题，所以依次按顺序把时间加0.01 ，改正完就是这样

就这样，依次把所有时间戳有问题的全改掉。

4.接下来就可以制作了，可能要下在kittibag2包，有问题根据提示安装就行。

着将下载好的2011_09_30_calib.zip文件解压，将其中的三个txt文件复制到如下位置

/media/gl/gl-U/kittidata/2011_09_30_drive_0033_sync/2011_09_30

然后就可以使用kitti2bag工具将KITTI数据集转化为rosbag包
切换到2011_09_30文件夹所在目录
路径：/media/gl/gl-U/kittidata/2011_09_30_drive_0033_sync
执行kitti2bag命令 

kitti2bag -t 2011_09_30 -r 0033 raw_synced

注意：0033是第多少次drive，注意自己下载的数据集的drive

静静等待，出现以下内容，就制作好了

 

运行VINS-mono

改配置文件

注意订阅的相机和外参要对应，否则轨迹会重置，会漂

如果订阅左相机image_topic: "/kitti/camera_gray_left/image_raw"，外参就用

body_T_cam1: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [-0.02218873, -0.01354233,  0.99989895,  1.1031531,
          -0.99989259,  0.00435299, -0.02270281, -0.85632959,
           0.03227481, -1.00072563,  0.00467743,  0.76942567,
           0.,          0.,          0.,          1.        ]

如果订阅右相机image_topic: "/kitti/camera_gray_right/image_raw"，外参就用

body_T_cam0: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [ 0.00875116, -0.00479609,  0.99995027,  1.10224312,
          -0.99986428, -0.01400249,  0.00868325, -0.31907194,
           0.01396015, -0.99989044, -0.00491798,  0.74606588,
           0.,          0.,          0.,          1.        ]

相机内参

distortion_parameters:
   k1: 0
   k2: 0
   p1: 0
   p2: 0
projection_parameters:
   fx: 7.070912e+02
   fy: 7.070912e+02
   cx: 6.018873e+02
   cy: 1.831104e+02

整体就是这样

YAML:1.0

#common parameters
# imu_topic: "/imu0"
# image_topic: "/cam0/image_raw"
imu_topic: "/kitti/oxts/imu"
image_topic: "/kitti/camera_gray_left/image_raw"

output_path: "/home/gl/SLAM/vins-mono/output"

#camera calibration 
model_type: PINHOLE
camera_name: camera
# image_width: 752
# image_height: 480
image_width: 1226
image_height: 370
# distortion_parameters:
#    k1: -2.917e-01
#    k2: 8.228e-02
#    p1: 5.333e-05
#    p2: -1.578e-04
# projection_parameters:
#    fx: 4.616e+02
#    fy: 4.603e+02
#    cx: 3.630e+02
#    cy: 2.481e+02
distortion_parameters:
   k1: 0
   k2: 0
   p1: 0
   p2: 0
projection_parameters:
   fx: 7.070912e+02
   fy: 7.070912e+02
   cx: 6.018873e+02
   cy: 1.831104e+02


# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 0   # 0  Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
                        # 1  Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
                        # 2  Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.                        
#If you choose 0 or 1, you should write down the following matrix.
#Rotation from camera frame to imu frame, imu^R_cam
# extrinsicRotation: !!opencv-matrix
#    rows: 3
#    cols: 3
#    dt: d
#    data: [0.0148655429818, -0.999880929698, 0.00414029679422,
#            0.999557249008, 0.0149672133247, 0.025715529948, 
#            -0.0257744366974, 0.00375618835797, 0.999660727178]
# #Translation from camera frame to imu frame, imu^T_cam
# extrinsicTranslation: !!opencv-matrix
#    rows: 3
#    cols: 1
#    dt: d
#    data: [-0.0216401454975,-0.064676986768, 0.00981073058949]
extrinsicRotation: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [-0.02218873, -0.01354233,  0.99989895,
          -0.99989259,  0.00435299, -0.02270281, 
         0.03227481, -1.00072563,  0.00467743]
#Translation from camera frame to imu frame, imu^T_cam
extrinsicTranslation: !!opencv-matrix
   rows: 3
   cols: 1
   dt: d
   data: [1.1031531,-0.85632959, 0.76942567]

保存，然后按照运行vins-mono的命令运行就行。

附上一张图，具体和真值轨迹对比有空再做吧！！！！