【3D目标检测】激光雷达和相机联合标定（一）——ROS同步解包

引言

总结步骤如下：

1. 采集同步数据：ROS录制（推荐），或者代码同步触发采集
2. ROS同步解包：一一对应的图片和点云数据
3. 相机内参标定
4. 激光雷达与相机外参标定
5. 标定信息标准化文件生成
6. 点云与图像配准映射

参考链接🔗：
1. 激光雷达和相机联合标定保姆级教程

1 鱼香ROS一键安装ros-docker脚本：

wget http://fishros.com/install -O fishros && . fishros

注：alianros为容器名称

2 指定目录映射

因为默认目录映射到/home/user，笔者想指定映射到专用的解包文件夹内

1. 提交当前容器为新镜像
   首先，将当前运行中的容器alianros提交为一个新的镜像。这将保留容器中的所有更改和数据。

docker commit alianros alian_custom

2. 启动新容器并指定新的目录映射
   使用新创建的镜像alian_custom启动一个新的容器，并在启动时指定新的目录映射。

docker run -it --name alian_new[新容器] \
	--env="DISPLAY=$DISPLAY" \
    --volume="/new/path/on/host[宿主机]:/new/path/in/container[容器]" \
    --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \
    alian_custom[镜像]

该指令的作用是基于 alianros2 镜像创建一个名为 humros2 的容器，具有以下特性：
将宿主机的 X11 图形系统连接到容器，允许图形界面应用在宿主机的屏幕上显示。
将宿主机的目录 /media/jd/4997BB1603CFE2C4/lianlirong 映射到容器的 /home/jd，实现数据共享。
保持容器的交互式终端运行，允许用户实时操作。
–gpus all 支持GPU

docker run -it --gpus all --env="DISPLAY=$DISPLAY" --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" --volume="/media/jd/4997BB1603CFE2C4/lianlirong:/home/jd" --name humros2 alianros2

3. 停止并移除旧容器（可选）
   如果新容器运行正常，您可以选择停止并移除旧容器alian。

docker stop alianros
docker rm alianros

4. 重命名新容器（可选）
   如果需要，您可以将新容器重命名为原容器的名称。

docker rename alian_new alianros

5. 设置终端输入容器名来启动容器
   打开 shell 配置文件

nano ~/.bashrc  # 如果是 bash 用户

在文件的最后，添加如下内容：

alias alianros='docker exec -it alianros /bin/bash'

保存并应用更改
编辑完成后，保存文件并退出 (Ctrl + O 保存，Ctrl + X 退出)。然后运行以下命令使更改生效：

source ~/.bashrc  # 如果是 bash 用户

只需在终端中输入 alianros，它就会执行 docker exec -it alianros /bin/bash：

alianros

当电脑重启后，如何启动上述容器

# 1 启动Docker 服务
sudo systemctl start docker
# 2  检查容器状态
docker ps -a
# 3 根据终端信息， 启动容器ID
docker start [CONTAINER ID]

3 数据解包

3.1 解包脚本

满足如下条件

1. 能够实现对‘.bag’进行解包，其中包括相机话题‘/use_cam/image_raw’和点云话题‘/lslidar_point_cloud’
2. 数据提取使用时间戳命名，并保持图像数据和点云数据命名的时间同步问题，假设点云有700+数据，图像有1000+，以点云的时间戳为基准，寻找最接近的图像数据，两者以相同的时间戳命名，其中图片文件后缀保存为‘.jpg’，点云文件后缀保存为‘.pcd’
3. 创建图像和点云的保存目录，‘savepath/images’和’savepath/pointcloud’
4. 输入变量包括：.bag路径，相机话题，点云话题，保存路径
5. 最终得到一个输出目录，包括两个子目录‘images’和’pointcloud’，子目录下的图片和点云文件一一匹配

新建脚本文件bag_unpacker.py，将下面的代码复制进去

#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
author:alian
2025.11.17
安装必要的包
sudo apt-get update
sudo apt-get install python-opencv python-numpy python-pip
sudo apt-get install ros-melodic-cv-bridge  # 根据您的 ROS 版本调整
sudo pip install pypcd
本脚本基于 Python 2.7 编写，适用于 Ubuntu 18.04 上的 ROS Melodic
赋予执行权限：
chmod +x bag_unpacker.py
运行脚本：
./bag_unpacker.py --bag_path /path/to/your.bag --camera_topic /use_cam/image_raw --pointcloud_topic /lslidar_point_cloud --save_path /path/to/save
参数说明：
--bag_path：指定要解包的 .bag 文件路径。
--camera_topic：指定相机话题名称，默认值为 /use_cam/image_raw。
--pointcloud_topic：指定点云话题名称，默认值为 /lslidar_point_cloud。
--save_path：指定数据保存的根目录。
"""
import rosbag
import rospy
import os
import cv2
import numpy as np
from cv_bridge import CvBridge
from sensor_msgs.msg import Image, PointCloud2
from pypcd import pypcd
import struct
import sys
from tqdm import tqdm  # 导入 tqdm
import sensor_msgs.point_cloud2 as pc2

class BagUnpacker_onlycam(object):  # 1摄像头
    def __init__(self, bag_path, camera_topic, save_path):
        """
        初始化BagUnpacker类。

        :param bag_path: .bag文件路径
        :param camera_topic: 相机话题名称
        :param save_path: 数据保存路径
        """
        self.bag_path = bag_path
        self.camera_topic = camera_topic
        self.save_path = save_path

        # 创建保存目录
        self.images_dir = os.path.join(self.save_path, 'images')
        if not os.path.exists(self.images_dir):
            os.makedirs(self.images_dir)

        self.bridge = CvBridge()

    def unpack(self):
        """
        解包.bag文件，提取指定话题的数据，并进行时间同步保存。
        """
        print("开始解包.bag文件...")
        bag = rosbag.Bag(self.bag_path, 'r')

        # 收集相机数据
        print("收集相机数据...")
        image_msgs = []
        for topic, msg, t in tqdm(bag.read_messages(self.camera_topic)):
            timestamp = msg.header.stamp.to_sec()
            cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
            image_msgs.append((timestamp, cv_image))

        bag.close()

        # 排序数据
        image_msgs.sort(key=lambda x: x[0])

        print("开始保存数据...")

        # 使用 tqdm 显示点云处理进度
        num = 0
        for img_timestamp, img_data in tqdm(image_msgs):
            if num>3:
                break
            # 使用图像的时间戳进行命名
            filename = "{0:.6f}".format(img_timestamp)

            # 保存图像
            image_filename = os.path.join(self.images_dir, filename