モチベーション
LiDARのViewer開いてcsvファイルをいちいち作るのが面倒なのでLiDARから取得した点群の情報を一気に解析したい！
前提知識
LiDARから送られるデータについて
LiDARからは取得したある範囲のデータがバイナリデータとして敷き詰められてそれが1パケットとしてUDPでPCに設定されたIPに送られてきます。
このパケットが1フレーム間で何個も送られてくることで全体の点群を構成しています。
基本的な解析方法
- LiDARのパケットデータの構造が書いてあるデータシートを見つけてくる
- データ通りに1パケットの情報を解析したコードを書く
- while分で回してあげる
具体的に考えてみる
実際にパケットファイルからPythonでデータを取得して、点群表示するところまでやってみます。
解析する対象
Livox HAP
座標がそのまま取り出せるため本題であるバイナリを読む処理のみに集中したコードが書けるため
パケット解析
livox texh社が公開しているlvx2ファイルのパケット形式が下のPDFファイルに示されています
https://terra-1-g.djicdn.com/65c028cd298f4669a7f0e40e50ba1131/LVX2%20Specifications.pdf
パケットの構成は
- public header block
- private header block
- device info block
- point data block
今回の目的は点群情報を取得することなので名前的にpoint data blockからデータをとってくる感じっぽいです
では実際にpoint cloud blockを見てみるとN+1個のフレームに分かれていて、このフレームは1つのFrame HeaderかM+1個のPackageに分かれています。そのパッケージの中にPointデータがP+1個入っています。このPointデータにx, y, z（2 Bytes）, reflectivity（1 bytes）, tag（1bytes）が入っています。（PDF内ではすべてNと表現されていますが、すべて違う値を示しています。）
図にしたものが下の図です（手書きで見ずらいですが）
よってこれらの文字の値L, Nを求める必要があります。これらの数もPDFファイルに書かれています。
L = 最初から29個目のバイナリ
N = NextOffset - Current Offset
M = 不明
P = 96（Livox HAP は 1frame にパッケージ存在とPDFの一番下に記載）
よって処理としては以下のようになります
- 各フレームのCurrent Offset, Next Offsetを取得
- 各フレームのFrame Headerを飛ばして1package分である96個分の点群情報をloop処理して取得
コードを書く
あとは上の処理をコードに書くだけです
import open3d as o3d
import numpy as np
def livoxFileReadHook(ReadFileName):
# Input: Read FileName
# Output: frameInfo[frameBegin, frameEnd]
t = 0
points = []
deg = []
lidar_pos = []
rf = open(ReadFileName, 'rb')
Idx = 29
d = rf.read()
FSIZE = len(d)
DeviceCount = d[Idx]
print("size", FSIZE)
# RETURN
frameInfo = []
for i in range(DeviceCount):
Idx += 63
print("Device", DeviceCount)
print("Fsize", FSIZE)
# end = 0
while (Idx < FSIZE) :
# Next Offset
nxt = int.from_bytes(d[Idx+8:(Idx+16)],'little')
print("Idx", Idx, "cframe", int.from_bytes(d[Idx:(Idx+8)],'little'))
frameInfo.append([int.from_bytes(d[Idx:(Idx+8)],'little'), int.from_bytes(d[Idx+8:(Idx+16)],'little')])
print("next offset", nxt)
Idx = int.from_bytes(d[Idx+8:(Idx+16)],'little')
t += 1
frameInfo = np.array(frameInfo)
return frameInfo
def livoxFileReadPoint(d, frame):
# Input: Read File NameReadFil, eName
# Output: [y,z,x,distance(r),Lidar ID]
points = []
# print("current frame", frame[0])
print("current frame", frame[0])
pointsPerFrame = []
Idx = frame[0] + 24
while Idx < frame[1] :
dtype = d[Idx + 17]
Idx = Idx + 27
if dtype==1:
# print("dtype",1)
for i in range(96):
B2D_X = int.from_bytes(d[Idx:Idx+4],'little', signed=True)
B2D_Y = int.from_bytes(d[Idx+4:Idx+8],'little', signed=True)
B2D_Z = int.from_bytes(d[Idx+8:Idx+12],'little', signed=True)
# print(B2D_X)
point_tmp = ([B2D_X,
B2D_Y,
B2D_Z,
d[Idx+12]
])
# print(B2D_X, B2D_Y, B2D_X)
# print(len(point_tmp))
pointsPerFrame.append(point_tmp)
# print("point_tmp", point_tmp)
Idx = Idx + 14
elif dtype==2:
print("dtype",2)
points.append(pointsPerFrame)
return pointsPerFrame
def main:
frame = livoxFileReadHook("../data/2023-06-12_18-19-16.lvx2")
vis = o3d.visualization.Visualizer()
vis.create_window()
scale = 10
line = np.linspace(0, scale, scale+1)
points = []
for x in line:
for y in line:
for z in line:
points.append([x,y,z])
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(np.array(points))
vis.add_geometry(pcd)
rf = open("../data/2023-06-12_18-19-16.lvx2", 'rb')
d = rf.read()
for f in frame:
# time.sleep(time_num)
pcd.points = o3d.utility.Vector3dVector(
np.array(livoxFileReadPoint(d, f))[:, :3]
)
vis.update_geometry(pcd)
vis.poll_events()
vis.update_renderer()
rf.close
vis.destroy_window()
終わりに
LiDARの生データを取得できるのは快適でよいですね
つたない文章ですが読んでいただきありがとうございました。