T265 recorded rosbag unpacking: unpacking IMU sequence and image sequence method and how to make binocular euroc, binocular tum data set

Table of contents

1. Record bag package

2. Disassembly of left and right images

3. Remove IMU data

4. How to make eruoc and tum data sets

4.1 eruoc dataset format

4.2 Aligning timestamps

4.3 Write imu.csv file

4.4 Generate index file

4. A script completes the unpacking

---

1. Record bag package

        Here I recommend my classmate's blog. You can refer to this blog to record the T265 ros package and solve some problems:

Use RealSense T265 to record bag https://blog.csdn.net/weixin_44760904/article/details/130512863?spm=1001.2014.3001.5501

2. Disassembly of left and right images

        Here we first check the information of the recording package, and we use the command to check the package name:

rosbag info <包名>

        We found three pieces of information:

        /fisheye1: corresponds to the left eye image

        /fisheye2: corresponds to the right image

        /imu: information corresponding to imu

        We use the following script to split the left and right images:

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

        Here path is the path where you want to store the image, and topic is the camera topic corresponding to the image (/fisheye1, /fisheye2). %.6f is to keep a few digits after the decimal point, which depends on the situation.

        We execute the script and get the left and right images:

3. Remove IMU data

        IMU data is divided into time stamp, three acceleration information, three angular velocity information:

        We can separate it out and form a csv file by executing the following script:

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()

        After we executed the script, we got the following csv file:

4. How to make eruoc and tum data sets

4.1 eruoc dataset format

        Photo format:

        First, the timestamps of the left and right images are aligned. Both are 19 bits.

        Among them is the file data.csv, which stores the relationship between timestamps and images, which are actually the same.

        This is the data of the IMU.

4.2 Aligning timestamps

        We found that the timestamps of our recorded packets are not aligned and we need to align them:

        We need to align the timestamps. The principle of alignment: Since we use binocular images mainly for the left-eye images, I align the right-eye images with the timestamps of the left-eye images, which can minimize the loss rate of the 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")

        After executing the script, we found that it has been aligned: (reminder: sometimes the binocular images are different, we need to delete the right-eye image or fill it with the left-eye image before executing this script)

4.3 Write imu.csv file

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()

        After executing the script we generated the csv file. Let's check it out:

        So far, our IMU file has also been generated.

        In the tum dataset, it needs to be converted into txt format. We execute the following script to convert the IMU information saved in csv into txt format:

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)

        We execute the script, and we can see that the csv file saving the IMU information is saved in txt file format (TUM dataset format):

4.4 Generate index file

        We use the following script file to generate the index file of the image:

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)

        After the generation is complete, as shown in the figure, here are the timestamps corresponding to the left and right items and their index files:

        At this point, our file is generated! We can package what we have done into the format of the euroc dataset.

        For the TUM dataset, we need to generate the timestamp file of the image. We use the following script to generate the image sequence and the corresponding timestamp file:

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

        We can see that the timestamp information required for the tum dataset was generated:

4. A script completes the unpacking

        Execute the following script to automatically disassemble the left and right images, automatically generate the csv information and txt information of the IMU, align the timestamps, and generate the timestamps of the left images.

# -*- 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()

        After we execute the following script, the file information required by the TUM dataset and the EUROC dataset is generated under the same name folder of the script.