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

LiDAR数据解析

Ubuntu18.04 系统,已安装ROS

1. 查看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,并且每一个对应的帧数是不一样的,在数据录录制过程中存在这丢帧的现象,希望能够对解析后输出的图片带有时间戳方便对齐

2. 提取点云数据

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.py

import 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:
                        print
                        e
                    timestr = "%.6f" % msg.header.stamp.to_sec()
                    # %.6f表示小数点后带有6位，可根据精确度需要修改；
                    image_name = timestr + ".jpg"  # 图像命名：时间戳.jpg
                    cv2.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.py

import 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=0
    for i in flist:
        oldname=filepath+flist[n]
        #取时间戳的前0-11位,重新命名
        newname=filepath+flist[n][:12]+appendix
        os.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+=1

    for n,name in enumerate(Camera_list):
        Camera_list[n]=name.rstrip('.jpg')
        n+=1
    n=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+=1
    return Both_list

def copy_file(old_path,new_path,appendix,item_list):
    for name in item_list:
        name=name+appendix
        from_path=os.path.join(old_path,name)
        to_path=new_path
        copy(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)