ROS環境を使わないPointCloud2型トピックのPCDファイルへの変換

の続き

やりたいこと

rosbagsを用いてトピックを読み出すことには成功した
IMUなどのデータはそのままCSVにでもすればよいが，LiDAR点群はそうもいかないのでなんとかしたい．
具体的にはPointCloud2型のトピックになっている点群データをPCD形式のファイルで保存できるようにする．

PointCloud2型の解釈

rosbagsで読んできたPointCloud2型のトピックがこんな感じ

> vars(msg)
{'header': std_msgs__msg__Header(stamp=builtin_interfaces__msg__Time(sec=1713513002, nanosec=460340972, __msgtype__='builtin_interfaces/msg/Time'), frame_id='lidar', __msgtype__='std_msgs/msg/Header'),
 'height': 1,
 'width': 346779,
 'fields': [sensor_msgs__msg__PointField(name='x', offset=0, datatype=7, count=1, INT8=1, UINT8=2, INT16=3, UINT16=4, INT32=5, UINT32=6, FLOAT32=7, FLOAT64=8, __msgtype__='sensor_msgs/msg/PointField'),
  sensor_msgs__msg__PointField(name='y', offset=4, datatype=7, count=1, INT8=1, UINT8=2, INT16=3, UINT16=4, INT32=5, UINT32=6, FLOAT32=7, FLOAT64=8, __msgtype__='sensor_msgs/msg/PointField'),
  sensor_msgs__msg__PointField(name='z', offset=8, datatype=7, count=1, INT8=1, UINT8=2, INT16=3, UINT16=4, INT32=5, UINT32=6, FLOAT32=7, FLOAT64=8, __msgtype__='sensor_msgs/msg/PointField'),
  sensor_msgs__msg__PointField(name='intensity', offset=16, datatype=7, count=1, INT8=1, UINT8=2, INT16=3, UINT16=4, INT32=5, UINT32=6, FLOAT32=7, FLOAT64=8, __msgtype__='sensor_msgs/msg/PointField')],
 'is_bigendian': False,
 'point_step': 32,
 'row_step': 11096928,
 'data': array([245,   8, 132, ...,   1, 116, 255], dtype=uint8),
 'is_dense': True,
 '__msgtype__': 'sensor_msgs/msg/PointCloud2'}

msg.dataにデータが入っているが，uint8型のクソ長1次元配列になっていて読み方がよくわからない．

読み方が書いてあるのがsensor_msgs__msg__PointFieldのあたりで，この場合

• point_stepが32なので，uint8の配列の32要素でひとかたまり（1つの点）である．
• datatype=7なので型はFLOAT32（uint8の配列4要素で1つの値）．
• [0]～[3]の値がx座標，[4]～[7]の値がy座標，[8]～[11]の値がz座標，[16]～[19]の値が信号強度を表している

ということがわかる．
（参考文献　https://ppdr.softether.net/ros-pointcloud2-data ）

実際適当にreshapeしてやるとそれっぽい感じデータが入っているように見える（[12:16]にも何か値が入っているが謎）

> msg.data[:64].reshape(-1,4)
array([[245,   8, 132,  61],
       [115, 181, 179,  64],
       [ 10,  92,  23,  63],
       [  0,   0, 128,  63],
       [  0,   0, 184,  65],
       [  0,   0,   0,   0],
       [  0,   0,   0,   0],
       [  0,   0,   0,   0],
       [223, 149, 174, 190],
       [112,  27, 219,  63],
       [136, 184, 222,  63],
       [  0,   0, 128,  63],
       [  0,   0, 240,  65],
       [  1,   0,   0,   0],
       [  0,   0,   0,   0],
       [  0,   0,   0,   0]], dtype=uint8)

あとはuint8の配列4要素（4バイト）分をfloat32として解釈してやれば意味のある値が得られる．
そのためにはtobytes()とnp.frombuffer()を用いる．

具体的にはこう
np.frombuffer(msg.data.tobytes(),dtype=np.float32).reshape(-1,8)

> np.frombuffer(msg.data.tobytes(),dtype=np.float32).reshape(-1,8)[:3]
array([[ 6.4470209e-02,  5.6158996e+00,  5.9124815e-01,  1.0000000e+00,
         2.3000000e+01,  0.0000000e+00,  0.0000000e+00,  0.0000000e+00],
       [-3.4098718e-01,  1.7117748e+00,  1.7400064e+00,  1.0000000e+00,
         3.0000000e+01,  1.4012985e-45,  0.0000000e+00,  0.0000000e+00],
       [ 1.3993801e-01,  6.2663093e+00,  6.5944964e-01,  1.0000000e+00,
         1.3000000e+01,  2.8025969e-45,  0.0000000e+00,  0.0000000e+00]],
      dtype=float32)

ここで例えば_[:,0]とかすればx座標列が得られる．

PCDファイルの書き方

実態はただのテキストファイルなので，お作法に従ってテキスト出力すればよい．
今回はとりあえず信号強度は気にせずにx,y,z座標だけ書き込むことにする．
書式は以下のような感じで，

VERSION 0.7
FIELDS x y z
SIZE 4 4 4
TYPE F F F
COUNT 1 1 1
WIDTH 346779
HEIGHT 1
POINTS 346779
DATA ascii
0.06447021 5.6158996 0.59124815
-0.34098718 1.7117748 1.7400064
(以下略)

ヘッダーに必要事項を書いた後，スペース区切りでx,y,zの値を1行ずつひたすら書いていけばよい．
ヘッダーのうちWIDTHとPOINTSのところには点の数を記入する．msg.widthの値を読んで入れてもいいし，respaheした配列の長さを調べてもいい．

結論

以下のスクリプトでタイムスタンプをファイル名としたPCDファイルが（大量に）出力される．
ファイルサイズが馬鹿にならないので，実用上はまとめて圧縮するなり，必要なファイルだけ逐一rosbagから変換するなり考える必要がある．

src=Path('ファイルパス')

with Reader(src) as reader:
    connections = [x for x in reader.connections if x.topic == トピック名]
   
    for connection, timestamp, rawdata in reader.messages(connections=connections):
        msg = deserialize_cdr(rawdata, connection.msgtype)
        
        p=np.frombuffer(msg.data.tobytes(),dtype=np.float32).reshape(-1,8)
        with open("str(timestamp)+".pcd","wt") as f:
            f.write("VERSION 0.7\nFIELDS x y z\nSIZE 4 4 4\nTYPE F F F\nCOUNT 1 1 1\nWIDTH "+str(len(p))+"\nHEIGHT 1\nPOINTS "+str(len(p))+"\nDATA ascii\n")
            for i in range(len(p)):
                f.write(" ".join(np.array(p[i,:3],dtype=str))+"\n")