提取Kinect录制的rosbag中的RGBD数据

利用kinect录制好rosbag后，没法通过ros自带的工具或kalibr提取出bag中的rgb和深度图（图像格式不对），因此自己按照网上的一些方法写了一个脚本，功能包括提取rosbag中的rgb和深度图像，以及相应的时间戳文件（后续如果要跑rgbd的slam，直接用TUM的associate.py对齐rgb和深度图的时间戳即可）。
源码如下：

# coding:utf-8
#!/usr/bin/python
    
# Extract images from a bag file.
    
#PKG = 'beginner_tutorials'
import roslib;   #roslib.load_manifest(PKG)
import rosbag
import rospy
import cv2
import argparse
import os

from sensor_msgs.msg import Image
from cv_bridge import CvBridge
from cv_bridge import CvBridgeError
    
class ImageCreator():
    def __init__(self, bagfile, rgbpath, depthpath, rgbstamp, depthstamp):
        self.bridge = CvBridge()
        with rosbag.Bag(bagfile, 'r') as bag:
            for topic,msg,t in bag.read_messages():
                if topic == "/camera/rgb/image_raw": #图像的topic；
                        try:
                            cv_image = self.bridge.imgmsg_to_cv2(msg,"bgr8")
                        except CvBridgeError as e:
                            print e
                        timestr = "%.6f" %  msg.header.stamp.to_sec()
                        #%.6f表示小数点后带有6位，可根据精确度需要修改；
                        image_name = timestr+ ".png" #图像命名：时间戳.png
                        cv2.imshow("color", cv_image)
                        cv2.waitKey(1);
                        cv2.imwrite(rgbpath + image_name, cv_image)  #保存；

                        #写入时间戳
                        with open(rgbstamp, 'a') as rgb_time_file:
                            rgb_time_file.write(timestr+" rgb/"+image_name+"\n")


                elif topic == "/camera/depth_registered/image_raw": #图像的topic；
                        try:
                            cv_image = self.bridge.imgmsg_to_cv2(msg,"16UC1")
                        except CvBridgeError as e:
                            print e
                        timestr = "%.6f" %  msg.header.stamp.to_sec()
                        #%.6f表示小数点后带有6位，可根据精确度需要修改；
                        image_name = timestr+ ".png" #图像命名：时间戳.png
                        cv2.imwrite(depthpath + image_name, cv_image)  #保存；

                        #写入时间戳
                        with open(depthstamp, 'a') as depth_time_file:
                            depth_time_file.write(timestr+" depth/"+image_name+"\n")
    
if __name__ == '__main__':
    #rospy.init_node(PKG)
    parser = argparse.ArgumentParser(description="Grab the rgb and depth images from a ros bag")
    # parser.add_argument("--verbose", "-v", action='store_true', help='verbose_mode')
    help = "The bag file"
    parser.add_argument('bag', help=help)
    help="The output folder"
    parser.add_argument('output_folder', help=help)
    args = parser.parse_args()

    bagfile = args.bag
    rgb_path = args.output_folder + '/rgb/'
    depth_path = args.output_folder + '/depth/'

    rgb_timestamp_txt =  args.output_folder + "/rgb.txt"
    depth_timestamp_txt = args.output_folder + "/depth.txt"

    if not os.path.exists(rgb_path):
        os.makedirs(rgb_path)
    if not os.path.exists(depth_path):
        os.makedirs(depth_path)
    
    try:
        image_creator = ImageCreator(bagfile, rgb_path, depth_path, rgb_timestamp_txt, depth_timestamp_txt)
    except rospy.ROSInterruptException:
        pass