【SLAM】合并rosbag中的加速度计和陀螺仪数据为单一IMU数据流

啵啵啵啵哲

于 2023-12-10 15:20:02 发布

阅读量590

点赞数 12

文章标签：机器人 python

本文链接：https://blog.csdn.net/xuzhengzhe/article/details/134908673

版权

1. 引言

在OpenLORIS-Scene数据集上执行如VINS-Mono这类视觉SLAM算法时，我们需要将rosbag中的加速度计数据"/d400/accel/sample"和陀螺仪数据"/d400/gyro/sample"消息合并为单一的IMU数据流。

OpenLORIS提供了一系列工具，可以在https://github.com/lifelong-robotic-vision/openloris-scene-tools/blob/master/dataprocess中找到。其中合并IMU数据的脚本merge_imu_topics.py运行缓慢。针对这一问题，在原版基础上编写了fast_merge_imu_topics.py，一个使用numpy和scipy进行数据处理以实现更快速的rosbag合并的脚本。

2. 脚本功能

fast_merge_imu_topics.py脚本的主要功能是从ROS bag文件中读取加速度计和陀螺仪的数据，然后将这些数据合并为标准的IMU数据流格式。它采用了numpy和scipy库来优化数据处理过程，显著提升了数据处理速度和效率。

3. 使用方法

依赖安装：确保你的环境中已安装ROS、numpy和scipy。如果尚未安装，可以使用以下命令安装：

pip install numpy scipy

脚本下载：可以从GitHub仓库 https://github.com/ZhengzheXu/slam_tools下载fast_merge_imu_topics.py脚本。未来如果有更多实用的脚本也会在这个仓库中发布。
执行脚本：在终端运行以下命令来执行脚本：

    python fast_merge_imu_topics.py <bag_file_1> <bag_file_2> ...

其中<bag_file_1> <bag_file_2>是原始ROS bag文件.

4. 示例代码

这里直接给出fast_merge_imu_topics.py脚本的代码：

#!/usr/bin/env python2
# -*- coding: utf-8 -*-

"""
Fastly merge gyro and accel topics into one rosbag file with linear interpolation.

Modified from: openloris-scene-tools/dataprocess/merge_imu_topics.py (original script is also provided in this folder)

Usage:
    python fast_merge_imu_topics.py <bag_file_1> <bag_file_2> ...

Output:
    vins1.bag, vins2.bag, ...

Note:
    1. The input bag file should contain two topics: /d400/accel/sample and /d400/gyro/sample.
    2. The output bag file will contain only one topic: /d400/imu0.
    3. The output bag file will be named as vins1.bag, vins2.bag, ... and so on.
    4. The output bag file will be saved in the same directory as the input bag file.
"""

import rospy
import rosbag
import sys
import numpy as np
import scipy.interpolate as interp
from sensor_msgs.msg import Imu


def merge_gyro_accel(file, file_index):
    """ Merge gyro and accel data from a ROS bag file. """
    outfile = "vins{}.bag".format(file_index)
    outbag = rosbag.Bag(outfile, 'w')

    # Initialize lists to store data
    d400_accer, d400_gyr = [], []
    d400_accer_data, d400_gyr_data = [], []

    with rosbag.Bag(file) as inbag:
        print("-------------------- Input: {} ------------------------------".format(file))
        print(inbag)
        msg_num = inbag.get_message_count()

        for count, (topic, msg, t) in enumerate(inbag.read_messages()):
            # If topic is not imu, copy it to output bag
            if topic == "/d400/imu0":
                # This means we have already merged gyro and accel data
                continue

            # Copy all topics to output bag
            outbag.write(topic, msg, t)

            # Store gyro and accel data
            if topic == "/d400/accel/sample":
                process_sensor_data(d400_accer, d400_accer_data, msg, t, 'linear_acceleration')
            elif topic == "/d400/gyro/sample":
                process_sensor_data(d400_gyr, d400_gyr_data, msg, t, 'angular_velocity')

            # Print progress
            if count % 10000 == 0:
                print("Copying data to output bag: {}/{}".format(count, msg_num))

        print("Start to merge gyro and accel topics with linear interpolation.")
        process_interpolation(d400_accer, d400_accer_data, d400_gyr, d400_gyr_data, outbag)

    outbag.close()
    print("------------------- Output: {} ------------------------------".format(outfile))
    print(rosbag.Bag(outfile))


def process_sensor_data(sensor_list, sensor_data_list, msg, t, attr_name):
    """ Process sensor data for gyro or accel. """
    sensor_list.append(msg)
    sensor_time = rospy.Time.to_sec(t)
    sensor_data_list.append((
        getattr(msg, attr_name).x,
        getattr(msg, attr_name).y,
        getattr(msg, attr_name).z,
        sensor_time
    ))


def process_interpolation(d400_accer, d400_accer_data, d400_gyr, d400_gyr_data, outbag):
    """ Interpolate and merge gyro and accel data. """
    d400_accer_data, d400_gyr_data = np.array(d400_accer_data), np.array(d400_gyr_data)
    accer_times, gyr_times = d400_accer_data[:, -1], d400_gyr_data[:, -1]

    # First step: Interpolate gyr data on accer timestamps
    print("Interpolating gyro data on accel timestamps.")
    interpolate_and_merge(d400_accer, d400_accer_data, d400_gyr_data, outbag, "/d400/imu0", 'angular_velocity')

    # Second step: Interpolate accer data on gyr timestamps
    print("Interpolating accel data on gyro timestamps.")
    interpolate_and_merge(d400_gyr, d400_gyr_data, d400_accer_data, outbag, "/d400/imu0", 'linear_acceleration')


def interpolate_and_merge(sensor_list, sensor_data, other_sensor_data, outbag, topic, attr_name):
    """ Interpolate sensor data and merge into output bag. """

    # Filter out invalid data
    valid_indices = (sensor_data[:, -1] >= other_sensor_data[:, -1].min()) & (sensor_data[:, -1] <= other_sensor_data[:, -1].max())
    sensor_data = sensor_data[valid_indices]
    sensor_list = [sensor_list[i] for i in range(len(sensor_list)) if valid_indices[i]]

    # Interpolate sensor data
    func_x = interp.interp1d(other_sensor_data[:, -1], other_sensor_data[:, 0], kind='linear')
    func_y = interp.interp1d(other_sensor_data[:, -1], other_sensor_data[:, 1], kind='linear')
    func_z = interp.interp1d(other_sensor_data[:, -1], other_sensor_data[:, 2], kind='linear')

    # Merge sensor data
    for count, (sensor, t) in enumerate(zip(sensor_list, sensor_data[:, -1])):
        sensor_attr = getattr(sensor, attr_name)
        sensor_attr.x = func_x(t)
        sensor_attr.y = func_y(t)
        sensor_attr.z = func_z(t)

        outbag.write(topic, sensor, rospy.Time.from_sec(t))
        if count % 1000 == 0:
            print("Writing data to output bag: {}/{}".format(count, len(sensor_data)))


if __name__ == '__main__':
    for index, file in enumerate(sys.argv[1:], start=1):
        merge_gyro_accel(file, index)

5. 补充说明

在使用以上脚本合并IMU数据时，有以下几点需要补充说明：

数据频率：合并加速度计和陀螺仪数据时同时保留了原始数据的时间戳。这意味着合并后的IMU数据流将具有比原始加速度计或陀螺仪数据更高的频率。
时间戳：在运行VINS-Mono等算法时，可能会遇到“imu message in disorder!”的警告信息。这表示IMU数据中的时间戳不是严格单调递增的。但实际上，经过测试，这种时间戳的问题并不会影响算法的正常运行。值得注意的是，原版脚本在处理数据时也存在同样的问题。我们计划在未来的版本中解决这一问题，以进一步优化数据处理流程。

希望这个脚本能够对大家有所帮助。如有任何问题或建议，欢迎在GitHub仓库中提出。