激光雷达LiDAR和相机的.bag数据解析与对齐

本文主要是介绍激光雷达LiDAR和相机的.bag数据解析与对齐，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

LiDAR数据解析

Ubuntu18.04 系统,已安装ROS

查看bag信息,在相应文件夹下打开终端

rosbag info xxx.bag

获得对应的topics

topics:      /velodyne_points                          940 msgs    : sensor_msgs/PointCloud2/zed2i/zed_node/left/image_rect_color     822 msgs    : sensor_msgs/Image      /zed2i/zed_node/right/image_rect_color   1111 msgs    : sensor_msgs/Image

可以看到我这有三个topic,并且每一个对应的帧数是不一样的,在数据录录制过程中存在这丢帧的现象,希望能够对解析后输出的图片带有时间戳方便对齐

提取点云数据

rosrun pcl_ros bag_to_pcd <your_bag_name> <choose_topic> <output_directory>
// for example
rosrun pcl_ros bag_to_pcd t1.bag /velodyne_points /home/data/output_t1

相机数据解析

可以直接用下面的python代码进行解析,解析后的每张图片以时间戳命名

# coding:utf-8
# get_image.pyimport roslib;
import rosbag
import rospy
import cv2
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
from cv_bridge import CvBridgeError# 存放解析输出图片的位置
path = '/home/summer/MarineData/t6/picture/'  class ImageCreator():def __init__(self):self.bridge = CvBridge()# 要读取的bag文件；with rosbag.Bag('t6.bag', 'r') as bag:  for topic, msg, t in bag.read_messages():# 图像的topic；if topic == "/zed2i/zed_node/right/image_rect_color":  try:cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")except CvBridgeError as e:printetimestr = "%.6f" % msg.header.stamp.to_sec()# %.6f表示小数点后带有6位，可根据精确度需要修改；image_name = timestr + ".jpg"  # 图像命名：时间戳.jpgcv2.imwrite(path + image_name, cv_image)  # 保存；if __name__ == '__main__':# rospy.init_node(PKG)try:image_creator = ImageCreator()except rospy.ROSInterruptException:pass

将这个pyhton文件和bag文件放在同一个目录下面,在该目录下打开终端,运行:

python2 get_image.py

因为我电脑中安装了python2 和python3 ,而这个python文件需要用到的是ros中自带的python2, ros中自带的pyhton2有用来解析图片的相关库,就不需要另外安装.故在此指定python2.

数据对齐

# align_data.pyimport os
from shutil import copy#初步解析后点云和图片的存放地址
Lpath='/home/summer/MarineData/t6/pointcloud/'
Cpath='/home/summer/MarineData/t6/picture/'# 存在对齐后的点云图片
New_L_path ='/home/summer/MarineData/t6/alignment/point/'
New_C_path ='/home/summer/MarineData/t6/alignment/picture/'jpg_appendix='.jpg'
pcd_appendix='.pcd'def file2txt(file_path,txt_path):names = os.listdir(file_path)image_ids = open(txt_path,'w')for name in names:image_ids.write('%s\n'%(name))image_ids.close()def changename(filepath,appendix):flist=os.listdir(filepath)n=0for i in flist:oldname=filepath+flist[n]#取时间戳的前0-11位,重新命名newname=filepath+flist[n][:12]+appendixos.rename(oldname,newname)n+=1#print(oldname,'---->',newname)def find_timestap(Lidar_path,Camera_path):Lidar_list = os.listdir(Lidar_path)Camera_list = os.listdir(Camera_path)Both_list=list()for n,name in enumerate(Lidar_list):Lidar_list[n]=name.rstrip('.pcd')n+=1for n,name in enumerate(Camera_list):Camera_list[n]=name.rstrip('.jpg')n+=1n=0# 利用图片的索引去点云中找,一般以帧数少的作为索引for i in Camera_list:current_time = Camera_list[n]if current_time in Lidar_list:Both_list.append(current_time)Lidar_list.remove(current_time)n+=1return Both_listdef copy_file(old_path,new_path,appendix,item_list):for name in item_list:name=name+appendixfrom_path=os.path.join(old_path,name)to_path=new_pathcopy(from_path,to_path)#按照指定位数时间戳重新命名
changename(Lpath,pcd_appendix)
changename(Cpath,jpg_appendix)
#寻找时间戳对齐的点云和图片,返回所有文件名的list
Both_list=find_timestap(Lpath,Cpath)
#从原文件夹复制到对齐文件夹中
copy_file(Lpath,New_L_path,pcd_appendix,Both_list)
copy_file(Cpath,New_C_path,jpg_appendix,Both_list)