从bag包中提取图片和点云数据为pcd格式点云文件

---

首先说明一下我运行的系统环境：

• python2
• Ubuntu18.04
• ROS melodic

---

1 开始提取bag包之前的准备工作

1、首先查看一下自己的bag包中，相机和激光雷达的topic

(base) shl@zhihui-mint:~$ rosbag info 2021-03-24-11-01-45.bag
path:        2021-03-24-11-01-45.bag
version:     2.0
duration:    23.7s
start:       Mar 24 2021 11:01:45.46 (1616554905.46)
end:         Mar 24 2021 11:02:09.15 (1616554929.15)
size:        860.0 MB
messages:    474
compression: none [474/474 chunks]
types:       sensor_msgs/Image       [060021388200f6f0f447d0fcd9c64743]
             sensor_msgs/PointCloud2 [1158d486dd51d683ce2f1be655c3c181]
topics:      /usb_cam/image_raw   238 msgs    : sensor_msgs/Image
             /velodyne_points     236 msgs    : sensor_msgs/PointCloud2
(base) shl@zhihui-mint:~$

如上：

• 我相机的topic为/usb_cam/image_raw
• 激光雷达的topic为：/velodyne_points

2、由于我相机和激光雷达采集数据的频率都是10Hz，因此提取出之后的数据几乎是每一帧都是保持对齐的！

3、下面提取数据保存的路径

root/
├── images   # 存放提取图片数据，格式jpg
└── pointcloud  # 存放提取的点云数据，格式pcd

2 从bag包中提取图片和点云数据

1、使用rosbag库包提取图片数据

注意：

在提取图片数据的时候，我的rosbag库包是在python2环境下的，因此在运行程序的时候也是使用python2作为解释器进行执行的！

2、把bag包中的点云数据提取为pcd格式点云文件，提取命令：

rosrun pcl_ros bag_to_pcd result.bag /velodyne_points ./pointcloud

• rosrun pcl_ros bag_to_pcd：前面是指定把bag包提取为pcd格式文件
• result.bag：是要提取的bag包文件
• /velodyne_points：是要提取bag包中对应的激光雷达的topic
• ./pointcloud：是提取的pcd格式的一帧一帧的点云文件

3、下面是完整的代码

#coding:utf-8
'''
env:
- python2
- ros melodic

* pcd格式的单帧点云数据，可以通过 pcl_viewer 20.pcd 的形式进行查看



每个msg中存放的信息：
# print(msg)
header:
  seq: 1769
  stamp:
    secs: 1616554905
    nsecs: 527051435
  frame_id: "usb_cam"
height: 720
width: 1280
encoding: "rgb8"
is_bigendian: 0
step: 3840
data: [146, 205, 228, 144, 203, 226, 149, 189, ...]   # 很长的一个列表


'''

__Author__ = "Shliang"
__Email__ = "shliang0603@gmail.com"


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, "images")
        self.pointcloud_dir = os.path.join(root, "pointcloud")

        #创建提取图片和点云的目录 ./root/images  root/pointcloud
        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

        # for topic, msg, t in bag_data_imgs:
        # for topic, msg, t in tqdm(bag_data_imgs):
        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")
            # print('\033[31m=\033[0m'*120)
            # print(topic)  # /usb_cam/image_raw
            # print(msg)
            # print(t)  # 1616554905461126311
            #print(type(cv_image))  # <type 'numpy.ndarray'>
            # cv2.imshow("Image window", cv_image)
            # cv2.waitKey(3)
            # 如果你需要使用时间戳对提取的图片命名，可以使用msg.header.stamp.to_sec()获取时间戳
            # timestr = "%.6f" %  msg.header.stamp.to_sec()
            cv2.imwrite(os.path.join(self.image_dir, str(index) + ".jpg"), cv_image)
            index += 1

    def extract_pointcloud_topic(self):
        '''
        # 提取点云数据为pcd后缀文件，默认提取以为时间戳命名
        # 提取命令：rosrun pcl_ros bag_to_pcd result.bag /velodyne_points ./pointcloud
        # 提取点云以时间戳命令：1616554905.476288682.pcd
        :return:
        '''
        cmd = "rosrun pcl_ros bag_to_pcd %s /velodyne_points %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)
        # print(zip(pcd_files_list, pcd_files_list_sorted))

        index = 0
        pbar = tqdm(pcd_files_list_sorted)
        for pcd_file in pbar:
            pbar.set_description("Processing extract poindcloud id: %s" % (index + 1))
            os.rename(os.path.join(self.pointcloud_dir, pcd_file),
                      os.path.join(self.pointcloud_dir, str(index) + ".pcd"))
            print("pcd_file name: ", pcd_file)
            index += 1



if __name__ == '__main__':
    # bagfile_path = "/home/shl/extract_rosbag_data/0324_bags/plycal_calib/2021-03-24-11-01-45.bag"
    # camera_topic = "/usb_cam/image_raw"
    # pointcloud_topic = "/velodyne_points"
    # extract_bag = ExtractBagData(bagfile_path, camera_topic, pointcloud_topic,  "root")
    # extract_bag.extract_camera_topic()
    # extract_bag.extract_pointcloud_topic()



    bagfile_path = "/home/shl/Desktop/calibrate_data/2021_05_06-17_21_40.bag"
    camera_topic = "/image"
    pointcloud_topic = "/velodyne_points"
    extract_bag = ExtractBagData(bagfile_path, camera_topic, pointcloud_topic,  "/home/shl/Desktop/calibrate_data/extract_data")
    extract_bag.extract_camera_topic()
    extract_bag.extract_pointcloud_topic()