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

ROS同步解包

引言
1 鱼香ROS一键安装ros-docker脚本：
2 指定目录映射
3 数据解包
- 3.1 解包脚本
- 3.2 依赖安装
- 3.3 运行脚本，解包

引言

总结步骤如下：

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

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

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

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

3d734e9f8827d0aee79c1668.png" alt="在这里插入图片描述" />

注：alianros为容器名称

2 指定目录映射

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

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

docker commit alianros alian_custom

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

docker run -it --name alian_new \-v /new/path/on/host:/new/path/in/container \[其他必要的参数] \alian_custom

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

docker stop alianros
docker rm alianros

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

docker rename alian_new alianros

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

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

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

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

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

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

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

alianros

3 数据解包

3.1 解包脚本

满足如下条件

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

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

#!/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  # 导入 tqdmclass 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_pathself.camera_topic = camera_topicself.pointcloud_topic = pointcloud_topicself.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_idxdef pointcloud2_to_xyz(self, pointcloud_msg):"""将PointCloud2消息转换为XYZ点云。:param pointcloud_msg: sensor_msgs/PointCloud2消息:return: Nx3的numpy数组"""# 使用pypcd解析PointCloud2try:# pypcd需要PCD格式的数据，这里需要手动转换# 先将PointCloud2转换为PCD格式的内存数据header = pointcloud_msg.headerwidth = pointcloud_msg.widthheight = pointcloud_msg.heightfields = pointcloud_msg.fieldsis_bigendian = pointcloud_msg.is_bigendianpoint_step = pointcloud_msg.point_steprow_step = pointcloud_msg.row_stepdata = pointcloud_msg.datais_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 pointfor i in range(width * height):offset = i * point_stepx = 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 xyzexcept 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_pathself.camera_topic0 = camera0_topicself.camera_topic1 = camera1_topicself.pointcloud_topic = pointcloud_topicself.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_idxdef pointcloud2_to_xyz(self, pointcloud_msg):"""将PointCloud2消息转换为XYZ点云。:param pointcloud_msg: sensor_msgs/PointCloud2消息:return: Nx3的numpy数组"""# 使用pypcd解析PointCloud2try:# pypcd需要PCD格式的数据，这里需要手动转换# 先将PointCloud2转换为PCD格式的内存数据header = pointcloud_msg.headerwidth = pointcloud_msg.widthheight = pointcloud_msg.heightfields = pointcloud_msg.fieldsis_bigendian = pointcloud_msg.is_bigendianpoint_step = pointcloud_msg.point_steprow_step = pointcloud_msg.row_stepdata = pointcloud_msg.datais_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 pointfor i in range(width * height):offset = i * point_stepx = 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 xyzexcept 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 argparseparser = 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
在这里插入图片描述

安装pip及必要依赖

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

在 Python 2.7 容器中安装 tqdm(进度条)/pypcd

pip install tqdm
安装完成后，您可以验证 tqdm 是否成功安装：
python -c "import tqdm; print(tqdm.__version__)"
# 安装 pypcd
pip install pypcd

在 Docker 容器中安装 vim nano gedit等终端文本编辑器（后续有用）
vim 通常也很适合在容器中使用。如果你想用 vim，可以通过以下命令安装：
apt-get update
apt-get install vim nano gedit
安装完成后，通过以下命令打开并编辑 .py 文件：
vim your_script.py

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

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

3.3 运行脚本，解包

执行指令如下：

#打开一个终端（进入容器内）
roscore
# 打开第二个终端（进入容器内）
rosbag info xxx.bag # 查看话题名称

笔者以1个激光雷达+2个相机录制的Bag包为例：
在这里插入图片描述

# 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

成功解包如下：
3d603e22ee7cd4.png" alt="在这里插入图片描述" />

BUG解决
BUG1：
3d5ad10aa7.png" alt="在这里插入图片描述" />
BUG2：

解决方法：

cd /usr/local/lib/python2.7/dist-packages/pypcd
vim pypcd.py

修改如下：
在这里插入图片描述