[学习点云]ROS中解析bag包中的点云文件到pcd格式

       在ROS中使用bag包来记录运行过程中各个topic中的message，对于这些message，我们可以使用

rosbag play *.bag

的方式进行播放，但如果要截取bag包中的某一些数据，并逐一对其进行处理，那么我们就需要的bag进行解析。

       ros中的点云文件为PointCloud2，我们将一个用velodyne64线雷达采集的bag包中的点云文件转换为我们能够使用pcl_viewer直接显示的pcd点云文件。

       我们先对我们的bag内有什么样的数据进行显示。输入以下命令：

rosbag info *.bag

就可以显示bag包中记录的话题及其记录的消息的类型。截图如下：

   

  上面的信息显示，在这个bag包中我们记录很多topic的数据以及这些topic的消息类型，以及对应的有多少帧消息。在上图中，除了我们需要的PointCloud2消息类型外还有其他不同的消息，例如GPS的数据，这些数据也是十分重要的。对于这个包，目前我们只解析其中的点云文件，这是因为ros中自带将bag包中点云文件解析的工具。

      输入以下命令：rosrun pcl_ros bag_to_pcd 3.bag /velodyne_points pcd

     上述命令中，三个参数分别为bag包名，需要解析的节点名称和解析后存储的位置。解析完成后，我们可以在pcd文件夹中看到我们所需的每一帧点云文件。

使用pcl_viewer，

pcl_viewer https://blog.csdn.net/KYJL888/article/details/1559121930.579004000.pcd

我们可以打开查看pcd的数据，我能打开第一个pcd文件，显示如下：

 

1、rosbag record cloud      

记录点云数据为bag包格式

2、rosrun pcl_ros bag_to_pcd 2017-11-02-10-40-32.bag cloud dd  

 将bag包转换为PCD格式，并放到dd目录下（保存格式以帧为单位）

3、 pcl_viewer ~/dd/1509590432.823038000.pcd     

 pcl显示每帧数据

4、PCD文件格式
 

PCD文件的文件头部分必须以上面的顺序精确指定，也就是如下顺序：

VERSION ： PCD_Vx来编号（例如，PCD_V5、PCD_V6、PCD_V7等等）

FIELDS ：·FIELDS –指定一个点可以有的每一个维度和字段的名字。

例如：

FIELDS x y z                                   # XYZ data

FIELDS x y z rgb                          # XYZ + colors

FIELDS x y z normal_xnormal_y normal_z         # XYZ + surface normals

FIELDS j1 j2 j3                                # moment invariants

...

SIZE  ：SIZE –用字节数指定每一个维度的大小。例如：

unsigned char/char has 1 byte

unsigned short/short has 2 bytes

unsignedint/int/float has 4 bytes

double has 8 bytes

   TYPE： 用一个字符指定每一个维度的类型。现在被接受的类型有：

I –表示有符号类型int8（char）、int16（short）和int32（int）；

U – 表示无符号类型uint8（unsigned char）、uint16（unsigned short）和uint32（unsigned int）；

F –表示浮点类型。

COUNT：指定每一个维度包含的元素数目。例如，x这个数据通常有一个元素，但是像VFH这样的特征描述子就有308个。实际上这是在给每一点引入n维直方图描述符的方法，把它们当做单个的连续存储块。默认情况下，如果没有COUNT，所有维度的数目被设置成1。

WIDTH：WIDTH 640       # 每行有640个点

HEIGHT：

有序点云例子：

WIDTH 640       # 像图像一样的有序结构，有640行和480列，

HEIGHT 480      # 这样该数据集中共有640*480=307200个点

无序点云例子：

WIDTH 307200

HEIGHT 1        # 有307200个点的无序点云数据集

VIEWPOINT：定数据集中点云的获取视点VIEWPOINT 0 0 0 1 0 0 0

POINTS：POINTS–指定点云中点的总数。从0.7版本开始，该字段就有点多余了，因此有可能在将来的版本中将它移除。

例子：

POINTS 307200   #点云中点的总数为307200

DATA：指定存储点云数据的数据类型。从0.7版本开始，支持两种数据类型：ascii和二进制。

注意：文件头最后一行（DATA）的下一个字节就被看成是点云的数据部分了，它会被解释为点云数据。

PCD文件的文件头部分必须以上面的顺序精确指定，之间用换行隔开。

 

 

1、.bag数据信息的查看

在rosbag数据包所在文件夹下新建终端输入如下代码即可查看数据包详细信息。

          rosbag info *.bag   //*为你自己的数据包的名称

如下图，注意topics这一信息，包含有你的数据的节点信息如图片为/image_raw，点云为/points_raw。两者数据量分别为5507帧和1830帧。

通过ros自带命令也可实现这一功能，但通过如下代码能一次性没有遗漏的解析出全部带有时间戳的图片。

将下面的.py文件与.bag文件置于同一目录下运行，即可在pic文件夹下得到时间戳的全部5507张图片。

#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='/home/song/Autoware/ros/pic/' #存放图片的位置
class ImageCreator():

    def __init__(self):
        self.bridge = CvBridge()
        with rosbag.Bag('test.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 = "%.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

  

在rosbag数据包所在文件夹下新建pcd文件夹，新建终端输入如下命令可实现pcd文件的解析。

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

示例如下：

rosrun pcl_ros bag_to_pcd test.bag /points_raw https://blog.csdn.net/KYJL888/article/details/pcd

/points_raw为查看.bag文件后，你的点云数据的节点名称。

解析结果如下