从批量bag包中提取图像和点云数据

提示：文章仅作为自己学习过程中的记录，有问题请指出

---

前言

提示：运行环境和前提

python3
ubuntu 20.04
我使用的数据是在ROS下，采用话题同步的方式对两个相机和雷达进行同步采集得到（因为采集的时候设备是低速运动的，要求的误差不是很高，如果误差精度要求很高的话需要使用硬件进行时间同步）

---

提示：以下是本篇文章正文内容，下面案例可供参考

一、rosbag录制数据和解包

1.录制数据使用rosbag record命令

rosbag record -o bag_name topic1 topic2 ...

这里的 -o 可以命名录制的包的名称，topic代表需要录制的数据的话题，例如我这里需要录制的话题就包含两个相机和一个雷达的话题

rosbag record -o lidar_camera /sync/img1 /sync/img2 /sync/lidar

2.如果只有一个包的话，直接使用下面的命令就行：

rosrun pcl_ros bag_to_pcd xxxx.bag topic out_dir

这里的xxxx.bag对应自己的bag包的名称，topic对应话题信息，out_dir对应保存pcd文件的位置。如果忘记了自己包的相关信息，可以通过下面的命令查看：

rosbag info xxxx.bag

对应信息如下：

如上，我的相机话题对应有两个：/sync/img1 和 /sync/img2
雷达话题：/sync/lidar

二、通过topic从批量bag包读取图像和点云

我这里在采集数据的时候，是在同一个点，相机在不同的曝光条件下进行的数据采集，所以分别录制了很多包；在自动驾驶场景中，采集的图像和点云数据一般是在运动过程中采集的，一般会录制一个比较大的包，就不会设计到批量处理bag包的情况。

1.单独解图像的python代码

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/cv/桌面/livox_basler/image/' #存放图片的位置
class ImageCreator():


    def __init__(self):
        self.bridge = CvBridge()
        with rosbag.Bag('xuexiao.bag', 'r') as bag:   #要读取的bag文件；
            for topic,msg,t in bag.read_messages():
                if topic == "/sync/img1":  #图像的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.imwrite(path+image_name, cv_image)  #保存；

if __name__ == '__main__':

    #rospy.init_node(PKG)

    try:
        image_creator = ImageCreator()
    except rospy.ROSInterruptException:
        pass

这里需要修改的地方有三个：1. path，即解包出来的图像保存的位置 2. bag包的名称 3.图像的topic

2.从批量bag包中同时解图像和点云

里面需要修改对应的图像和雷达的话题信息，以及bag包保存的位置，和解包保存数据的路径。
解包的图像和点云数据我都是用的时间戳来命名，可以验证同步的时间误差是否满足要求。
代码如下（示例）：

import os
import rosbag
import cv2
from cv_bridge import CvBridge
from tqdm import tqdm
import time

class ExtractBagData(object):

    def __init__(self, bagfile_path, camera_topic1,camera_topic2, pointcloud_topic, root):
        self.bagfile_path = bagfile_path
        self.camera_topic1 = camera_topic1
        self.camera_topic2 = camera_topic2
        self.pointcloud_topic = pointcloud_topic
        self.root = root
        self.image_dir1 = os.path.join(root, "image2")
        self.image_dir2 = os.path.join(root, "image6")
        self.pointcloud_dir = os.path.join(root, "pointcloud")

        #创建提取图片和点云的目录
        if not os.path.exists(self.image_dir1):
            os.makedirs(self.image_dir1)
        if not os.path.exists(self.image_dir2):
            os.makedirs(self.image_dir2)
        if not os.path.exists(self.pointcloud_dir):
            os.makedirs(self.pointcloud_dir)

    def extract_camera_topic(self):
        for file_name in os.listdir(self.bagfile_path):
            if file_name.endswith(".bag"):
            	bag_path = os.path.join(self.bagfile_path,file_name)
                bag = rosbag.Bag(bag_path, "r")
                bridge = CvBridge()
                bag_data_imgs = bag.read_messages()

              
                pbar = tqdm(bag_data_imgs)
                for topic, msg, t in pbar:
                    if topic == "/sync/img1":
                        cv_image = bridge.imgmsg_to_cv2(msg, "bgr8")
                        timestr = "%.6f" %  msg.header.stamp.to_sec() #获取时间戳命名
                        cv2.imwrite(os.path.join(self.image_dir1, str(timestr) + ".png"), cv_image)
                    if topic == "/sync/img2":
                        cv_image = bridge.imgmsg_to_cv2(msg, "bgr8")
                        timestr = "%.6f" %  msg.header.stamp.to_sec()
                        cv2.imwrite(os.path.join(self.image_dir2, str(timestr) + ".png"), cv_image)
                   

        image1_list = os.listdir(self.image_dir1)
        image2_list = os.listdir(self.image_dir2)
        image1_list_sort = sorted(image1_list)
        image2_list_sort = sorted(image2_list)


    def extract_pointcloud_topic(self):
        for file_name in os.listdir(self.bagfile_path):
            if file_name.endswith(".bag"):
            	bag_name = self.bagfile_path + "/" + file_name
                cmd = "rosrun pcl_ros bag_to_pcd %s /sync/lidar %s" % (bag_name, self.pointcloud_dir)
                os.system(cmd)

        # 再读取提取的pcd点云数据，把文件名修改为按照时间戳命名
        pcd_files_list = os.listdir(self.pointcloud_dir)
        # 因为提取的pcd是以时间戳命令的，但是存放到列表中并不是按照时间戳从小到大排列，这里对时间戳进行重新排序
        pcd_files_list_sorted = sorted(pcd_files_list)

if __name__ == '__main__':
   
    bagfile_path = "/home/cv/桌面/livox_basler/data1/第二次/bag"
    camera_topic1 = "/sync/img1"
    camera_topic2 = "/sync/img2"
    pointcloud_topic = "/sync/lidar"
    extract_bag = ExtractBagData(bagfile_path, camera_topic1, camera_topic2, pointcloud_topic, "/home/cv/桌面/livox_basler/image")
    extract_bag.extract_camera_topic()
    extract_bag.extract_pointcloud_topic()

如果只需要从一个bag包解图像和点云数据的话，将代码里面的for循环读取bag删除，路径改为对应的bag包就行，代码如下（示例）：

import os
import rosbag
import cv2
from cv_bridge import CvBridge
from tqdm import tqdm
import time

class ExtractBagData(object):

    def __init__(self, bagfile_path, camera_topic, pointcloud_topic, root):
        self.bagfile_path = bagfile_path
        self.camera_topic = camera_topic
        self.pointcloud_topic = pointcloud_topic
        self.root = root
        self.image_dir = os.path.join(root, "image")
        self.pointcloud_dir = os.path.join(root, "pointcloud2")

        #创建提取图片和点云的目录
        if not os.path.exists(self.image_dir):
            os.makedirs(self.image_dir)
        if not os.path.exists(self.pointcloud_dir):
            os.makedirs(self.pointcloud_dir)

    def extract_camera_topic(self):
        bag = rosbag.Bag(self.bagfile_path, "r")
        bridge = CvBridge()
        bag_data_imgs = bag.read_messages(self.camera_topic)

        index = 0
        pbar = tqdm(bag_data_imgs)
        for topic, msg, t in pbar:
            pbar.set_description("Processing extract image id: %s" % (index+1))
            cv_image = bridge.imgmsg_to_cv2(msg, "bgr8")
            timestr = "%.6f" %  msg.header.stamp.to_sec() #获取时间戳命名
            cv2.imwrite(os.path.join(self.image_dir, str(timestr) + ".png"), cv_image)
            index += 1

        image_list = os.listdir(self.image_dir)
        image_list_sort = sorted(image_list)


    def extract_pointcloud_topic(self):
        cmd = "rosrun pcl_ros bag_to_pcd %s /sync/lidar %s" % (self.bagfile_path, self.pointcloud_dir)
        os.system(cmd)

        # 再读取提取的pcd点云数据，把文件名修改为按照时间戳命名
        pcd_files_list = os.listdir(self.pointcloud_dir)
        # 因为提取的pcd是以时间戳命令的，但是存放到列表中并不是按照时间戳从小到大排列，这里对时间戳进行重新排序
        pcd_files_list_sorted = sorted(pcd_files_list)

if __name__ == '__main__':
   
    bagfile_path = "/home/cv/桌面/livox_basler/data1/第二次/bag/1.bag"
    camera_topic = "/sync/img1"
    pointcloud_topic = "/sync/lidar"
    extract_bag = ExtractBagData(bagfile_path, camera_topic, pointcloud_topic, "/home/cv/桌面/livox_basler/image")
    extract_bag.extract_camera_topic()
    extract_bag.extract_pointcloud_topic()

---