Kalibr将EuRoC公共数据集转换为bag格式

---

使用脚本

6. EUROC公用数据集打包成ROSBAG方法使用的是python2,下面使用python3进行了修改。

#!/usr/bin/env python
print("importing libraries")

import rosbag
import rospy
from sensor_msgs.msg import Image
from sensor_msgs.msg import Imu
from geometry_msgs.msg import PointStamped
import time, sys, os
import argparse
import cv2
import numpy as np
import csv

# structure
# dataset/cam0/data/TIMESTAMP.png
# dataset/camN/data/TIMESTAMP.png
# dataset/imu0/data.csv
# dataset/imuN/data.csv
# dataset/leica0/data.csv

# setup the argument list
parser = argparse.ArgumentParser(
    description="Create a ROS bag using the images and imu data."
)
parser.add_argument("--folder", metavar="folder", nargs="?", help="Data folder")
parser.add_argument(
    "--output-bag",
    metavar="output_bag",
    default="output.bag",
    help="ROS bag file %(default)s",
)

# print help if no argument is specified
if len(sys.argv) < 2:
    parser.print_help()
    sys.exit(0)

# parse the args
parsed = parser.parse_args()


def getImageFilesFromDir(dir):
    """Generates a list of files from the directory"""
    image_files = list()
    timestamps = list()
    if os.path.exists(dir):
        for path, names, files in os.walk(dir):
            for f in files:
                if os.path.splitext(f)[1] in [".bmp", ".png", ".jpg"]:
                    image_files.append(os.path.join(path, f))
                    timestamps.append(os.path.splitext(f)[0])
        # sort by timestamp
        sort_list = sorted(zip(timestamps, image_files))
        image_files = [file[1] for file in sort_list]
    return image_files


def getCamFoldersFromDir(dir):
    """Generates a list of all folders that start with cam e.g. cam0"""
    cam_folders = list()
    if os.path.exists(dir):
        for path, folders, files in os.walk(dir):
            for folder in folders:
                if folder[0:3] == "cam":
                    cam_folders.append(folder)
    return cam_folders


def getImuFoldersFromDir(dir):
    """Generates a list of all folders that start with imu e.g. imu0"""
    imu_folders = list()
    if os.path.exists(dir):
        for path, folders, files in os.walk(dir):
            for folder in folders:
                if folder[0:3] == "imu":
                    imu_folders.append(folder)
    return imu_folders


def getImuCsvFiles(dir):
    """Generates a list of all csv files that start with imu"""
    imu_files = list()
    if os.path.exists(dir):
        for path, folders, files in os.walk(dir):
            for file in files:
                if file[0:3] == "imu" and os.path.splitext(file)[1] == ".csv":
                    imu_files.append(os.path.join(path, file))

    return imu_files


def loadImageToRosMsg(filename):
    image_np = cv2.imread(filename, cv2.IMREAD_GRAYSCALE)

    timestamp_nsecs = os.path.splitext(os.path.basename(filename))[0]
    timestamp = rospy.Time(
        secs=int(timestamp_nsecs[0:-9]), nsecs=int(timestamp_nsecs[-9:])
    )

    rosimage = Image()
    rosimage.header.stamp = timestamp
    rosimage.height = image_np.shape[0]
    rosimage.width = image_np.shape[1]
    rosimage.step = (
        rosimage.width
    )  # only with mono8! (step = width * byteperpixel * numChannels)
    rosimage.encoding = "mono8"
    rosimage.data = image_np.tobytes()  # Python 3: use tobytes() instead of tostring()

    return rosimage, timestamp


def createImuMessge(timestamp_int, omega, alpha):
    timestamp_nsecs = str(timestamp_int)
    timestamp = rospy.Time(int(timestamp_nsecs[0:-9]), int(timestamp_nsecs[-9:]))

    rosimu = Imu()
    rosimu.header.stamp = timestamp
    rosimu.angular_velocity.x = float(omega[0])
    rosimu.angular_velocity.y = float(omega[1])
    rosimu.angular_velocity.z = float(omega[2])
    rosimu.linear_acceleration.x = float(alpha[0])
    rosimu.linear_acceleration.y = float(alpha[1])
    rosimu.linear_acceleration.z = float(alpha[2])

    return rosimu, timestamp


# create the bag
try:
    bag = rosbag.Bag(parsed.output_bag, "w")

    # write images
    camfolders = getCamFoldersFromDir(parsed.folder)
    for camfolder in camfolders:
        camdir = parsed.folder + "/{0}".format(camfolder) + "/data"
        image_files = getImageFilesFromDir(camdir)
        for image_filename in image_files:
            image_msg, timestamp = loadImageToRosMsg(image_filename)
            bag.write("/{0}/image_raw".format(camfolder), image_msg, timestamp)

    # write imu data
    imufolders = getImuFoldersFromDir(parsed.folder)
    for imufolder in imufolders:
        imufile = parsed.folder + "/" + imufolder + "/data.csv"
        topic = os.path.splitext(os.path.basename(imufolder))[0]
        with open(
            imufile, "r"
        ) as csvfile:  # Change to text mode ('r') instead of binary mode ('rb')
            reader = csv.reader(csvfile, delimiter=",")
            headers = next(reader, None)  # Read the header
            for row in reader:
                imumsg, timestamp = createImuMessge(row[0], row[1:4], row[4:7])
                bag.write("/{0}".format(topic), imumsg, timestamp)

finally:
    bag.close()

结果

damon@damon-virtual-machine:/mnt/hgfs/Shared/datasets/EuRoC/MH_01_easy$ ls
bagcreater.py  mav0
damon@damon-virtual-machine:/mnt/hgfs/Shared/datasets/EuRoC/MH_01_easy$ python3 bagcreater.py --folder mav0 --output-bag output.bag
importing libraries
damon@damon-virtual-machine:/mnt/hgfs/Shared/datasets/EuRoC/MH_01_easy$ rosbag info output.bag 
path:        output.bag
version:     2.0
duration:    3:04s (184s)
start:       Jun 25 2014 03:02:59.76 (1403636579.76)
end:         Jun 25 2014 03:06:03.85 (1403636763.85)
size:        2.5 GB
messages:    44184
compression: none [2472/2472 chunks]
types:       sensor_msgs/Image [060021388200f6f0f447d0fcd9c64743]
             sensor_msgs/Imu   [6a62c6daae103f4ff57a132d6f95cec2]
topics:      /cam0/image_raw    3682 msgs    : sensor_msgs/Image
             /cam1/image_raw    3682 msgs    : sensor_msgs/Image
             /imu0             36820 msgs    : sensor_msgs/Imu
damon@damon-virtual-machine:/mnt/hgfs/Shared/datasets/EuRoC/MH_01_easy$ rosbag info ../MH_01_easy.bag
path:        ../MH_01_easy.bag
version:     2.0
duration:    3:06s (186s)
start:       Jun 25 2014 03:02:59.81 (1403636579.81)
end:         Jun 25 2014 03:06:06.70 (1403636766.70)
size:        2.5 GB
messages:    47283
compression: none [2456/2456 chunks]
types:       geometry_msgs/PointStamped [c63aecb41bfdfd6b7e1fac37c7cbe7bf]
             sensor_msgs/Image          [060021388200f6f0f447d0fcd9c64743]
             sensor_msgs/Imu            [6a62c6daae103f4ff57a132d6f95cec2]
topics:      /cam0/image_raw    3682 msgs    : sensor_msgs/Image         
             /cam1/image_raw    3682 msgs    : sensor_msgs/Image         
             /imu0             36820 msgs    : sensor_msgs/Imu           
             /leica/position    3099 msgs    : geometry_msgs/PointStamped
damon@damon-virtual-machine:/mnt/hgfs/Shared/datasets/EuRoC/MH_01_easy$ 

1.Ubuntu20.04安装kalibr

Ubuntu20.04安装kalibr

2.kalibr打包

damon@damon-virtual-machine:~/kalibr_ws$ source devel/setup.bash
damon@damon-virtual-machine:~/kalibr_ws$ devel/lib/kalibr/kalibr_bagcreater --folder /mnt/hgfs/Shared/datasets/EuRoC/MH_01_easy/mav0/ --output-bag /mnt/hgfs/Shared/datasets/EuRoC/MH_01_easy_kalibr.bag
importing libraries

此时，rosbag info如下

damon@damon-virtual-machine:~/kalibr_ws$ rosbag info /mnt/hgfs/Shared/datasets/EuRoC/MH_01_easy_kalibr.bag
path:        /mnt/hgfs/Shared/datasets/EuRoC/MH_01_easy_kalibr.bag
version:     2.0
duration:    3:04s (184s)
start:       Jun 25 2014 03:02:59.76 (1403636579.76)
end:         Jun 25 2014 03:06:03.81 (1403636763.81)
size:        2.5 GB
messages:    7364
compression: none [2455/2455 chunks]
types:       sensor_msgs/Image [060021388200f6f0f447d0fcd9c64743]
topics:      /cam0/image_raw   3682 msgs    : sensor_msgs/Image
             /cam1/image_raw   3682 msgs    : sensor_msgs/Image
damon@damon-virtual-machine:~/kalibr_ws$ rosbag info /mnt/hgfs/Shared/datasets/EuRoC/MH_01_easy.bag
path:        /mnt/hgfs/Shared/datasets/EuRoC/MH_01_easy.bag
version:     2.0
duration:    3:06s (186s)
start:       Jun 25 2014 03:02:59.81 (1403636579.81)
end:         Jun 25 2014 03:06:06.70 (1403636766.70)
size:        2.5 GB
messages:    47283
compression: none [2456/2456 chunks]
types:       geometry_msgs/PointStamped [c63aecb41bfdfd6b7e1fac37c7cbe7bf]
             sensor_msgs/Image          [060021388200f6f0f447d0fcd9c64743]
             sensor_msgs/Imu            [6a62c6daae103f4ff57a132d6f95cec2]
topics:      /cam0/image_raw    3682 msgs    : sensor_msgs/Image         
             /cam1/image_raw    3682 msgs    : sensor_msgs/Image         
             /imu0             36820 msgs    : sensor_msgs/Imu           
             /leica/position    3099 msgs    : geometry_msgs/PointStamped
damon@damon-virtual-machine:~/kalibr_ws$ 

Note：使用这个方法没有bag中imu0数据，有问题。还没解决。

3.报错

终端无法识别 kalibr_bagcreater 这个命令

damon@damon-virtual-machine:~/kalibr_ws$ kalibr_bagcreater
kalibr_bagcreater：未找到命令

1. 检查是否存在 kalibr_bagcreater： 使用 find 命令搜索 kalibr_bagcreater 是否存在：

find . -name kalibr_bagcreater

2. 如果 kalibr_bagcreater 存在，你可以通过完整路径运行它，例如：

devel/lib/kalibr/kalibr_bagcreater

基本上正确安装kalibr，到这一步就行了。

3. 检查 PATH 环境变量： 如果你想通过简单的命令 kalibr_bagcreater 来调用它，可以检查是否将 devel/lib/kalibr 路径添加到 PATH 环境变量中：

echo $PATH

如果没有包含 ~/kalibr_ws/devel/lib/kalibr，你可以手动将该路径添加到 PATH：

export PATH=$PATH:~/kalibr_ws/devel/lib/kalibr

成功的话，则使用kalibr_bagcreater即可。

参考

6. EUROC公用数据集打包成ROSBAG方法

---