最近在公司搞了搞激光雷达，把代码写一写。
参数简介
Velodyne VLP-16
测距精度 ±3.6 cm
转速 RPM 600
数据端口 2368
通讯协议 DUP
Python 3.6.8
数据包介绍
１、数据包结构参考VLP16手册，以下为激光雷达一个完整的数据包，在解析过程中底层会过滤掉42 字节的UDP报头。所以实际接收到的数据长度为1206字节。
２、每个数据包有12个数据块，每个数据块有2字节的0xFFEE数据标记，2个字节的方位角数据，2个数据列，每个数据列又包含总共48字节的16线的点距离信息（每个点距离信息占3个字节——2字节的距离信息，1字节的反射率信息）。
实际数据介绍
注意：在进行数据转换的时候注意将数据字节进行倒置
坐标转换关系
俯仰角：
单位：角度 / °（注意不是弧度）
vertical_angle_list = [-15, 1, -13, 3, -11, 5, -9, 7, -7, 9, -5, 11, -3, 13, -1, 15]
Z轴垂直高度修正值：
单位：毫米/mm
vertical_correction_list = [11.2, -0.7, 9.7, -2.2, 8.1, -3.7, 6.6, -5.1, 5.1, -6.6, 3.7, -8.1, 2.2, -9.7, 0.7, -11.2]
补偿半径
注意：在坐标转换的时候注意加上41.910mm的补偿半径。
运行结果
代码
import socket
import open3d as o3d
import numpy as np
from math import radians, cos, sin
# 垂直角度 w 列表,单位:角度/° [来源请参考VLP16手册]
g_vertical_angle_list = [-15, 1, -13, 3, -11, 5, -9, 7, -7, 9, -5, 11, -3, 13, -1, 15]
# 垂直距离矫正列表,单位:毫米/mm [来源请参考VLP16手册]
g_vertical_correction_list = [11.2, -0.7, 9.7, -2.2, 8.1, -3.7, 6.6, -5.1, 5.1, -6.6, 3.7, -8.1, 2.2, -9.7, 0.7, -11.2]
# 将点云保存到ply文件
def save_to_ply(save_file, point_list):
length = len(point_list)
with open(save_file, 'w') as f:
f.writelines((
"ply\n",
"format ascii 1.0\n",
"element vertex {}\n".format(length),
"property float x\n",
"property float y\n",
"property float z\n",
"property uchar red\n",
"property uchar green\n",
"property uchar blue\n",
"element face 0\n",
"property list uint8 int32 vertex_index\n",
"end_header\n"))
for i in range(length):
f.writelines("%f %f %f %d %d %d\n" % (point_list[i][0], point_list[i][1], point_list[i][2],
point_list[i][3], point_list[i][4], point_list[i][5]))
class PointCloud:
# @ distance：测出的点云距离，单位：m(米)
# @ azimuth_angle：数据点与发射中心点的连线在XOY面的分向量与Y轴的夹角,即:方位角
# @ w_angle：数据点与发射中心点的连线与平面的夹角,或激光的俯仰角
# @ reflect：反射率，单位(百分比 % )
# [坐标换算请参考VLP16手册]
def __init__(self, distance, azimuth_angle, w_angle, reflect, correction_index):
# 根据手册提供的关系式求出 XYZ 坐标
launch_radius = 41.910 / 1000 # 单位：m/米
w_angle = radians(w_angle) # 弧度转换
azimuth_angle = radians(azimuth_angle) # 弧度转换
self.x = (distance * cos(w_angle) + launch_radius) * sin(azimuth_angle)
self.y = (distance * cos(w_angle) + launch_radius) * cos(azimuth_angle)
self.z = distance * sin(w_angle) + g_vertical_correction_list[correction_index] / 1000
self.r = reflect # 颜色以反射率为基准
self.g = reflect
self.b = reflect
self.reflect = reflect
def to_string(self):
return '(' + str(self.x) + 'm,' + str(self.y) + 'm,' + str(self.z) + 'm) '
def de_code_lidar_data(data):
delta_count = 0
delta_value = 0
point_cloud_list = []
azimuth_angle_list = []
# 计算并存储12个 data block 的方位角(azimuth_angle) [详细来源请参考VLP16手册]
for i in range(0, 12):
block_index = i * 100
azimuth_angle_index = block_index + 2
azimuth_angle = float(int(data[azimuth_angle_index + 1] << 8) + data[azimuth_angle_index]) * 0.01
azimuth_angle_list.append(azimuth_angle)
# 计算出每个data block的第15-31个点的方位角并存储至 azimuth_angle_list [详细来源请参考VLP16手册]
azimuth_angle_list_tmp = azimuth_angle_list.copy()
for i in range(0, 11):
a1 = azimuth_angle_list[i]
a2 = azimuth_angle_list[i + 1]
if a2 < a1:
a2 += 360
avg = (a1 + a2) / 2
delta_value += (a2 - a1) / 2
delta_count += 1
azimuth_angle_list_tmp.insert(i * 2 + 1, avg)
# 方位角平均增量
delta_degrees_inc = delta_value / delta_count
# 求出最后一个数据序列的方位角
azimuth_angle_list_tmp.append(azimuth_angle_list_tmp[len(azimuth_angle_list_tmp) - 1] + delta_degrees_inc)
azimuth_angle_list = azimuth_angle_list_tmp.copy()
# 遍历12个 data block 的每个数据点 [详细来源请参考VLP16手册]
for i in range(0, 12): # 每个数据包的第 i 个block [详细来源请参考VLP16手册]
block_index = i * 100
block_point_index = block_index + 4
data_flag = int(data[block_index] << 8) + int(data[block_index + 1])
if data_flag == 0xffee:
for j in range(0, 2): # 每个block的第 j 组数据 [详细来源请参考VLP16手册]
azimuth_angle = azimuth_angle_list[i * 2 + j]
if 0.0 <= azimuth_angle <= 1.0: # 筛选方向角为 0 - 1 的数据点
print('azimuth_angle = ' + str(azimuth_angle))
for k in range(0, 16): # 16线中的第 k 个线
# 对应线路的俯仰角 [详细来源请参考VLP16手册]
w_angle = g_vertical_angle_list[k]
# 点到激光雷达中心的距离 [详细来源请参考VLP16手册]
distance = (int(data[block_point_index + j * 48 + k * 3 + 1] << 8) + int(
data[block_point_index + j * 48 + k * 3])) / 500
# 点的反射率 [详细来源请参考VLP16手册]
reflectivity = int(data[block_point_index + j * 48 + k * 3 + 2])
print('w_angle = ' + str(w_angle) + ',distance = ' + str(distance) + ',degrees = ' + str(
azimuth_angle))
# 生成点云对象
point = PointCloud(distance, azimuth_angle, w_angle, reflectivity, k)
# 数据存入点云列表
point_cloud_list.append([point.x, point.y, point.z, point.r, point.g, point.b])
else:
print('data error !')
return point_cloud_list
if __name__ == '__main__':
np_points = []
target_running_time = 5 # 单位:秒/s
start_record_time_flag = 0
ip_config = ('', 2368) # VLP16 的UDP协议端口号为 2368, 可不指定IP地址 [详细来源请参考VLP16手册]
my_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
my_socket.bind(ip_config)
while True:
lidar_data, address = my_socket.recvfrom(1206)
# 计算数据包的时间戳, 单位:微妙/us [详细来源请参考VLP16手册]
time_stamp = int(lidar_data[1203] << 24) + int(lidar_data[1202] << 16) + int(lidar_data[1201] << 8) + int(lidar_data[1200])
if start_record_time_flag == 0:
first_lidar_data_time_stamp = time_stamp
time_stamp_old = time_stamp
start_record_time_flag = 1
np_points += de_code_lidar_data(lidar_data)
running_time = (time_stamp - first_lidar_data_time_stamp) / 1000000
if running_time >= target_running_time:
break
file_name = 'C:/lidar_point_cloud_01.ply'
save_to_ply(file_name, np_points)
# 利用 open3d 显示点云
print(np_points)
pcd = o3d.io.read_point_cloud(file_name)
pcd.points = o3d.utility.Vector3dVector(pcd.points)
print(pcd)
print(np.asarray(pcd.points))
o3d.visualization.draw_geometries([pcd])