Convert Realsense ROS bag to TUM format image files

convert_rgbd.py

import os, sys
import os.path as osp
import cv2
from cv_bridge import CvBridge
import rosbag
import sensor_msgs
import sensor_msgs.point_cloud2 as pc
import zipfile
import tqdm

def convert(filepath):
    output = zipfile.ZipFile(filepath[:-4] + ".zip", 'w')
    # color_counter = 0
    # depth_counter = 0
    br = CvBridge()

    print("Loading rosbag...")

    with rosbag.Bag(filepath, 'r') as bag:
        depth_topic = "/device_0/sensor_0/Depth_0/image/data"
        depth_timelist = []
        color_topic = "/device_0/sensor_1/Color_0/image/data"
        color_timelist = []
        print("Saving images...")
        with tqdm.tqdm(total=bag.get_end_time(), unit="s") as tl:
            for topic, data, time in bag.read_messages(topics=[depth_topic, color_topic]):
                timestr = "%d.%06d" % (time.secs, int(time.nsecs/1000))
                if topic == depth_topic:
                    ret, buf = cv2.imencode(".png", br.imgmsg_to_cv2(data, desired_encoding="bgr8"))
                    if ret != True: raise RuntimeError("Error encoding image!")
                    output.writestr("depth/%s.png" % timestr, buf)
                if topic == color_topic:
                    ret, buf = cv2.imencode(".png", br.imgmsg_to_cv2(data, desired_encoding="bgr8"))
                    if ret != True: raise RuntimeError("Error encoding image!")
                    output.writestr("rgb/%s.png" % timestr, buf)
                depth_timelist.append("{0} depth/{0}.png".format(timestr))
                color_timelist.append("{0} rgb/{0}.png".format(timestr))
                tl.n = time.secs + time.nsecs/1e9
                tl.refresh()
    timeheader = '''# %s images
# file: '{0}'
# timestamp filename
'''.format(filepath)

    output.writestr("depth.txt", timeheader % 'depth maps' + '\n'.join(depth_timelist))
    output.writestr("rgb.txt", timeheader % 'color images' + '\n'.join(color_timelist))
    output.close()
    print("Done.")

if __name__ == "__main__":
    convert(sys.argv[1])