ROS的pointcloud2 格式和PCD格式总结以及转换

pointcloud2

这个是ROS定义的消息类型， 它的结构如下：

Header header

# 2D structure of the point cloud. If the cloud is unordered, height is
# 1 and width is the length of the point cloud.
uint32 height
uint32 width

# Describes the channels and their layout in the binary data blob.
PointField[] fields

bool    is_bigendian # Is this data bigendian?
uint32  point_step   # Length of a point in bytes
uint32  row_step     # Length of a row in bytes
uint8[] data         # Actual point data, size is (row_step*height)

bool is_dense        # True if there are no invalid points

row_step：

因为row_step 对于有序点云来说是指的一行点云的个数， 但是对于无序点云就是整个点云的数量， 也就是整个数据就是一行，height也是1

point_step：

一个点的大小，比如xyz 都是float32 , intensity 是float

sensor_msgs/PointField[]：

这个是定义了这个点云数据有哪些数据：
本身的结构如下：

# This message holds the description of one point entry in the
# PointCloud2 message format.
uint8 INT8    = 1
uint8 UINT8   = 2
uint8 INT16   = 3
uint8 UINT16  = 4
uint8 INT32   = 5
uint8 UINT32  = 6
uint8 FLOAT32 = 7
uint8 FLOAT64 = 8

string name      # Name of field
uint32 offset    # Offset from start of point struct
uint8  datatype  # Datatype enumeration, see above
uint32 count     # How many elements in the field

它里面每一个元素定义了一种数据， 比如我的一个点有如下数据：

x
y
z
intensity
ring 
timestamp

那么这个数组PointField的长度就是6， 其中举例intensity
在PointField的位置是3
PointField[3].name 就是intensity，其他信息举例如下：

PointField[3].offset=12 //假设xyz都是float，那intensity就是从第12个字节开始的
PointField[3].datatype就是float
PointField[3].count 就是点云的个数

pcd

对于pcd, 有两种类型， 一种是明文形式， 一种是bin的形式，顾名思义明文形式是直接可以打开就能看的

# .PCD v0.7 - Point Cloud Data file format
VERSION 0.7
FIELDS x y z intensity
SIZE 4 4 4 4
TYPE F F F F
COUNT 1 1 1 1
WIDTH 320000
HEIGHT 1
VIEWPOINT 0 0 0 1 0 0 0
POINTS 320000
DATA ascii
1.0994911 1.7750537 -0.0069012814 97
0 0 0 0
0 0 0 0
0 0 0 0
1.0993335 1.7747993 -0.036019325 97
0 0 0 0
0 0 0 0
0 0 0 0
。。。。。。。

转换

举例将pointcloud2 保存为pcd的文件

    void dump_point_cloud(const sensor_msgs::msg::PointCloud2& cloud_msg) const {
        pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
        pcl::fromROSMsg(cloud_msg, *cloud);

        std::ostringstream filename_stream;
        filename_stream << "pcd_" << cloud_msg.header.stamp.sec << "_" << std::setw(9) << std::setfill('0') <<  cloud_msg.header.stamp.nanosec << ".pcd";
        std::string filename = filename_stream.str();

        RCLCPP_INFO(this->get_logger(), "aved timestamps:L  %u.%09u", cloud_msg.header.stamp.sec, cloud_msg.header.stamp.nanosec); 
        // std::string filename = "frame_" + std::to_string(cloud_msg.header.stamp.sec) + "_" + std::to_string(cloud_msg.header.stamp.nanosec) + ".pcd";
        if(!save_bin_pcd_)
        {
            if (pcl::io::savePCDFileASCII(save_path_ + filename, *cloud) == -1) {
                RCLCPP_ERROR(this->get_logger(), "Failed to save PCD");
                return;
            }            
        }
        else
        {
            if (pcl::io::savePCDFileBinary(save_path_ + "bin_"+filename, *cloud) == -1)
            {
                RCLCPP_ERROR(this->get_logger(), "Failed to save PCD");
            }
        }
    }