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

ROS同步解包

  • 引言
  • [1 鱼香ROS一键安装ros-docker脚本:](#1 鱼香ROS一键安装ros-docker脚本:)
  • [2 指定目录映射](#2 指定目录映射)
  • [3 数据解包](#3 数据解包)
    • [3.1 解包脚本](#3.1 解包脚本)
    • [3.2 依赖安装](#3.2 依赖安装)
    • [3.3 运行脚本,解包](#3.3 运行脚本,解包)

引言

总结步骤如下:

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

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

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

python 复制代码
wget http://fishros.com/install -O fishros && . fishros



注:alianros为容器名称

2 指定目录映射

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

  1. 提交当前容器为新镜像
    首先,将当前运行中的容器alianros提交为一个新的镜像。这将保留容器中的所有更改和数据。
python 复制代码
docker commit alianros alian_custom
  1. 启动新容器并指定新的目录映射
    使用新创建的镜像alian_custom启动一个新的容器,并在启动时指定新的目录映射。
python 复制代码
docker run -it --name alian_new \
    -v /new/path/on/host:/new/path/in/container \
    [其他必要的参数] \
    alian_custom
  1. 停止并移除旧容器(可选)
    如果新容器运行正常,您可以选择停止并移除旧容器alian。
python 复制代码
docker stop alianros
docker rm alianros
  1. 重命名新容器(可选)
    如果需要,您可以将新容器重命名为原容器的名称。
python 复制代码
docker rename alian_new alianros
  1. 设置终端输入容器名来启动容器
    打开 shell 配置文件
python 复制代码
nano ~/.bashrc  # 如果是 bash 用户

在文件的最后,添加如下内容:

python 复制代码
alias alianros='docker exec -it alianros /bin/bash'

保存并应用更改

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

python 复制代码
source ~/.bashrc  # 如果是 bash 用户

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

python 复制代码
alianros

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,将下面的代码复制进去

python 复制代码
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
author:alian
2024.10.03
安装必要的包
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

class BagUnpacker(object):  # 1激光雷达+1摄像头同步
    def __init__(self, bag_path, camera_topic, pointcloud_topic, save_path):
        """
        初始化BagUnpacker类。

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

        # 创建保存目录
        self.images_dir = os.path.join(self.save_path, 'images')
        self.pointcloud_dir = os.path.join(self.save_path, 'pointcloud')
        if not os.path.exists(self.images_dir):
            os.makedirs(self.images_dir)
        if not os.path.exists(self.pointcloud_dir):
            os.makedirs(self.pointcloud_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))


        # 收集点云数据
        print("收集点云数据...")
        pointcloud_msgs = []
        for topic, msg, t in tqdm(bag.read_messages(self.pointcloud_topic)):
            timestamp = msg.header.stamp.to_sec()
            pointcloud = self.pointcloud2_to_xyz(msg)
            pointcloud_msgs.append((timestamp, pointcloud))

        bag.close()

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

        print("开始时间同步和保存数据...")
        image_timestamps = [msg[0] for msg in image_msgs]
        image_data = [msg[1] for msg in image_msgs]

        # 使用 tqdm 显示点云处理进度
        for pc_timestamp, pc_data in tqdm(pointcloud_msgs):
            # 寻找最接近的图像时间戳
            closest_idx = self.find_closest_timestamp(pc_timestamp, image_timestamps)
            closest_image = image_data[closest_idx]
            closest_image_timestamp = image_timestamps[closest_idx]

            # 使用点云的时间戳进行命名
            filename = "{0:.6f}".format(pc_timestamp)

            # 保存图像
            image_filename = os.path.join(self.images_dir, filename + ".jpg")
            cv2.imwrite(image_filename, closest_image)

            # 保存点云
            pcd_filename = os.path.join(self.pointcloud_dir, filename + ".pcd")
            self.save_pcd(pc_data, pcd_filename)

            # 可选:打印保存的信息
            # print("保存文件: ", filename)

        print("解包完成。")

    def find_closest_timestamp(self, target, timestamps):
        """
        寻找最接近的时间戳索引。

        :param target: 目标时间戳
        :param timestamps: 时间戳列表
        :return: 最接近的时间戳索引
        """
        closest_idx = np.argmin(np.abs(np.array(timestamps) - target))
        return closest_idx

    def pointcloud2_to_xyz(self, pointcloud_msg):
        """
        将PointCloud2消息转换为XYZ点云。

        :param pointcloud_msg: sensor_msgs/PointCloud2消息
        :return: Nx3的numpy数组
        """
        # 使用pypcd解析PointCloud2
        try:
            # pypcd需要PCD格式的数据,这里需要手动转换
            # 先将PointCloud2转换为PCD格式的内存数据
            header = pointcloud_msg.header
            width = pointcloud_msg.width
            height = pointcloud_msg.height
            fields = pointcloud_msg.fields
            is_bigendian = pointcloud_msg.is_bigendian
            point_step = pointcloud_msg.point_step
            row_step = pointcloud_msg.row_step
            data = pointcloud_msg.data
            is_dense = pointcloud_msg.is_dense

            # 将数据写入临时文件
            temp_pcd_path = "/tmp/temp_pointcloud.pcd"
            with open(temp_pcd_path, 'w') as f:
                f.write("# .PCD v0.7 - Point Cloud Data file format\n")
                f.write("VERSION 0.7\n")
                f.write("FIELDS x y z\n")
                f.write("SIZE 4 4 4\n")
                f.write("TYPE F F F\n")
                f.write("COUNT 1 1 1\n")
                f.write("WIDTH {}\n".format(width))
                f.write("HEIGHT {}\n".format(height))
                f.write("VIEWPOINT 0 0 0 1 0 0 0\n")
                f.write("POINTS {}\n".format(width * height))
                f.write("DATA ascii\n")

                # Extract x, y, z from each point
                for i in range(width * height):
                    offset = i * point_step
                    x = struct.unpack('f', data[offset:offset+4])[0]
                    y = struct.unpack('f', data[offset+4:offset+8])[0]
                    z = struct.unpack('f', data[offset+8:offset+12])[0]
                    f.write("{0} {1} {2}\n".format(x, y, z))

            # 使用pypcd读取临时PCD文件
            pc = pypcd.PointCloud.from_path(temp_pcd_path)
            xyz = np.vstack((pc.pc_data['x'], pc.pc_data['y'], pc.pc_data['z'])).transpose()

            # 删除临时文件
            os.remove(temp_pcd_path)

            return xyz
        except Exception as e:
            print("转换点云时出错: ", e)
            return np.array([])

    def save_pcd(self, pointcloud, filename):
        """
        将点云数据保存为PCD文件。

        :param pointcloud: Nx3的numpy数组
        :param filename: 保存路径
        """
        if pointcloud.size == 0:
            print("空点云数据,跳过保存。")
            return

        # 创建 PCD 对象
        pointcloud = np.array(pointcloud, dtype=np.float32)
        pc = pypcd.make_xyz_point_cloud(pointcloud)
        pc.save_pcd(filename, compression='binary')

class BagUnpacker_2cam(object):  # 1激光雷达+2摄像头同步
    def __init__(self, bag_path, camera0_topic,camera1_topic, pointcloud_topic, save_path):
        """
        初始化BagUnpacker类。

        :param bag_path: .bag文件路径
        :param camera_topic: 相机话题名称
        :param pointcloud_topic: 点云话题名称
        :param save_path: 数据保存路径
        """
        self.bag_path = bag_path
        self.camera_topic0 = camera0_topic
        self.camera_topic1 = camera1_topic
        self.pointcloud_topic = pointcloud_topic
        self.save_path = save_path

        # 创建保存目录
        self.images0_dir = os.path.join(self.save_path, 'images0')
        self.images1_dir = os.path.join(self.save_path, 'images1')
        self.pointcloud_dir = os.path.join(self.save_path, 'pointcloud')
        if not os.path.exists(self.images0_dir):
            os.makedirs(self.images0_dir)
        if not os.path.exists(self.images1_dir):
            os.makedirs(self.images1_dir)
        if not os.path.exists(self.pointcloud_dir):
            os.makedirs(self.pointcloud_dir)

        self.bridge = CvBridge()

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

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

        # 收集点云数据
        print("收集点云数据...")
        pointcloud_msgs = []
        for topic, msg, t in tqdm(bag.read_messages(self.pointcloud_topic)):
            timestamp = msg.header.stamp.to_sec()
            pointcloud = self.pointcloud2_to_xyz(msg)
            pointcloud_msgs.append((timestamp, pointcloud))

        bag.close()

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

        print("开始时间同步和保存数据...")
        image0_timestamps = [msg[0] for msg in image0_msgs]
        image0_data = [msg[1] for msg in image0_msgs]
        image1_timestamps = [msg[0] for msg in image1_msgs]
        image1_data = [msg[1] for msg in image1_msgs]

        # 使用 tqdm 显示点云处理进度
        for pc_timestamp, pc_data in tqdm(pointcloud_msgs):
            # 寻找最接近的图像时间戳
            closest_idx0 = self.find_closest_timestamp(pc_timestamp, image0_timestamps)
            closest_image0 = image0_data[closest_idx0]
            closest_idx1 = self.find_closest_timestamp(pc_timestamp, image1_timestamps)
            closest_image1 = image1_data[closest_idx1]

            # 使用点云的时间戳进行命名
            filename = "{0:.6f}".format(pc_timestamp)

            # 保存图像
            image0_filename = os.path.join(self.images0_dir, filename + ".jpg")
            cv2.imwrite(image0_filename, closest_image0)
            image1_filename = os.path.join(self.images1_dir, filename + ".jpg")
            cv2.imwrite(image1_filename, closest_image1)

            # 保存点云
            pcd_filename = os.path.join(self.pointcloud_dir, filename + ".pcd")
            self.save_pcd(pc_data, pcd_filename)

            # 可选:打印保存的信息
            # print("保存文件: ", filename)

        print("解包完成。")

    def find_closest_timestamp(self, target, timestamps):
        """
        寻找最接近的时间戳索引。

        :param target: 目标时间戳
        :param timestamps: 时间戳列表
        :return: 最接近的时间戳索引
        """
        closest_idx = np.argmin(np.abs(np.array(timestamps) - target))
        return closest_idx

    def pointcloud2_to_xyz(self, pointcloud_msg):
        """
        将PointCloud2消息转换为XYZ点云。

        :param pointcloud_msg: sensor_msgs/PointCloud2消息
        :return: Nx3的numpy数组
        """
        # 使用pypcd解析PointCloud2
        try:
            # pypcd需要PCD格式的数据,这里需要手动转换
            # 先将PointCloud2转换为PCD格式的内存数据
            header = pointcloud_msg.header
            width = pointcloud_msg.width
            height = pointcloud_msg.height
            fields = pointcloud_msg.fields
            is_bigendian = pointcloud_msg.is_bigendian
            point_step = pointcloud_msg.point_step
            row_step = pointcloud_msg.row_step
            data = pointcloud_msg.data
            is_dense = pointcloud_msg.is_dense

            # 将数据写入临时文件
            temp_pcd_path = "/tmp/temp_pointcloud.pcd"
            with open(temp_pcd_path, 'w') as f:
                f.write("# .PCD v0.7 - Point Cloud Data file format\n")
                f.write("VERSION 0.7\n")
                f.write("FIELDS x y z\n")
                f.write("SIZE 4 4 4\n")
                f.write("TYPE F F F\n")
                f.write("COUNT 1 1 1\n")
                f.write("WIDTH {}\n".format(width))
                f.write("HEIGHT {}\n".format(height))
                f.write("VIEWPOINT 0 0 0 1 0 0 0\n")
                f.write("POINTS {}\n".format(width * height))
                f.write("DATA ascii\n")

                # Extract x, y, z from each point
                for i in range(width * height):
                    offset = i * point_step
                    x = struct.unpack('f', data[offset:offset+4])[0]
                    y = struct.unpack('f', data[offset+4:offset+8])[0]
                    z = struct.unpack('f', data[offset+8:offset+12])[0]
                    f.write("{0} {1} {2}\n".format(x, y, z))

            # 使用pypcd读取临时PCD文件
            pc = pypcd.PointCloud.from_path(temp_pcd_path)
            xyz = np.vstack((pc.pc_data['x'], pc.pc_data['y'], pc.pc_data['z'])).transpose()

            # 删除临时文件
            os.remove(temp_pcd_path)

            return xyz
        except Exception as e:
            print("转换点云时出错: ", e)
            return np.array([])

    def save_pcd(self, pointcloud, filename):
        """
        将点云数据保存为PCD文件。

        :param pointcloud: Nx3的numpy数组
        :param filename: 保存路径
        """
        if pointcloud.size == 0:
            print("空点云数据,跳过保存。")
            return

        # 创建 PCD 对象
        pointcloud = np.array(pointcloud, dtype=np.float32)
        pc = pypcd.make_xyz_point_cloud(pointcloud)
        pc.save_pcd(filename, compression='binary')

if __name__ == "__main__":
    import argparse

    parser = argparse.ArgumentParser(description="解包ROS .bag文件,提取相机和点云数据并同步保存。")
    parser.add_argument('--bag_path', type=str, required=True, help='.bag文件路径')
    parser.add_argument('--camera0_topic', type=str, default='/use_cam/image_raw', help='相机话题名称')
    parser.add_argument('--camera1_topic', type=str, default='/use_cam/image_raw', help='相机话题名称')
    parser.add_argument('--pointcloud_topic', type=str, default='/lslidar_point_cloud', help='点云话题名称')
    parser.add_argument('--save_path', type=str, required=True, help='数据保存路径')

    args = parser.parse_args()
    # 1激光雷达+1摄像头同步
    # unpacker = BagUnpacker(
    #     bag_path=args.bag_path,
    #     camera_topic=args.camera0_topic,
    #     pointcloud_topic=args.pointcloud_topic,
    #     save_path=args.save_path
    # )

    # unpacker.unpack()
    # 1激光雷达+2摄像头同步
    unpacker = BagUnpacker_2cam(
        bag_path=args.bag_path,
        camera0_topic=args.camera0_topic,
        camera1_topic=args.camera1_topic,
        pointcloud_topic=args.pointcloud_topic,
        save_path=args.save_path
    )

    unpacker.unpack()
"""
终端命令:
roscore
rosbag info xxx.bag # 查看话题名称
python ROS_bag/bag_unpacker.py --bag_path ROS_bag/cali_lidar_camera.bag --camera1_topic /usb_cam/image_raw --pointcloud_topic /lslidar_point_cloud --save_path ROS_bag/cali
python ROS_bag/bag_unpacker.py --bag_path ROS_bag/2024-07-08-19-54-56_lidar2cam_12.bag --camera0_topic /img0 --camera1_topic /img1 --pointcloud_topic /benewake//SyncX2PointCloud --save_path ROS_bag/cali_12
"""

3.2 依赖安装

确保进入自定义的ROS1容器

查看容器的python版本,如图为2.7

  1. 安装pip及必要依赖
python 复制代码
sudo apt-get update
sudo apt-get install python-opencv python-numpy python-pip
sudo apt-get install ros-melodic-cv-bridge  # 根据您的 ROS 版本调整
如果您的容器中没有 pip,您可以按照以下步骤安装 pip:
下载 get-pip.py 脚本:
curl https://bootstrap.pypa.io/pip/2.7/get-pip.py -o get-pip.py
使用 Python 2.7 运行该脚本:
python get-pip.py
  1. 在 Python 2.7 容器中安装 tqdm(进度条)/pypcd
python 复制代码
pip install tqdm
安装完成后,您可以验证 tqdm 是否成功安装:
python -c "import tqdm; print(tqdm.__version__)"
# 安装 pypcd
pip install pypcd
  1. 在 Docker 容器中安装 vim nano gedit等终端文本编辑器(后续有用)
    vim 通常也很适合在容器中使用。如果你想用 vim,可以通过以下命令安装:
    apt-get update
    apt-get install vim nano gedit
    安装完成后,通过以下命令打开并编辑 .py 文件:
    vim your_script.py

vim 的基本使用步骤和常用命令说明:

  1. 打开文件 在终端中输入以下命令来打开文件: vim your_script.py 如果文件不存在,vim 会创建一个新的文件。
  2. 进入插入模式(编辑模式) 默认情况下,vim 启动时是 命令模式,你无法直接编辑文件。要开始编辑,需要进入 插入模式: 按下 i 键,进入插入模式。此时可以输入、修改文件的内容。
  3. 保存文件并退出 编辑完成后,需要返回 命令模式 来保存文件并退出。按如下步骤操作: 退出插入模式:按下 Esc 键。 保存并退出: 输入 :wq 然后按 Enter 保存文件并退出。 如果只想保存文件但不退出,可以输入 :w 然后按 Enter。
    如果想退出但不保存,可以输入 :q! 然后按 Enter。
  4. 常用命令 i:进入插入模式(在当前光标位置编辑)。 Esc:退出插入模式,回到命令模式。 :w:保存文件但不退出。 :wq 或 ZZ:保存并退出。 :q!:强制退出(不保存修改)。 x:删除当前光标处的字符。 dd:删除当前行。 yy:复制当前行。
    p:在光标下方粘贴。 u:撤销上一次操作。
  5. 基本工作流程 打开文件:vim your_file.py 进入插入模式:按 i,编辑文件。 完成编辑后:按 Esc 退出插入模式。 保存并退出:输入 :wq 然后按 Enter。
  6. 查找内容 在命令模式下,按 / 键然后输入你要查找的内容: /find_this_text 然后按 Enter,vim 会跳到匹配的地方。使用 n 和 N 可以跳到下一个或上一个匹配项。
  7. 常见问题 如果 vim 在启动时显示成只读模式,使用 :w! 可以强制保存文件。

3.3 运行脚本,解包

执行指令如下:

python 复制代码
#打开一个终端(进入容器内)
roscore
# 打开第二个终端(进入容器内)
rosbag info xxx.bag # 查看话题名称

笔者以1个激光雷达+2个相机录制的Bag包为例:

python 复制代码
# 1个激光雷达+1个相机执行指令
python ROS_bag/bag_unpacker.py --bag_path ROS_bag/cali_lidar_camera.bag --camera1_topic /usb_cam/image_raw --pointcloud_topic /lslidar_point_cloud --save_path ROS_bag/cali
# 1个激光雷达+2个相机执行指令
python ROS_bag/bag_unpacker.py --bag_path ROS_bag/2024-07-08-19-54-56_lidar2cam_12.bag --camera0_topic /img0 --camera1_topic /img1 --pointcloud_topic /benewake//SyncX2PointCloud --save_path ROS_bag/cali_12

成功解包如下:


BUG解决

BUG1:

BUG2:

解决方法:

python 复制代码
cd /usr/local/lib/python2.7/dist-packages/pypcd
vim pypcd.py

修改如下:

相关推荐
工业3D_大熊2 小时前
【CAE开发SDK】CEETRON Envision:适用于桌面端、Web端的数据可视化与分析
3d·数据分析·虚拟仿真·cae·cae系统开发·cae可视化·cae数据分析
天天进步20152 小时前
使用 Three.js 创建一个 3D 人形机器人仿真系统
3d
LL.。5 小时前
目标检测——基于yolov8和pyqt的螺栓松动检测系统
yolo·目标检测·pyqt
亿达四方SOLIDWORKS6 小时前
3D制图软件SOLIDWORKS:开启高效设计与创新的新纪元
3d·csdn开发云
资源补给站11 小时前
大恒相机开发(2)—Python软触发调用采集图像
开发语言·python·数码相机
supermapsupport14 小时前
iClient3D for Cesium在Vue中快速实现场景卷帘
前端·vue.js·3d·cesium·supermap
火星机器人life15 小时前
基于ceres优化的3d激光雷达开源算法
算法·3d
jndingxin21 小时前
OpenCV相机标定与3D重建(26)计算两个二维点集之间的部分仿射变换矩阵(2x3)函数 estimateAffinePartial2D()的使用
opencv·3d
烧技湾1 天前
RTMW:实时多人2D和3D 全人体姿态估计
3d·全人体关键点检测