devbox
2023面试高手
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

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

作者：2023面试高手 | 2024-02-29 21:15:04

赞

踩

rosbag中雷达和相机数据时间同步

一. 引言

图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 ErrorRotation ErrorReprojection Error
时间戳校准15对0.03063.24765.1702
时间戳校准4对0.04194.07097.2469
时间戳未校准15对0.178813.533034.8668
时间戳未校准4对0.292028.796152.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>

 如：

  1. roscore
  2. 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文件不带时间戳），所以我就在这里给它注释修改了一下：

  1. #! /usr/bin/python
  2. # coding=utf-8
  3.  
  4. import os
  5. import rosbag
  6. import cv2
  7. from cv_bridge import CvBridge
  8. from tqdm import tqdm
  9.  
  10. class ExtractBagData(object):
  11.  
  12.     def __init__(self, bagfile_path, camera_topic, pointcloud_topic, root):
  13.         self.bagfile_path = bagfile_path
  14.         self.camera_topic = camera_topic
  15.         self.pointcloud_topic = pointcloud_topic
  16.         self.root = root
  17.         self.image_dir = os.path.join(root, "images")
  18.         self.pointcloud_dir = os.path.join(root, "pointcloud")
  19.  
  20.         #创建提取图片和点云的目录 ./root/images  root/pointcloud
  21.         if not os.path.exists(self.image_dir):
  22.             os.makedirs(self.image_dir)
  23.         if not os.path.exists(self.pointcloud_dir):
  24.             os.makedirs(self.pointcloud_dir)
  25.  
  26.     def extract_camera_topic(self):
  27.         bag = rosbag.Bag(self.bagfile_path, "r")
  28.         bridge = CvBridge()
  29.         bag_data_imgs = bag.read_messages(self.camera_topic)
  30.  
  31.         index = 0
  32.  
  33.         pbar = tqdm(bag_data_imgs)
  34.         for topic, msg, t in pbar: # type: ignore
  35.             pbar.set_description("Processing extract image id: %s" % (index+1))
  36.             cv_image = bridge.imgmsg_to_cv2(msg, "bgr8")
  37.             # 如果你需要使用时间戳对提取的图片命名，可以使用msg.header.stamp.to_sec()获取时间戳
  38.             timestr = "%.6f" %  msg.header.stamp.to_sec()
  39.             cv2.imwrite(os.path.join(self.image_dir, str(timestr) + ".jpg"), cv_image) # type: ignore
  40.             index += 1
  41.  
  42.     def extract_pointcloud_topic(self):
  43.         '''
  44.         # 提取点云数据为pcd后缀文件,默认提取以为时间戳命名
  45.         # 提取命令:rosrun pcl_ros bag_to_pcd result.bag /velodyne_points ./pointcloud
  46.         # 提取点云以时间戳命令:1616554905.476288682.pcd
  47.         :return:
  48.         '''
  49.         cmd = "rosrun pcl_ros bag_to_pcd %s /velodyne_points %s" % (self.bagfile_path, self.pointcloud_dir)
  50.         os.system(cmd)
  51.  
  52.         # 再读取提取的pcd点云数据，把文件名修改为按照顺序索引名
  53.         #pcd_files_list = os.listdir(self.pointcloud_dir)
  54.         # 因为提取的pcd是以时间戳命令的，但是存放到列表中并不是按照时间戳从小到大排列，这里对时间戳进行重新排序
  55.         #pcd_files_list_sorted = sorted(pcd_files_list)
  56.         # print(zip(pcd_files_list, pcd_files_list_sorted))
  57.  
  58.         #index = 0
  59.         #pbar = tqdm(pcd_files_list_sorted)
  60.         #for pcd_file in pbar:
  61.         #    pbar.set_description("Processing extract poindcloud id: %s" % (index + 1))
  62.         #    os.rename(os.path.join(self.pointcloud_dir, pcd_file),
  63.         #              os.path.join(self.pointcloud_dir, str(index) + ".pcd"))
  64.         #    print("pcd_file name: ", pcd_file)
  65.         #    index += 1
  66.  
  67. if __name__ == '__main__':
  68.     bagfile_path = '/home/wrk/001.bag'
  69.     camera_topic = "/usb_cam/image_raw"
  70.     pointcloud_topic = "/velodyne_points"
  71.     extract_bag = ExtractBagData(bagfile_path, camera_topic, pointcloud_topic,  "/home/wrk/data_imgpcd")
  72.     extract_bag.extract_camera_topic()
  73.     extract_bag.extract_pointcloud_topic()

具体使用方法：

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

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

3. 重命名

  1. #coding=utf-8
  2. import os
  3.  
  4. def rename(filepath):
  5.     """
  6.     filePath:文件目录
  7.     return:None
  8.     """
  9.     filenamelist = os.listdir(filepath)
  10.     filenamelist = sorted(filenamelist, key=lambda x: eval(x[0:-4]))
  11.     #将filepath目录下的文件读取为列表,并排序
  12.     
  13.     newnamelist = ["{:0>4}".format(i) for i in range(len(filenamelist))]
  14.     #重命名为占4个字符宽度,非零数字右对齐,其余空位用0补齐. 如"0001"、"0002"、"0099"
  15.     for i in range(len(filenamelist)):
  16.         old_path = os.path.join(filepath,filenamelist[i])
  17.         #旧文件路径
  18.         new_path = os.path.join(filepath,newnamelist[i]+filenamelist[i][-4::])
  19.         #新文件路径(保留文件后缀)
  20.         os.rename(old_path,new_path)
  21.         #重命名操作
  22.     """"""
  23.     print("Finish")
  24.     return None
  25.  
  26. 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>

如：

  1. cd ~/catkin_workspace/src
  2. catkin_create_pkg data2jpgcsv rospy sensor_msgs cv_bridge
(三). 编译

cd ~/工作空间名/src

catkin_make

 如：

  1. cd ~/catkin_workspace/
  2. catkin_make
(四). 编写py文件

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

或：

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

mkdir scripts

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

  1. #! /usr/bin/python
  2. # coding = utf-8
  3.  
  4. import cv2
  5. import rospy
  6. import numpy as np
  7. import pandas as pd
  8. from sensor_msgs.msg import Image
  9. from sensor_msgs import point_cloud2
  10. from sensor_msgs.msg import PointCloud2
  11. from cv_bridge import CvBridge
  12.  
  13.  
  14. def LidarCallBack(msg):
  15.     if isinstance(msg, PointCloud2):
  16.         gen = point_cloud2.read_points(msg)
  17.         arr = []
  18.         for p in gen:
  19.             arr.append(p)
  20.         arr = np.array(arr)
  21.         pd.DataFrame(arr).to_csv("/home/wrk/data_imgcsv/{}.csv".format(msg.header.stamp))
  22.         rospy.logwarn("Success Write in:{}.csv".format(msg.header.stamp))
  23.     elif isinstance(msg, Image):
  24.         cv_img = bridge.imgmsg_to_cv2(msg, "bgr8")
  25.         cv2.imwrite("/home/wrk/data_imgcsv/{}.jpg".format(msg.header.stamp) , cv_img) # type: ignore
  26.         rospy.loginfo("Success Write in:{}.jpg".format(msg.header.stamp))
  27.     else:
  28.         pass
  29.  
  30.     return None
  31.  
  32. rospy.init_node("lidar_node")
  33. bridge = CvBridge()
  34. camer_sub = rospy.Subscriber("/usb_cam/image_raw",Image,LidarCallBack,queue_size=10)
  35. lidar_sub = rospy.Subscriber("/velodyne_points",PointCloud2,LidarCallBack,queue_size=10)
  36. rospy.spin()

注：

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

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

(五). 编写launch文件

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

或：

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

mkdir launch

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

  1. <launch>
  2.  
  3.     <node pkg="rosbag" type="play" name="player" output="screen" args="--clock /home/wrk/001.bag"/>
  4.  
  5.     <node pkg="data2jpgcsv" type="data2jpgcsv.py" name="data2jpgcsv"/>
  6.  
  7. </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文件，并复制以下内容：

  1. #! /usr/bin/python
  2. # coding = utf-8
  3.  
  4. import rospy
  5. import rosbag
  6. from sensor_msgs.msg import Image
  7. from sensor_msgs.msg import PointCloud2
  8.  
  9. bag = rosbag.Bag('/home/wrk/dataset.bag','w')
  10.  
  11. k = 0
  12.  
  13. def MsgsCallBack(msg):
  14.     global k
  15.     if isinstance(msg, PointCloud2):
  16.         bag.write('PointCloud2',msg)
  17.         k = 0
  18.     elif isinstance(msg, Image):
  19.         k = k +1
  20.         if k == 1:
  21.             bag.write('Image',msg)
  22.         else:
  23.             pass
  24.     else:
  25.         pass
  26.  
  27. rospy.init_node('drone_track_data_saver')
  28. rospy.Subscriber("/usb_cam/image_raw",Image,MsgsCallBack,queue_size=10)
  29. rospy.Subscriber("/velodyne_points",PointCloud2,MsgsCallBack,queue_size=10)
  30.  
  31. rate = rospy.Rate(1) # 10hz
  32. while not rospy.is_shutdown():
  33.     rate.sleep()
  34. else:
  35.     bag.close()

注：

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

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

 (二). 编写launch文件

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

  1. <launch>
  2.  
  3.     <node pkg="rosbag" type="play" name="player" output="screen" args="--clock /home/wrk/001.bag"/>
  4.  
  5.     <node pkg="data2jpgcsv" type="data2bag.py" name="data2bag"/>
  6.  
  7. </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文件，将消息发布在电脑中，然后启动节点读取数据；在线读取是启动传感器节点之后，消息同样也被发布在电脑中，然后启动节点读取转化数据。

最终效果如图所示：

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/2023面试高手/article/detail/168457?site
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号