【将KITTI中的Odometry数据转换ros bag文件】

将KITTI中的Odometry数据转换ros bag文件

我们从官网kitti2bag下载下来的转换工具，虽然能够将kitti转换为bag，但是使用过程中我们可以发现，该代码仅能将raw栏目的原始数据进行bag转换，但是对于kitti中的Odometry数据仅能转换彩色color和灰度gray图像，无法转换点云数据。因此，本人在原始工具包的基础上对python代码进行修改，使其可以转换Odometry雷达数据。

1 安装kitti2bag

我们首先登录github，从连接https://github.com/tomas789/kitti2bag下载下来基础的kitti2bag转换工具包(具体使用前需要提前安装的依赖可以参考这个连接https://www.cnblogs.com/hollow-knight/p/14659545.html)：

2 下载并解压odometry数据

到网页https://www.cvlibs.net/datasets/kitti/eval_odometry.php下载odometry的图像、点云数据，并按照如下结构放在kitti2bag文件夹中：

3 完善外参矩阵

考虑到原始odometry里程计数据的calib文件中仅包含了四个矩阵如下图

但是ktti2bag.py调用函数的时候，此时需要雷达到相机0的Tr外参数矩阵。因此，我们要从KITTI官方的odometry数据中找到外参数文件calibration files进行下载，如图所示

此时打开下载的calib文件，就会发现文件内部包含Tr矩阵如图所示

因此将calibration files中的全部calib文件替换原始odometry里程计数据的calib文件，此时外参文件便完整。

4 修改kitti2bag.py转换odometry数据集

因为原始的kitti2bag.py文件中虽然有转化odometry选项，但最后发布仅发布了图像数据，因此，需要在最后定义并添加一个save_odo_velo_data函数，如图所示：

            save_dynamic_tf(bag, kitti, args.kitti_type, initial_time=current_epoch)
            for camera in used_cameras:
                save_camera_data(bag, args.kitti_type, kitti, util, bridge, camera=camera[0], camera_frame_id=camera[1], topic=camera[2], initial_time=current_epoch)
            save_odo_velo_data(bag, kitti, 'velo_link', '/kitti/velo', initial_time=current_epoch)            
        finally:
            print("## OVERVIEW ##")
            print(bag)
            bag.close()

def save_odo_velo_data(bag, kitti, velo_frame_id, topic, initial_time):
    print("Exporting velodyne data")
    #velo_path = os.path.join(kitti.sequence_path, 'velodyne')
    velo_path = kitti.sequence_path
    velo_data_dir = os.path.join(velo_path, 'velodyne')
    velo_filenames = sorted(os.listdir(velo_data_dir))
    velo_datetimes = map(lambda x: initial_time + x.total_seconds(), kitti.timestamps)
        
    iterable = zip(velo_datetimes, velo_filenames)
    bar = progressbar.ProgressBar()
    for dt, filename in bar(iterable):
        if dt is None:
            continue

        velo_filename = os.path.join(velo_data_dir, filename)

        # read binary data
        scan = (np.fromfile(velo_filename, dtype=np.float32)).reshape(-1, 4)

        # create header
        header = Header()
        header.frame_id = velo_frame_id
        header.stamp = rospy.Time.from_sec(dt)

        # fill pcl msg
        fields = [PointField('x', 0, PointField.FLOAT32, 1),
                  PointField('y', 4, PointField.FLOAT32, 1),
                  PointField('z', 8, PointField.FLOAT32, 1),
                  PointField('intensity', 12, PointField.FLOAT32, 1)]
        pcl_msg = pcl2.create_cloud(header, fields, scan)

        bag.write(topic + '/pointcloud', pcl_msg, t=pcl_msg.header.stamp)

对于路径的话，为了便于程序找到数据，需要修改路径为：

args.dir = args.dir + "/dataset" 

为了便于大家使用，文章后面也附上了本文最后修改好的代码链接

5 运行程序得到bag文件

在kitti2bag文件目录下，打开终端输入python kitti2bag.py -s 00 odom_color指令，最终在同级目录下可以得到bag包数据

附本人修改好的代码: https://gitee.com/abl4/kitti_odometry2bagk