使用Open3D库直接从激光雷达中读取点云信息并显示

本文的目的是直接从雷达的端口读取到数据，并将数据解析成点云的形式然后显示出来。

使用的Python库

import numpy as np
import open3d as o3d
import threading
from socket import *
from queue import Queue

文章中使用的雷达是robosense16，不同厂家的雷达和线束会带来参数上的差异。

准备工作
使用udp的方法，绑定对应的端口。

udpsocket = socket(AF_INET, SOCK_DGRAM)
udpsocket.bind(("", 6699))
q = Queue(maxsize=1) # 队列的长度为1，来达到实时

第一个函数根据不同的线束来映射相对应的角度，16线雷达的线束ID为1到8对应-15到-1，9到16对应15到1。
第二个函数根据收到的每个block中的数据和当前水平角度，来计算点的x,y,z和反射值。（在本方法中过滤了距离大于120的点，方向为任意方向。）

def channelmaptheta(c):
    theta = np.array([-15, -13, -11, -9, -7, -5, -3, -1, 15, 13, 11, 9, 7, 5, 3, 1])
    theta = theta * np.pi / 180
    return theta[c]

def distance(lidardata, alpha):
    points = []
    for i in range(0, 96, 6):
        omega = channelmaptheta(int(i / 6))
        # print(alpha, omega)
        dis = int(lidardata[i + 0:i + 4], 16) / 100
        if (dis > 120):
            continue
        reflex = int(lidardata[i + 4:i + 6], 16)
        x = dis * np.cos(omega) * np.sin(alpha)
        y = dis * np.cos(omega) * np.cos(alpha)
        z = dis * np.sin(omega)

        points.append(np.asarray([x, y, z]))
    return points

具体实现
每1248个字节为一个package，该package中从第43个字节开始为点云数据，一直到1242个字节。
这1200个字节又分为12个block，每个block又有32个channel。
本文仅处理了单回波模式类的数据，即仅处理前16个channel。（TODO）

def process_points():
    cnt = 0
    pointcloud = []
    while True:
        if (cnt == 900):
            q.put(pointcloud)
            cnt = 0
            pointcloud = []

        recvInfo = udpsocket.recv(1248)
        data = recvInfo[42:-6].hex()
        strin = data.split('ffee')
        for j in range(1, len(strin)):
            alpha = int(strin[j][0:4], 16) / 100 * np.pi / 180
            # print(alpha)
            cnt += 1
            lidardata = strin[j][4:]
            points = distance(lidardata, alpha)
            pointcloud.extend(points)

使用Open3D的库进行显示。

def visualize_pointscloud():
    vis = o3d.visualization.Visualizer()
    vis.create_window()
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(q.get())
    vis.add_geometry(pcd)

    while True:
        pcd.points = o3d.utility.Vector3dVector(q.get())
        vis.update_geometry(pcd)
        vis.poll_events()
        vis.update_renderer()

在最后使用多线程实现生产者消费者的典型模式。

process = threading.Thread(target=process_points)
visual = threading.Thread(target=visualize_pointscloud)

process.start()
visual.start()

process.join()
visual.join()

效果

总结
本文的初衷是便于在Win下能够直接得到点云，然后用于深度学习推理，同时也可以通过该案例对激光雷达数据的原理和Open3D库的使用进行学习。该方法仍存在BUG，鉴于鄙人水平有限，仍有很大的优化空间。欢迎大家交流批评指正，谢谢。

完整代码

import numpy as np
import open3d as o3d
import threading
from socket import *
from queue import Queue

udpsocket = socket(AF_INET, SOCK_DGRAM)

udpsocket.bind(("", 6699))

lock = threading.Lock()
q = Queue(maxsize=1)


def channelmaptheta(c):
    theta = np.array([-15, -13, -11, -9, -7, -5, -3, -1, 15, 13, 11, 9, 7, 5, 3, 1]) # 根据自己的传感器模型需要修改垂直角度
    theta = theta * np.pi / 180
    return theta[c]


def distance(lidardata, alpha):
    points = []
    for i in range(0, 96, 6):
        omega = channelmaptheta(int(i / 6))
        # print(alpha, omega)
        dis = int(lidardata[i + 0:i + 4], 16) / 100
        if (dis > 120):
            continue
        reflex = int(lidardata[i + 4:i + 6], 16)
        x = dis * np.cos(omega) * np.sin(alpha)
        y = dis * np.cos(omega) * np.cos(alpha)
        z = dis * np.sin(omega)

        points.append(np.asarray([x, y, z]))
    return points


def process_points():
    cnt = 0
    pointcloud = []
    while True:
        if (cnt == 900): # 根据传感器模型不同，包的长度可能不同，所以需要修改接收的长度。
            q.put(pointcloud)
            cnt = 0
            pointcloud = []

        recvInfo = udpsocket.recv(1248)
        data = recvInfo[42:-6].hex() # 同理需要修改这里
        strin = data.split('ffee')
        for j in range(1, len(strin)):
            alpha = int(strin[j][0:4], 16) / 100 * np.pi / 180
            # print(alpha)
            cnt += 1
            lidardata = strin[j][4:]
            points = distance(lidardata, alpha)
            pointcloud.extend(points)


def visualize_pointscloud():
    vis = o3d.visualization.Visualizer()
    vis.create_window()
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(q.get())
    vis.add_geometry(pcd)

    while True:
        pcd.points = o3d.utility.Vector3dVector(q.get())
        vis.update_geometry(pcd)
        vis.poll_events()
        vis.update_renderer()


process = threading.Thread(target=process_points)
visual = threading.Thread(target=visualize_pointscloud)

process.start()
visual.start()

process.join()
visual.join()