Matlab写入点云数据到Rosbag

最近有需要读取一个点云并做处理后，重新写回rosbag。网上有很多读取的教程，但没有写入。自己写入时也遇到了很多麻烦，踩了一堆坑进行记录。

1. rosbag中一个lidar的msg有哪些信息？

通过如下代码，先读取一个rosbag的lidar的msg：

bag = rosbag('E:\04_Data\Share\test_data\double-water.bag'); lidarTopic = '/velodyne_points'; lidarMsgs = readMessages(select(bag, 'Topic', lidarTopic)); N_lidar = size(lidarMsgs, 1); % 输出topic看一下： lidarMsgs{1}

可以看到，输出的结果如下： 其中： Header对应rosbag中的header Fields对应rosbag中每个点有多少个“字段”， Height和width中，height永远是1，width的数量和点云点数相同 IsBigendian大端模式，根据系统选择。我的系统是0。 PointStep，一个数据点的“step”，可以认为，一个数据点有多长。多长取决于有多少个Fields，以及每个Fields的数据类型 RowStep，数值等于 PointStep*Width，这个是计算出来的 Data，注意是84400*1的uint8类型。这个很重要。84400和RowStep一致，uint8是ros在传输数据时，将message中的所有数据都编码成uint8类型。 IsDense表示是否是稠密的？这个暂不清楚有什么用。

这里重点解释Data的长度、类型以及Fields是什么

我们进一步看Fields有哪些内容。这里有5个，我们先列出来第一个： 最上方的INT8~FLOAT64这几行，是说明，并不是实际的数据；这表示，在ROS中数据类型的编号（后面还会涉及在matlab中的数据类型，所以强调区分；例如，ROS中FLOAT32占4个bytes，对应matlab中应该用占4bytes的single类型转化）是什么。

1.2 Fileds字段详解

Fileds(1)的Name是’x’，表示“这个字段的含义是x”； Offset是0，表示“这个字段在一个（长度为PointStep中的）数据点中的“起始位置”是0，也就是说从第1位开始读某个长度的字节Bytes，就是’x’的数值。读多长呢？往下看 Datatype是7，表示这个字段的数据类型是7，7是什么？上面给出了 ‘FLOAT32: 7’，即是一个FLOAT32类型。 Count是1，表示只有一个数据（lidar字段中好像所有的count都是1）。

那么，这表示，“每一个数据的第一个字段是，从第1个byte开始，读取类型FLOAT32（实际上是4个bytes）长度的数据，读取1个，这个字段的名称是x”。

同样，y和z也是如此，如下（第二个字段是y）： 由于刚才讲到，Datatype=7表示一个FLOAT32，长度需要4个bytes，所以x的下一个字段y就要从Offset=4开始。

接下来看不同其他Fileds。这里我的LiDAR数据包括: x, y, z, intensity, ring信息，所以共有5个字段。需要注意的是，每个字段用什么类型，是和rosbag中读取和转化有关的，这又涉及到ros中PCL的转化，这里不做展开介绍，可以参考之前的博客：【学习记录】Ouster雷达运行fastlio提示 Failed to find match for field ‘ring‘ 的解决办法。我们这里只讲，数据是这个格式，在matlab里面是什么样子的。为什么用FLOAT32，或者uint16，这些是雷达驱动底层、代码接口定义的，这里不做探究。

在我的数据中，intensity也是FLOAT32类型，ring是uint16类型，所以具体的intensity如下，从第12个bytes开始读取7类型的数据： ring的格式如下，从16tytes开始读取Datatype=4的数据：

至此，我们搞清楚了rosbag读取一个message后，有哪些数据了。

1.3 Data字段详解

可以看到，msg中的Data字段是一个: 84400*1的uint8的数据。 84400=4220*20，其中4220是总的lidar点数，20是一个点所包含的上述5个Fields的长度。

具体的，我们看一下msg.Data的具体内容，如下：

可以看出，Data字段的前20个数据，是一个完整的lidar点。注释如下： 不信的话，我们可以将x转化回FLOAT32类型，看一下是不是真实的x坐标，如下图。确实如此。typecast将一个数据转成指定的类型，这里用single是因为，MATLAB中的single类型对应ros中的FLOAT32，字节都是4bytes。 y、z、intensity和ring的验证这里不表。

还注意到，在长度为20的一个数据中，ring后面有2个0，这是为什么？因为ros要求4字节对齐，必须是4的整数倍。目前到ring总共有18个bytes，因此需要再补2个凑到20。这两个0是不影响读取的，因为Field(5)字段已经给出了从16开始读取uint16长度数据，因此会忽略后面的。

2. 创建msg

创建msg需要把所有字段都创建，重点是Data部分怎么处理。我们首先假设点云数据是这样的，10个随机数：

% 假设数据 K = 10; xyz = rand(K,3); % 100个点的XYZ坐标 intensity = rand(K); % 随机强度值 ring = randi(16,K)-1; % 环号（0~15） % 数据格式转化为对应的类型。matlab->ros对应关系：single->float32, uint16->uint16 xyz = single(xyz); % 强制转换为FLOAT32 intensity = single(intensity); % 强制转换为FLOAT32 ring = uint16(ring); % 强制转换为UINT16 2.1 msg各个字段创建

对Header, Field等字段创建。这里我们先不管Data怎么搞。

% 创建PointCloud2消息对象 lidarMsg = rosmessage('sensor_msgs/PointCloud2'); % 设置消息头信息 lidarMsg.Header.Stamp = rostime('now'); lidarMsg.Header.FrameId = 'lidar_frame'; % 获取点云数量 numPoints = size(xyz, 1); % 设置点云基本信息 lidarMsg.Height = 1; lidarMsg.Width = numPoints; lidarMsg.IsDense = true; % 定义字段（x, y, z, intensity, ring） fields = {'x','y','z','intensity','ring'}; offsets = uint32([0, 4, 8, 12, 16]); % 各字段的字节偏移量 datatypes = [7, 7, 7, 7, 4]; % 7=FLOAT32, 4=UINT16 counts = [1, 1, 1, 1, 1]; % 添加字段到消息 lidarMsg.Fields = []; for i = 1:length(fields) field = rosmessage('sensor_msgs/PointField'); field.Name = fields{i}; field.Offset = offsets(i); field.Datatype = datatypes(i); field.Count = counts(i); lidarMsg.Fields = [lidarMsg.Fields; field]; end % 设置点云数据步长（每个点20字节） lidarMsg.PointStep = 20; lidarMsg.RowStep = lidarMsg.PointStep * lidarMsg.Width; lidarMsg.IsBigendian = false; % 小端字节序

搞清楚上面数据的分析，具体的创建就简单多了。 需要注意的是，需要对应准确相应字段的数据格式。

2.2 重点关注Data字段

重点关注Data字段怎么创建。 Data是把所有点按顺序拉成一列，然后每个点有Fields里面的字段。所以首先需要把所有点先组成一个Data，再拉成列。需要注意：

Data数据是，先一个点(x,y,z,intensity, ring…)，再下一个点，拼接的；而不是所有的x再所有的y这样拼接；拼接时，注意首先要把每个点的所有数据格式都转成uint8，而不是先拼一起再转uint8，因为拼接时会有自动转化导致格式错误；不足4字节的，补0

代码如下：

% 预分配内存并逐个点填充字节 dataBytes = zeros(numPoints, 20, 'uint8'); for i = 1:numPoints % 转换x, y, z, intensity为小端字节序 xyzIntensityBytes = typecast([xyz(i,:), intensity(i)], 'uint8'); % 转换ring为小端字节序 ringBytes = typecast(ring(i), 'uint8'); % 合并数据（16字节xyzIntensity + 2字节ring + 2字节填充） dataBytes(i,:) = [xyzIntensityBytes, ringBytes, 0, 0]; end % 将数据转换为列向量并赋值 lidarMsg.Data = reshape(dataBytes', [], 1); % 注意这里的转置，否则reshape时按列拼接不对。 3. 其他

Data字段并不要求每个Field必须是连续的。我某个雷达录制的数据，'z’和’intensity’的起始位置分别是8和16，显然12-16这4个Bytes是空的，但不影响。可能是LiDAR自身预留的字段。

后记

折腾了一个晚上加一个白天，才把这些问题搞清楚。 真是不容易啊。