T265录制的rosbag拆包：拆IMU序列和图像序列方法以及如何制作双目euroc、双目tum数据集

目录

1.录制bag包

2.左右目图像的拆解

3.拆IMU数据

4.如何制作eruoc与tum数据集

4.1 eruoc数据集格式

4.2 对齐时间戳

4.3 编写imu.csv文件

4.4 生成索引文件

4.一个脚本完成拆包

---

1.录制bag包

        这里推荐我的同学的博客，大家可以参考这篇博客录制T265的ros包并解决一些问题：

使用 RealSense T265录制baghttps://blog.csdn.net/weixin_44760904/article/details/130512863?spm=1001.2014.3001.5501

2.左右目图像的拆解

        这里我们先查看录制包的信息，我们用命令查看包名：

rosbag info <包名>

        我们发现有三个信息：

        /fisheye1：对应左目的图像

        /fisheye2：对应右目的图像

        /imu：对应imu的信息

        我们用下面的脚本拆左右目图像：

import roslib
import rosbag
import rospy
import cv2
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
from cv_bridge import CvBridgeError
 
path='/home/xxxx/Desktop/left/' 
class ImageCreator():
 
 
    def __init__(self):
        self.bridge = CvBridge()
        with rosbag.Bag('/home/xxxx/Desktop/out.bag', 'r') as bag:   
            for topic,msg,t in bag.read_messages():
                if topic == "/fisheye1": 
                        try:
                            cv_image = self.bridge.imgmsg_to_cv2(msg,"bgr8")
                        except CvBridgeError as e:
                            print e
                        timestr = "%.6f" %  msg.header.stamp.to_sec()

                        image_name = timestr+ ".jpg" 
                        cv2.imwrite(path+image_name, cv_image) 

 
 
if __name__ == '__main__':
    try:
        image_creator = ImageCreator()
    except rospy.ROSInterruptException:
        pass

        这里path是你要将图片存放的路径，topic是图像对应的相机话题（/fisheye1、/fisheye2）。%.6f是要把小数点后保留几位数，这个视情况而定。

        我们执行脚本，得到了左右目图像：

3.拆IMU数据

        IMU数据分为时间戳、三个加速度信息、三个角速度信息：

        我们执行下面的脚本就能将其分离出来并组成csv文件：

import rosbag
import csv
from sensor_msgs.msg import Imu


bag = rosbag.Bag('/home/xxxx/Desktop/out.bag')


csvfile = open('imu.csv', 'w')
csvwriter = csv.writer(csvfile)


csvwriter.writerow(['timestamp', 'ax', 'ay', 'az', 'wx', 'wy', 'wz'])


for topic, msg, t in bag.read_messages(topics=['/imu']):

    timestamp = msg.header.stamp.to_nsec()
    ax = msg.linear_acceleration.x
    ay = msg.linear_acceleration.y
    az = msg.linear_acceleration.z
    wx = msg.angular_velocity.x
    wy = msg.angular_velocity.y
    wz = msg.angular_velocity.z
    csvwriter.writerow([timestamp, ax, ay, az, wx, wy, wz])

bag.close()
csvfile.close()

        我们执行完脚本之后，得到了如下的csv文件：

4.如何制作eruoc与tum数据集

4.1 eruoc数据集格式

        照片格式：

        首先，左右目图片时间戳是对齐的。都是19位的。

        其中有文件data.csv，存储着时间戳和图像的关系，其实都是一样的。

        这是IMU的数据。

4.2 对齐时间戳

        我们发现我们录包的时间戳不是对齐的我们需要将其对齐：

        我们需要将时间戳进行对齐，对齐的原则：由于我们使用双目图像主要是使用的左目图像，因此我按照左目图像的时间戳去对齐右目，这样可以将IMU的损失率降到最小。

import os
import os
import shutil

folder1_path = "/home/liuhongwei/Desktop/left"
folder2_path = "/home/liuhongwei/Desktop/right"

output_folder_path = "/home/liuhongwei/Desktop/righti"



folder1_files = sorted(os.listdir(folder1_path))


folder2_files = sorted(os.listdir(folder2_path))


if len(folder1_files) != len(folder2_files):
    print("no")
else:

    for i in range(len(folder2_files)):
        source_path = os.path.join(folder2_path, folder2_files[i])
        target_path = os.path.join(output_folder_path, folder1_files[i])
        shutil.copyfile(source_path, target_path)

    print("yes")

        执行完脚本后我们发现已经对齐了：（提示：有时候双目图片不一样我们需要对右目图像进行删减或用左目图像补齐再执行这个脚本）

4.3 编写imu.csv文件

import rosbag
import csv
from sensor_msgs.msg import Imu


bag = rosbag.Bag('bag包的地址')


csvfile = open('imu1.csv', 'w')
csvwriter = csv.writer(csvfile)


csvwriter.writerow(['timestamp [ns]', 'w_RS_S_x [rad s^-1]', 'w_RS_S_y [rad s^-1]', 'w_RS_S_z [rad s^-1]', 'a_RS_S_x [rad m s^-2]', 'a_RS_S_y [rad m s^-2]', 'a_RS_S_z [rad m s^-2]'])


for topic, msg, t in bag.read_messages(topics=['/imu']):

    timestamp = msg.header.stamp.to_nsec()
    ax = msg.linear_acceleration.x
    ay = msg.linear_acceleration.y
    az = msg.linear_acceleration.z
    wx = msg.angular_velocity.x
    wy = msg.angular_velocity.y
    wz = msg.angular_velocity.z
    csvwriter.writerow([timestamp, wx, wy, wz, ax, ay, az])

bag.close()
csvfile.close()

        执行脚本后我们生成了csv文件。我们查看一下：

        至此，我们IMU文件也生成了。

        在tum数据集中，需要将其转换成txt格式。我们执行下面的脚本，会把以csv保存的IMU信息转化成txt格式：

import csv

def csv_to_txt(csv_file, txt_file):
    with open(csv_file, 'r') as file:
        reader = csv.reader(file)
        with open(txt_file, 'w') as output_file:
            writer = csv.writer(output_file, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL)
            for row in reader:
                writer.writerow(row)

csv_file = 'csv文件的地址'
txt_file = '转换保存的txt文件地址'
csv_to_txt(csv_file, txt_file)

        我们执行脚本，可以看到保存IMU信息的csv文件被保存为txt文件格式（TUM数据集格式）了：

4.4 生成索引文件

        我们利用如下脚本文件生成图像的索引文件：

import os
import csv

def create_image_csv(folder_path, csv_file_path):
    with open(csv_file_path, 'wb') as csvfile:
        writer = csv.writer(csvfile)
        writer.writerow(['TimeStamp', 'Image Name'])

        for filename in os.listdir(folder_path):
            if filename.endswith('.jpg') or filename.endswith('.png'):
                image_name = os.path.splitext(filename)[0]
                writer.writerow([image_name, filename])

folder_path = '/home/liuhongwei/Desktop/right'  
csv_file_path = '/home/liuhongwei/Desktop/right.csv'  

create_image_csv(folder_path, csv_file_path)

        生成完后如图，这是左右目对应的时间戳和它们的索引文件：

        至此，我们的文件就生成完毕啦！我们将我们所做的东西打包成euroc数据集的格式就可以了。

        对于TUM数据集，我们需要生成图像的时间戳文件，我们通过下面的脚本去生成图像序列和对应的时间戳文件：

import roslib
import rosbag
import rospy
import cv2
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
from cv_bridge import CvBridgeError

path = '要保存的图像序列地址'
txt_file = '时间戳文件的地址（自动创建）'  # Path to the text file

class ImageCreator():
    def __init__(self):
        self.bridge = CvBridge()
        image_names = []  # List to store image names

        with rosbag.Bag('录制的bag包地址', 'r') as bag:
            for topic, msg, t in bag.read_messages():
                if topic == "/fisheye1":
                    try:
                        cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
                    except CvBridgeError as e:
                        print(e)
                        continue

                    timestr = "%.9f" % msg.header.stamp.to_sec()
                    image_name = timestr.replace('.', '')  # Remove periods from the timestamp
                    cv2.imwrite(path + image_name + '.png', cv_image)  # Save as PNG format
                    image_names.append(image_name)  # Add image name to the list

        # Save image names to the text file
        with open(txt_file, 'w') as f:
            f.write('\n'.join(image_names))

if __name__ == '__main__':
    try:
        image_creator = ImageCreator()
    except rospy.ROSInterruptException:
        pass

        我们可以看到生成了tum数据集所需的时间戳信息：

4.一个脚本完成拆包

        执行下面的脚本，自动拆左右目图像，自动生成IMU的csv信息和txt信息，对齐时间戳、生成左目图像的时间戳。

# -*- coding: utf-8 -*-

import rosbag
import csv
from sensor_msgs.msg import Imu
import os
import roslib
import rospy
import cv2
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
from cv_bridge import CvBridgeError
import shutil

def CreateDIR():
    folder_name = 'bag_tum'
    subfolders = ['left', 'righti']

    if not os.path.exists(folder_name):
        os.makedirs(folder_name)

    # 在主文件夹下创建子文件夹
    for subfolder in subfolders:
        subfolder_path = os.path.join(folder_name, subfolder)
        if not os.path.exists(subfolder_path):
            os.makedirs(subfolder_path)


def CreateIMUCSV(umpackbag):
    csvfile = open('imudata.csv', 'w')
    csvwriter = csv.writer(csvfile)
    csvwriter.writerow(['timestamp [ns]', 'w_RS_S_x [rad s^-1]', 'w_RS_S_y [rad s^-1]', 'w_RS_S_z [rad s^-1]', 'a_RS_S_x [rad m s^-2]', 'a_RS_S_y [rad m s^-2]', 'a_RS_S_z [rad m s^-2]'])
    for topic, msg, t in umpackbag.read_messages(topics=['/imu']):
        timestamp = msg.header.stamp.to_nsec()
        ax = msg.linear_acceleration.x
        ay = msg.linear_acceleration.y
        az = msg.linear_acceleration.z
        wx = msg.angular_velocity.x
        wy = msg.angular_velocity.y
        wz = msg.angular_velocity.z
        csvwriter.writerow([timestamp, wx, wy, wz, ax, ay, az])
    #umpackbag.close()
    csvfile.close()

def TransIMUdatatotxt():
    csv_file = './imudata.csv'
    txt_file = './imudata.txt'
    with open(csv_file, 'r') as file:
        reader = csv.reader(file)
        with open(txt_file, 'w') as output_file:
            writer = csv.writer(output_file, delimiter=',', quotechar='"', quoting=csv.QUOTE_MINIMAL)
            for i, row in enumerate(reader):
                if i == 0:
                    writer.writerow(['#' + cell for cell in row])  # 添加#号
                else:
                    writer.writerow(row)

def SaveImageFishereyeleft(umpackbag):
    path = './bag_tum//left/'
    txt_file = './timestamp.txt'
    bridge = CvBridge()
    image_names = []

    with rosbag.Bag(bagname, 'r') as bag:
        for topic, msg, t in umpackbag.read_messages():
            if topic == "/fisheye1":
                try:
                    cv_image = bridge.imgmsg_to_cv2(msg, "bgr8")
                except CvBridgeError as e:
                    print(e)
                    continue

                timestr = "%.9f" % msg.header.stamp.to_sec()
                image_name = timestr.replace('.', '')  # Remove periods from the timestamp
                cv2.imwrite(path + image_name + '.png', cv_image)  # Save as PNG format
                image_names.append(image_name)  # Add image name to the list
    with open(txt_file, 'w') as f:
        f.write('\n'.join(image_names))

def SaveImageFishereyeright(umpackbag):
    path = './bag_tum//righti/'
    bridge = CvBridge()
    image_names = []

    with rosbag.Bag(bagname, 'r') as bag:
        for topic, msg, t in umpackbag.read_messages():
            if topic == "/fisheye2":
                try:
                    cv_image = bridge.imgmsg_to_cv2(msg, "bgr8")
                except CvBridgeError as e:
                    print(e)
                    continue

                timestr = "%.9f" % msg.header.stamp.to_sec()
                image_name = timestr.replace('.', '')  # Remove periods from the timestamp
                cv2.imwrite(path + image_name + '.png', cv_image)  # Save as PNG format
                image_names.append(image_name)  # Add image name to the list

def dealwithTimeStamp():
    folder1_path = './bag_tum//left/'
    folder2_path = './bag_tum//right/'
    output_folder_path = './bag_tum//righti/'
    folder1_files = sorted(os.listdir(folder1_path))
    folder2_files = sorted(os.listdir(folder2_path))
    if len(folder1_files) != len(folder2_files):
        print("录制包时左右目图像数量不一致，请手动处理")
    else:
        for i in range(len(folder2_files)):
            source_path = os.path.join(folder2_path, folder2_files[i])
            target_path = os.path.join(output_folder_path, folder1_files[i])
            shutil.copyfile(source_path, target_path)
        print("图像序列生成完毕")
    if os.path.exists(folder2_path):
        shutil.rmtree(folder2_path)


# Press the green button in the gutter to run the script.
if __name__ == '__main__':
    bagname = './road.bag'
    umpackbag = rosbag.Bag(bagname)
    CreateDIR()
    CreateIMUCSV(umpackbag)
    TransIMUdatatotxt()
    SaveImageFishereyeleft(umpackbag)
    SaveImageFishereyeright(umpackbag)
    dealwithTimeStamp()

        我们执行下面的脚本后，在脚本的同名文件夹下生成了TUM数据集以及EUROC数据集所需的文件信息。