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自身预留的字段。

后记

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