Matlab写入点云数据到Rosbag

近来有需要读取一个点云并做处理后，重新写回rosbag。网上有很多读取的教程，但没有写入。自己写入时也遇到了很多麻烦，踩了一堆坑进行记载。
1. rosbag中一个lidar的msg有哪些信息？

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

1. bag = rosbag('E:\04_Data\Share\test_data\double-water.bag');
2. lidarTopic = '/velodyne_points';
3. lidarMsgs = readMessages(select(bag, 'Topic', lidarTopic));
4. N_lidar = size(lidarMsgs, 1);
6. % 输出topic看一下：
7. 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个随机数：

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

复制代码

2.1 msg各个字段创建

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

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

复制代码

搞清晰上面数据的分析，具体的创建就简单多了。
需要注意的是，需要对应正确相应字段的数据格式。
2.2 重点关注Data字段

重点关注Data字段怎么创建。
Data是把全部点按序次拉成一列，然后每个点有Fields内里的字段。以是起首需要把全部点先构成一个Data，再拉成列。需要注意：

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

代码如下：

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

复制代码

3. 其他

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

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

免责声明：如果侵犯了您的权益，请联系站长，我们会及时删除侵权内容，谢谢合作！更多信息从访问主页：qidao123.com:ToB企服之家，中国第一个企服评测及商务社交产业平台。