激光雷达和相机数据时间同步的几种方法

从小就看凹凸曼^o^

已于 2023-12-16 21:45:05 修改

阅读量4.5k

点赞数 40

分类专栏： ROS python 文章标签：自动驾驶 ubuntu python

于 2023-12-11 20:59:43 首次发布

本文链接：https://blog.csdn.net/weixin_69667614/article/details/134935582

版权

python 同时被 2 个专栏收录

4 篇文章 0 订阅

订阅专栏

ROS

3 篇文章 0 订阅

订阅专栏

文章讲述了使用Matlab2022b相机和激光雷达联合标定工具箱对时间未校准和已校准的数据进行标定，比较了不同情况下联合标定的误差，并介绍了基于ROS的相机雷达时间同步方法，以及提取和处理bag文件中的时间戳数据的几种方法。

摘要由CSDN通过智能技术生成

一. 引言

图1图2为数据时间未校准，使用Matlab 2022b相机和激光雷达联合标定工具箱进行的联合标定（图1为使用4对jpg和pcd文件时的联合标定效果，图2为使用15对jpg和pcd文件时的联合标定效果）；图3图4为数据时间已校准后，使用Matlab 2022b相机和激光雷达联合标定工具箱进行的联合标定（图3为使用4对jpg和pcd文件时的联合标定效果，图4为使用15对jpg和pcd文件时的联合标定效果）；

四种情况联合标定结果对比如表1所示。

图1：时间未校准（4对）

图2：时间未校准（15对）

图3：时间已校准（4对）

图4：时间已校准（15对）

表1：4种情况效果对比
	Translation Error	Rotation Error	Reprojection Error
时间戳校准15对	0.0306	3.2476	5.1702
时间戳校准4对	0.0419	4.0709	7.2469
时间戳未校准15对	0.1788	13.5330	34.8668
时间戳未校准4对	0.2920	28.7961	52.3936

由表1可以看出，时间同步对于多传感器联合标定来说，具有十分重要的意义。

二. 介绍

相机雷达时间同步（基于ROS）和基于ROS实现多传感器时间软同步提出的方法，是针对bag文件进行离线时间同步，并且当且仅当它们有着相同的时间戳的时候，才把它们发布出去，从而实现一定程度上的多传感器时间同步。在实际应用（本文使用Apollo D-Kit套件中的LI-USB30-AR023ZWDR相机、Velodyne VLP-16激光雷达）中，由于两者话题没有相同的时间戳，因此提取出来的bag文件是个空包。

1. 录制bag文件

rosbag record -O <包名.bag> <话题1> <话题2> ... <话题n>

如：录制激光雷达话题/velodyne_points和相机话题/usb_cam/image_raw，并保存为001.bag：

rosbag record -O 001.bag /usb_cam/image_raw /velodyne_points

2. 显示bag文件的信息

rosbag info <包名.bag>

如：

rosbag info 001.bag

3. 查看时间戳

roscore

rqt_bag <包名.bag>

如：

roscore
rqt_bag 001.bag

4. 提取带时间戳的pcd文件

rosrun pcl_ros bag_to_pcd <包名.bag> <话题名> <要保存的路径>

如：

rosrun pcl_ros bag_to_pcd 001.bag /velodyne_points /home/wrk/data_imgpcd/velodyne

最终效果如上图所示。

三. 方法一

我没有学习过ROS，也没学习过“话题”“消息”“发布者”“订阅者”，这个只能算是笨办法：首先从bag文件中提取出带时间戳的jpg文件和pcd文件，然后将文件一起放在同一个目录下面，最后按时间戳进行排序并重新命名。

1. 提取带时间戳的jpg和pcd文件

在从.bag文件中读取并保存.jpg图片和.pcd点云中找到了提取带时间戳的jpg文件（但是pcd文件不带时间戳），所以我就在这里给它注释修改了一下：

#! /usr/bin/python
# coding=utf-8

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

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

        pbar = tqdm(bag_data_imgs)
        for topic, msg, t in pbar: # type: ignore
            pbar.set_description("Processing extract image id: %s" % (index+1))
            cv_image = bridge.imgmsg_to_cv2(msg, "bgr8")
            # 如果你需要使用时间戳对提取的图片命名，可以使用msg.header.stamp.to_sec()获取时间戳
            timestr = "%.6f" %  msg.header.stamp.to_sec()
            cv2.imwrite(os.path.join(self.image_dir, str(timestr) + ".jpg"), cv_image) # type: ignore
            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/wrk/001.bag'
    camera_topic = "/usb_cam/image_raw"
    pointcloud_topic = "/velodyne_points"
    extract_bag = ExtractBagData(bagfile_path, camera_topic, pointcloud_topic,  "/home/wrk/data_imgpcd")
    extract_bag.extract_camera_topic()
    extract_bag.extract_pointcloud_topic()

具体使用方法：

在VScode（可以使用其他软件）中复制并粘贴以上内容；
将“bagfile_path”换成自己的包文件地址、将“camera_topic”换成自己的相机话题名、将“pointcloud_topic”换成自己的激光雷达话题名、将“extract_bag”换成自己的保存路径；
右键选择“运行python”，选择“在终端中运行python文件”。

2. 将两种文件放在同一目录下

3. 重命名

#coding=utf-8
import os
 
def rename(filepath):
    """
    filePath:文件目录
    return:None
    """
    filenamelist = os.listdir(filepath)
    filenamelist = sorted(filenamelist, key=lambda x: eval(x[0:-4]))
    #将filepath目录下的文件读取为列表,并排序
    
    newnamelist = ["{:0>4}".format(i) for i in range(len(filenamelist))]
    #重命名为占4个字符宽度,非零数字右对齐,其余空位用0补齐. 如"0001"、"0002"、"0099"
    for i in range(len(filenamelist)):
        old_path = os.path.join(filepath,filenamelist[i])
        #旧文件路径
        new_path = os.path.join(filepath,newnamelist[i]+filenamelist[i][-4::])
        #新文件路径(保留文件后缀)
        os.rename(old_path,new_path)
        #重命名操作
    """"""
    print("Finish")
    return None
 
rename("/home/wrk/data")

具体使用方法：

在VScode（可以使用其他软件）中复制并粘贴以上内容；
将“filepath”换成自己的目录地址；
右键选择“运行python”，选择“在终端中运行python文件”。

最终效果如下图所示：

4. 总结

选择相邻的jpg和pcd文件，这个笨办法虽然没有写的那么漂亮，只能针对bag文件提取信息（离线使用），不能连接传感器在线实时使用，但是也基本满足我的需求了。

四. 方法二

稍微学习了一下ROS机器人操作系统，尝试自己写代码将图像数据和激光雷达数据提取出来。并根据时间戳进行命名，然后手动选取相邻的图像和激光雷达数据，以此实现时间的校准。

1. 离线读取bag数据

主要步骤为：

创建工作空间（有工作空间就忽略）
创建软件包
编译
编写py文件
编写launch文件
启动launch文件

(一). 创建工作空间：

mkdir -p 工作空间名/src

如：

mkdir -p catkin_workspace/src

(二). 创建软件包

cd ~/工作空间名/src

catkin_create_pkg <软件包名> <依赖项1> <依赖项2> ...<依赖项n>

如：

cd ~/catkin_workspace/src
catkin_create_pkg data2jpgcsv rospy sensor_msgs cv_bridge

(三). 编译

cd ~/工作空间名/src

catkin_make

如：

cd ~/catkin_workspace/
catkin_make

(四). 编写py文件

(1). 使用VScode，在自己“～/home/用户名/工作空间名/src/软件包”目录下创建scripts文件夹：

或：

cd ~/工作空间/src/软件包/

mkdir scripts

(2). 在“～/home/用户名/工作空间名/src/软件包/scripts”目录下创建data2jpgcsv.py文件，并复制以下内容：

#! /usr/bin/python
# coding = utf-8

import cv2
import rospy
import numpy as np
import pandas as pd
from sensor_msgs.msg import Image
from sensor_msgs import point_cloud2
from sensor_msgs.msg import PointCloud2
from cv_bridge import CvBridge


def LidarCallBack(msg):
    if isinstance(msg, PointCloud2):
        gen = point_cloud2.read_points(msg)
        arr = []
        for p in gen:
            arr.append(p)
        arr = np.array(arr)
        pd.DataFrame(arr).to_csv("/home/wrk/data_imgcsv/{}.csv".format(msg.header.stamp))
        rospy.logwarn("Success Write in:{}.csv".format(msg.header.stamp))
    elif isinstance(msg, Image):
        cv_img = bridge.imgmsg_to_cv2(msg, "bgr8")
        cv2.imwrite("/home/wrk/data_imgcsv/{}.jpg".format(msg.header.stamp) , cv_img) # type: ignore
        rospy.loginfo("Success Write in:{}.jpg".format(msg.header.stamp))
    else:
        pass

    return None

rospy.init_node("lidar_node")
bridge = CvBridge()
camer_sub = rospy.Subscriber("/usb_cam/image_raw",Image,LidarCallBack,queue_size=10)
lidar_sub = rospy.Subscriber("/velodyne_points",PointCloud2,LidarCallBack,queue_size=10)
rospy.spin()

注：

将文件夹路径换成自己的
将相机话题、激光雷达话题换成自己的

(3). 右键此py文件，勾选“允许作为程序执行文件”

(五). 编写launch文件

(1). 使用VScode，在自己“～/home/用户名/工作空间名/src/软件包”目录下创建launch文件夹：

或：

cd ~/工作空间/src/软件包/

mkdir launch

(2). 在“～/home/用户名/工作空间名/src/软件包/launch”目录下创建outline.launch文件，并复制以下内容：

<launch>

    <node pkg="rosbag" type="play" name="player" output="screen" args="--clock /home/wrk/001.bag"/>

    <node pkg="data2jpgcsv" type="data2jpgcsv.py" name="data2jpgcsv"/>

</launch>

注：

将bag文件路径换成自己的路径
将pkg换成自己的包名，将type换成自己的文件名，将name换成自己的节点名

(六). 启动launch文件

请确保文件结构如下图所示一致：

catkin_workspace
├── build
├── devel
└── src
├── CMakeLists.txt -> /opt/ros/melodic/share/catkin/cmake/toplevel.cmake
          └── data2jpgcsv
                        ├── CMakeLists.txt
                        ├── launch
│          └── outline.launch
                        ├── package.xml
                        ├── scripts
                        │          └── data2jpgcsv.py
                        └── src

启动launch文件的命令格式：

roslaunch <软件包名> <launch文件名.launch>

如：

roslaunch data2jpgcsv outline.launch

2. 在线读取传感器数据

(一). 启动相机节点

ROS下usb_cam的安装

Ubuntu 18.04 下使用 RViz 显示出摄像头视频教程

使用Rviz完成摄像头（camera）的视频采集

ROS下使用摄像头

ubuntu20.04的usb_cam安装测试（亲测）

(二). 启动激光雷达节点

ubuntu20.04 ROS 环境下使用velodyne激光雷达与 Flir 工业相机

ROS采集激光雷达点云数据

Velodyne VLP-32C/VLP-16在ROS下的数据获取、显示及录制

Ubuntu18.04 运行velodyne

激光雷达Velodyne16配置及录制rosbag

(三). 启动录制节点

rosrun data2jpgcsv data2jpgcsv.py

3. 总结

离线读取bag文件和在线读取传感器数据的原理一致。区别在于，离线读取是先用launch文件启动播放bag文件，将消息发布在电脑中，然后启动节点读取数据；在线读取是启动传感器节点之后，消息同样也被发布在电脑中，然后启动节点读取转化数据。

最终效果如图所示：

五. 方法三

从提取出来的数据可以看出，两个相邻的激光雷达数据之间有3～4个图像数据，因此我们选择激光雷达数据及其相邻的图像数据，并写入bag文件中，以此实现时间上的校准。

写入bag文件的目的，可以根据方法一进行提取出jpg和pcd文件，方便使用Matlab进行联合标定。

1. 离线读取bag数据

将bag文件读取成为新的bag文件

(一). 编写py文件

(1). 在“～/home/用户名/工作空间名/src/软件包/scripts”目录下创建data2jpgcsv.py文件，并复制以下内容：

#! /usr/bin/python
# coding = utf-8

import rospy
import rosbag
from sensor_msgs.msg import Image
from sensor_msgs.msg import PointCloud2

bag = rosbag.Bag('/home/wrk/dataset.bag','w')

k = 0

def MsgsCallBack(msg):
    global k
    if isinstance(msg, PointCloud2):
        bag.write('PointCloud2',msg)
        k = 0
    elif isinstance(msg, Image):
        k = k +1
        if k == 1:
            bag.write('Image',msg)
        else:
            pass
    else:
        pass

rospy.init_node('drone_track_data_saver')
rospy.Subscriber("/usb_cam/image_raw",Image,MsgsCallBack,queue_size=10)
rospy.Subscriber("/velodyne_points",PointCloud2,MsgsCallBack,queue_size=10)

rate = rospy.Rate(1) # 10hz
while not rospy.is_shutdown():
    rate.sleep()
else:
    bag.close()

注：

将文件夹路径换成自己的
将相机话题、激光雷达话题换成自己的

(2). 右键此py文件，勾选“允许作为程序执行文件”

(二). 编写launch文件

(1). 在“～/home/用户名/工作空间名/src/软件包/launch”目录下创建outline1.launch文件，并复制以下内容：

<launch>

    <node pkg="rosbag" type="play" name="player" output="screen" args="--clock /home/wrk/001.bag"/>

    <node pkg="data2jpgcsv" type="data2bag.py" name="data2bag"/>

</launch>

注：

将bag文件路径换成自己的路径
将pkg换成自己的包名，将type换成自己的文件名，将name换成自己的节点名

(三). 启动launch文件

请确保文件结构如下图所示一致：

catkin_workspace
├── build
├── devel
└── src
├── CMakeLists.txt -> /opt/ros/melodic/share/catkin/cmake/toplevel.cmake
          └── data2jpgcsv
                        ├── CMakeLists.txt
                        ├── launch
│          └── outline.launch

│          └── outline1.launch
├── package.xml
├── scripts
│          └── data2jpgcsv.py

│          └── data2bag.py
└── src