kitti分割数据格式转换脚本和bag包解析

一、点云分割结果转为KITTI保存的格式：

def pcd_to_label(pcd_src_path, dst):
    txt_data = np.load(pcd_src_path)
    data_bytes = txt_data['data']
    if '-en' in pcd_src_path:
        data_bytes = my_aes.decrypt_str(data_bytes)
    seg_data = np.frombuffer(data_bytes, dtype=np.float64).reshape(-1, 5)
    res = seg_data[:, 3].copy()
    np.putmask(res, res == -1, 2)
    res = res.astype(np.uint32)
    if len(seg_data) == len(res):
        res.tofile(dst)
    else:
        print('数据错误2：', dst)

读取脚本：

label = np.fromfile(label_path, dtype=np.int32).reshape((-1))

二、.bag包解析步骤

1、rosbag info 查看点云数据和图片的topic

2、roscore启动

3、新建终端提取点云数据

rosrun pcl_ros bag_to_pcd <*.bag> <topic> <output_directory>

4、提取图片数据

# coding:utf-8

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 = '/media/cuicuizhang/hold/2020.8.14bag/pics/01/img/'  # 存放图片的位置


class ImageCreator():

    def __init__(self):
        self.bridge = CvBridge()
        with rosbag.Bag('/media/cuicuizhang/hold/2020.8.14bag/01.bag', 'r') as bag:  # 要读取的bag文件；
            for topic, msg, t in bag.read_messages():
                if topic == "/image_raw":  # 图像的topic；
                    try:
                        cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
                    except CvBridgeError as e:
                        print e
                    timestr = "%.9f" % msg.header.stamp.to_sec()
                    # %.6f表示小数点后带有6位，可根据精确度需要修改；
                    image_name = timestr + ".jpg"  # 图像命名：时间戳.jpg
                    print (path + image_name)
                    cv2.imwrite(path + image_name, cv_image)  # 保存；


if __name__ == '__main__':

    # rospy.init_node(PKG)

    try:
        image_creator = ImageCreator()
    except rospy.ROSInterruptException:
        pass

5、抽取图片数据中时间戳最接近点云的数据

import numpy as np

img_path_list = "/media/cuicuizhang/hold/2020.8.14bag/pics/06/jpg.txt"
pcd_path_list = "/media/cuicuizhang/hold/2020.8.14bag/pics/06/pcd.txt"
img_to_pcd_list = "/media/cuicuizhang/hold/2020.8.14bag/pics/06/jp.sh"

img_list = []
pcd_list = []
with open(img_path_list) as fimg:
    lines = fimg.readlines()
    for line in lines:
        img_list.append(line.strip().split('.jpg')[0])

with open(pcd_path_list) as fpcd:
    lines = fpcd.readlines()
    for line in lines:
        pcd_list.append(line.strip().split('.pcd')[0])


with open(img_to_pcd_list,'w') as fw:

    for pcd in pcd_list:
        print("pcd:", pcd)
        idx = np.searchsorted(img_list, pcd) - 1
        mask = idx >= 0
        if idx + 1 < len(img_list) and abs(float(img_list[idx])-float(pcd))>abs(float(img_list[idx+1])-float(pcd)):
            mask = idx+1
        else:
            mask = idx
        print("img:",img_list[mask])
        fw.write("cp img/")
        fw.write(img_list[mask])
        fw.write('.jpg ')
        fw.write('img_correct/')
        fw.write(pcd)
        fw.write(".jpg")
        fw.write('\n')

三、运行生成的jp.sh脚本

四、pcd文件转bin

import os
import pcl
import numpy as np

def pcl_read(file_path):
    p = pcl.load(file_path)
    points = np.asarray(p)
    return points

def pcd_to_bin(pcd_src_path, bin_dst_path):
    ''' pcd转bin'''
    p = pcl_read(pcd_src_path)
    points = np.asarray(p)
    points.tofile(bin_dst_path)

附录：

批处理命令

for i in 47-16 52-46 53-06 -55-36 -57-29 05-25 ; do rosrun pcl_ros bag_to_pcd $i.bag /points_raw $i/;done

for i in47-16 52-46 53-06 -55-36 -57-29 05-25 ; do mkdir $i'_img'  ; done

# coding:utf-8
import os

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 = '/media/cuicuizhang/hold/bag/2020.8.14bag/car/'  # 存放图片的位置

class ImageCreator():

    def __init__(self):
        self.bridge = CvBridge()
        for file in os.listdir(path):
            if file.endswith('.bag'):
                img_path = file.strip().split('.bag')[0] + '_img'
                with rosbag.Bag(path+file, 'r') as bag:  # 要读取的bag文件；
                    for topic, msg, t in bag.read_messages():
                        if topic == "/image_raw":  # 图像的topic；
                            try:
                                cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
                            except CvBridgeError as e:
                                print e
                            timestr = "%.9f" % msg.header.stamp.to_sec()
                            # %.6f表示小数点后带有6位，可根据精确度需要修改；
                            image_name = timestr + ".jpg"  # 图像命名：时间戳.jpg

                            print (os.path.join(path, img_path, image_name))
                            cv2.imwrite(os.path.join(path, img_path, image_name), cv_image)  # 保存；


if __name__ == '__main__':

    # rospy.init_node(PKG)

    try:
        image_creator = ImageCreator()
    except rospy.ROSInterruptException:
        pass

import os

import numpy as np


def single(img_path_list, pcd_path_list,img_to_pcd_list, file):
    img_list = []
    pcd_list = []
    with open(img_path_list) as fimg:
        lines = fimg.readlines()
        for line in lines:
            img_list.append(line.strip().split('.jpg')[0])

    with open(pcd_path_list) as fpcd:
        lines = fpcd.readlines()
        for line in lines:
            pcd_list.append(line.strip().split('.pcd')[0])
    save_path = img_path_list.strip().split('_img/jpg.txt')[0]+"_correct"
    if not os.path.exists(save_path):
        os.mkdir(save_path)
    with open(img_to_pcd_list,'w') as fw:

        for pcd in pcd_list:
            print("pcd:", pcd)
            idx = np.searchsorted(img_list, pcd) - 1
            mask = idx >= 0
            if idx + 1 < len(img_list) and abs(float(img_list[idx])-float(pcd))>abs(float(img_list[idx+1])-float(pcd)):
                mask = idx+1
            else:
                mask = idx
            print("img:",img_list[mask])
            fw.write("cp "+file+"/")
            fw.write(img_list[mask])
            fw.write('.jpg ')
            fw.write(save_path+"/")
            fw.write(pcd)
            fw.write(".jpg")
            fw.write('\n')

if __name__ == '__main__':
    # img_path_list = "/media/cuicuizhang/hold/2020.8.14bag/pics/06/jpg.txt"
    # pcd_path_list = "/media/cuicuizhang/hold/2020.8.14bag/pics/06/pcd.txt"
    # img_to_pcd_list = "/media/cuicuizhang/hold/2020.8.14bag/pics/06/jp.sh"
    path = "/media/cuicuizhang/hold/bag/2020.8.14bag/people"
    for file in os.listdir(path):
        if "img" in file:
            with open(os.path.join(path,file,"jpg.txt"),'w') as fw:
                for img_name in os.listdir(os.path.join(path,file)):
                    if img_name.endswith('.jpg'):
                        fw.write(img_name)
                        fw.write('\n')
            pcd_file = file.strip().split('_img')[0]+"_pcd"
            with open(os.path.join(path,file,"pcd.txt"),'w') as fw:
                for pcd_name in os.listdir(os.path.join(path,pcd_file)):
                    if pcd_name.endswith('.pcd'):
                        fw.write(pcd_name+'\n')
            img_path_list = os.path.join(path,file,"jpg.txt")
            pcd_path_list = os.path.join(path,file,"pcd.txt")
            img_to_pcd_list = os.path.join(path, file, "jp.sh")
            single(img_path_list, pcd_path_list, img_to_pcd_list, file)

    path = r"H:\bag\2020.8.14bag\people"
    for pcd_src_path in os.listdir(path):
        if "pcd" in pcd_src_path:
            bin_dst_path = pcd_src_path.strip().split("_pcd")[0]+"_bin"
            if not os.path.exists(os.path.join(path, bin_dst_path)):
                os.mkdir(os.path.join(path, bin_dst_path))
            for file in os.listdir(os.path.join(path, pcd_src_path)):
                print(file)
                pcd_src = os.path.join(os.path.join(path, pcd_src_path), file)
                bin_dst = os.path.join(os.path.join(path, bin_dst_path), file.split('.pcd')[0]+'.bin')
                pcd_to_bin(pcd_src, bin_dst)