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

一. 引言

图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可以看出，时间同步对于多传感器联合标定来说，具有十分重要的意义。

二. 介绍

 相机雷达时间同步（基于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()

具体使用方法：

1. 在VScode（可以使用其他软件）中复制并粘贴以上内容；
2. 将“bagfile_path”换成自己的包文件地址、将“camera_topic”换成自己的相机话题名、将“pointcloud_topic”换成自己的激光雷达话题名、将“extract_bag”换成自己的保存路径；
3. 右键选择“运行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")

具体使用方法：

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

最终效果如下图所示：

4. 总结

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

四. 方法二

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

1. 离线读取bag数据

主要步骤为：

1. 创建工作空间（有工作空间就忽略）
2. 创建软件包
3. 编译
4. 编写py文件
5. 编写launch文件
6. 启动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()

注：

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

 (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>

 注：

1. 将bag文件路径换成自己的路径
2. 将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()

注：

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

 (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>

 注：

1. 将bag文件路径换成自己的路径
2. 将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

启动launch文件的命令格式：

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

 如：

roslaunch data2jpgcsv outline1.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 data2bag.py

3. 总结

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

最终效果如图所示：

文章来源地址https://www.toymoban.com/news/detail-770521.html

到了这里，关于激光雷达和相机数据时间同步的几种方法的文章就介绍完了。如果您还想了解更多内容，请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章，希望大家以后多多支持TOY模板网！