第二章 python-pcl、open3d讀取、顯示pcd、bin格式點云數(shù)據(jù)

前言

點云數(shù)據(jù)實際上就是許多組點的集合，每個點由{x,y,z}組成。當然理論上的只包含有3D坐標。
實際激光雷達獲取的點云數(shù)據(jù)還會包含強度、反射率等等。但我們一般只用提取{x,y,z}來處理即可。

點云數(shù)據(jù)相比于其他傳感器數(shù)據(jù)的核心優(yōu)勢就是在于 精準的深度信息。可惜獲取具體的坐標信息。

環(huán)境

因為涉及到深度學習的應用，所以運用的整體環(huán)境是基于 python 使用pycharm編輯。
主要涉及庫 python-pcl open3D mayavi numpy 等等用來處理點云數(shù)據(jù)并顯示。

實際上點云的核心處理庫應該是PCL 類似于圖像處理中的Opencv 。后面也會有擴展。

一、點云數(shù)據(jù)類型

基于激光雷達錄制的點云文件具有多種格式，如pcd、npy、ply、bin、pcap、lvx等等。
而讀取點云數(shù)據(jù)也有多個庫，python-pcl open3D mayavi numpy。
這里主要使用 python-pcl 和 open3D 兩個核心庫。

1.基于python-pcl 讀取顯示pcd、bin格式文件

代碼如下（示例）：

import numpy as np
import pcl.pcl_visualization
pt = pcl.load("D://code-python//Data//lidar//2094.799809520.pcd")

#轉為數(shù)組 形如 24000 x 3 
points = pt.to_array()

#但是pcl顯示是要 N*4 所以要擴展一列 取一列插入數(shù)組使 N*3 變?yōu)?N*4
x = points[:,0]
points = np.insert(points,3,x,axis=1)

# 這里對第四列進行賦值，它代表顏色值，根據(jù)你自己的需要賦值即可；
points[:, 3] = 255

# PointCloud_PointXYZRGB 需要點云數(shù)據(jù)是N*4，分別表示x,y,z,RGB ,其中RGB 用一個整數(shù)表示顏色；
color_cloud = pcl.PointCloud_PointXYZRGB(points)
visual = pcl.pcl_visualization.CloudViewing()

#窗口名 
visual.ShowColorCloud(color_cloud, b'sta')

flag = True
while flag:
    flag != visual.WasStopped()

這里就可以成功顯示 顏色是根據(jù)自己設置

下面基于 numpy 讀取bin格式點云文件

import pcl.pcl_visualization
import numpy as np

# lidar_path 指定一個kitti 數(shù)據(jù)的點云bin文件就行了
#bin文件為2進制文件
lidar_path = r'D:/code-python/Data/lidar/000000.bin'

# reshape成 N*4
points = np.fromfile(lidar_path, dtype=np.float32).reshape(-1, 4)  


# 在這里對第四列進行賦值，它代表顏色值，根據(jù)你自己的需要賦值即可；
points[:,3] = 255

# PointCloud_PointXYZRGB 需要點云數(shù)據(jù)是N*4，分別表示x,y,z,RGB ,其中RGB 用一個整數(shù)表示顏色；
color_cloud = pcl.PointCloud_PointXYZRGB(points)
visual = pcl.pcl_visualization.CloudViewing()
visual.ShowColorCloud(color_cloud, b'cloud')
flag = True
while flag:
    flag != visual.WasStopped()

2.基于open3d 讀取顯示pcd格式文件

代碼如下（示例）：

import open3d as o3d
import numpy as np

point = o3d.io.read_point_cloud("D:/code-python/Data/lidar/000000.pcd")

o3d.visualization.draw_geometries([point])

#open3d顯示的時候有個bug 要把python  pycharm設置為高性能  系統(tǒng)顯示里 圖形設置

效果相對于 python-pcl的顯示還是差一點，不過也挺不錯。同時是可以調的，它也是一個成熟的3D處理庫。有許多內置的處理函數(shù)。

3.解析pcap格式點云文件并通過python-pcl顯示

這里參考一位大佬的代碼：
作者：lonlon ago
https://zhuanlan.zhihu.com/p/158621756
代碼如下：

# -*- coding: UTF-8 -*-
import dpkt
import collections  # 有序字典需要的模塊
import time
import numpy as np
import struct

# 安裝了pcl , 可以使用它來進行可視化
import pcl.pcl_visualization
viewer = pcl.pcl_visualization.PCLVisualizering()#初始化一個對象
viewer.SetBackgroundColor(0, 0, 0) #顏色
viewer.AddCoordinateSystem()
viewer.InitCameraParameters()

# vlp 16 的參數(shù)
'''
https://blog.csdn.net/qq_34911636/article/details/89946329#commentBox
激光雷達每一幀的數(shù)據(jù)長度固定為1248字節(jié)，其中分別為前42字節(jié)的前數(shù)據(jù)包標識、12組數(shù)據(jù)包、4字節(jié)時間戳和最后兩字節(jié)雷達型號參數(shù)。
12組數(shù)據(jù)包中前兩字節(jié)為數(shù)據(jù)包的開始標識（0xFFEE）、接下去兩字節(jié)為的旋轉角度（當前角度）值和連續(xù)32*（2字節(jié)的距離值+1字節(jié)的激光反射強度值）字節(jié)的距離信息，
其中32*3字節(jié)分別為雷達兩次獲取探測信息，每個數(shù)據(jù)包開頭所攜帶的旋轉角度是指當前數(shù)據(jù)包前16*3字節(jié)對應的角度，而后16*3字節(jié)對應的旋轉角度激光雷達沒有直接給出，
需要通過計算前后兩次旋轉角度然后求取平均值獲得。
1248 = 42 + 12*(2 + 2 + 32*(2+1)) + 4 + 2 =1248

雷達掃描頻率為10Hz，每秒數(shù)據(jù)包在480幀左右，即每次掃描會產生48個左右的數(shù)據(jù)包，需要將分散的數(shù)據(jù)包數(shù)據(jù)合并稱為一次掃描的點云數(shù)據(jù)
75個udp包產生一圈數(shù)據(jù) vlp格式解析那篇文章 下面評論沒理解 為什么是75 75*384*10 = 288000 
若按照這樣理解 角分辨率按照0.1度 360/0.1 = 3600 一圈轉3600次 一次16個點 = 57600 10hz 1s 10圈 即 57600*10 = 576000 反正
有點亂
'''
DISTANCE_RESOLUTION = 0.002   # 距離數(shù)值分辨率 2mm轉換為單位米
udp_package_num = 1
line_per_udp = 12        # 每個UDP 有多少列
point_per_udp_line = 32  # 每個UDP 的每列包含有多少個點
point_num_per_udp = point_per_udp_line * line_per_udp  # 32*12=384

thetas_lines = [-15, 1, -13, 3, -11, 5, -9, 7, -7, 9, -5, 11, -3, 13, -1, 15] #垂直角度w代表值
thetas_point = thetas_lines * 2 * line_per_udp * udp_package_num   #感覺是嵌套列表 列表乘以一個數(shù)字 [[x],[x],[x]...]
thetas_point = np.radians(thetas_point) #角度從度轉為弧度 thetas_point為輸入的角度 它返回一個數(shù)組, 其中包含輸入數(shù)組中給定度數(shù)的等效弧度角。
thetas_point_cos = np.cos(thetas_point)  #cos弧度
thetas_point_sin = np.sin(thetas_point)  #sin弧度


data_fmt = '<' + (('H' + 'H' + 'HB' * point_per_udp_line) * line_per_udp + 'IH') * udp_package_num
base_range = np.array(range(2, point_per_udp_line*2+1, 2))  # 32， 距離值的基礎索引
#range (start,stop,step) range(2,65,2) 32個數(shù)
angle_base_range = np.array([1])
d_range = []
r_range = []
angle_range = []
k = 0
data_gap = 2 + 2*point_per_udp_line  # 每一列的長度 其實是2字節(jié)標識 2字節(jié)旋轉角度 32*(2字節(jié)距離,1字節(jié)反射強度)點 這里貌似只計算旋轉角度+距離66

for i in range(udp_package_num):
    for j in range(line_per_udp):
        d_range.append(base_range + k * data_gap + i * 2)  # 66 是 HH + HB*32  d.range 列表增加,多個列表嵌套
        r_range.append(base_range + k * data_gap + i * 2 + 1)
        angle_range.append(angle_base_range + k * data_gap + i * 2)
        k += 1
d_range = np.hstack(d_range)  # 多個array組成的列表
r_range = np.hstack(r_range)
angle_range = np.hstack(angle_range)

# 水平角度插值，如果有角度跳變會怎么樣？  針對從360跳變到20的這部分擬合的并不是很好，誤差很大；已經改正
x_index = np.arange(point_num_per_udp)
xp_index = np.arange(0, point_num_per_udp, point_per_udp_line) # array([  0,  32,  64,  96, 128, 160, 192, 224, 256, 288, 320, 352])




def unpack_udp(data):

    data_tuple = struct.unpack(data_fmt, data)  # 原始格式是元祖，要轉array，元祖不能索引
    data_unpack = np.array(data_tuple, dtype=np.int64)  # np.array會多耗時15毫秒  todo 這里為什么會報錯？？ 可能是時間戳的數(shù)值太大了

    distances = data_unpack[d_range]  # 115200
    refs = data_unpack[r_range] / 255
    angles = data_unpack[angle_range]
    angles = np.radians(angles / 100).astype(np.float32)  # 除以100再弧度值
    # 第一種處理角度的方式
    # angles = np.tile(angles, (32, 1)).flatten('F')  # 因為angle只有1個，數(shù)據(jù)有32個，需要復制32次
    # 第二種方式
    angles_interp = np.interp(x_index, xp_index, angles).astype(np.float32)
    if angles[0] > angles[-1]:  # 出現(xiàn)了角度的轉折點
        # replace_angle = np.linspace(0,20,32) # 針對從360跳變到20 的角度替換
        change_index = np.argmax(angles)
        replace_index = change_index * 32 + 1
        interp_num_2 = int(angles[change_index+1]*32/40)  # 每個UDP數(shù)據(jù)包之間的角度間隔為 40，每個包有32條線；
        interp_num_1 = 32 - interp_num_2
        replace_angle_1 = np.linspace(angles[change_index], 35999, interp_num_1)  # 針對從360跳變到 20 的角度替換
        replace_angle_2 = np.linspace(0, angles[change_index+1],   interp_num_2)  # 針對從360跳變到 20 的角度替換
        angles_interp[replace_index:(replace_index+interp_num_1)] = replace_angle_1
        angles_interp[(replace_index+interp_num_1):(replace_index+32)] = replace_angle_2


    distances = distances * DISTANCE_RESOLUTION
    x = distances * thetas_point_cos * np.sin(angles_interp)
    y = distances * thetas_point_cos * np.cos(angles_interp)
    z = distances * thetas_point_sin
    raw_points = np.stack((x, y, z), axis=1).astype(np.float32)
    # raw_points = np.stack((distances, angles_interp, refs, ), axis=1)   # 也可以只要原始數(shù)據(jù)

    print(type(x))
    print(len(x)) #384一組 每個udp384一組
    #print(x)


    return raw_points






def main(file_path):
    # f = open(file_path)          # 此寫法為python2之下，
    f = open(file_path, mode='rb') #python3
    try:
        pcap = dpkt.pcap.Reader(f)  # 先按.pcap格式解析，若解析不了，則按pcapng格式解析
    except:
        print("it is not pcap ... format, pcapng format...")
        pcap = dpkt.pcapng.Reader(f)
        # 接下來就可以對pcap做進一步解析了，記住在使用結束后最好使用f.close()關掉打開的文件，雖然程序運行結束后，
        # 系統(tǒng)會自己關掉，但是養(yǎng)成好習慣是必不可少的。當前變量pcap中是按照“間戳：單包”的格式存儲著各個單包
    # 將時間戳和包數(shù)據(jù)分開，一層一層解析，其中ts是時間戳，buf存放對應的包
    all_pcap_data = collections.OrderedDict()  # 有序字典
    # all_pcap_data_hex = collections.OrderedDict()  # 有序字典,存十六進制形式
    cir_point = []
    i = 1
    for (ts, buf) in pcap:
        try:
            eth = dpkt.ethernet.Ethernet(buf)  # 解包，物理層
            if not isinstance(eth.data, dpkt.ip.IP):  # 解包，網絡層，判斷網絡層是否存在，
                continue
            ip = eth.data
            # if not isinstance(ip.data, dpkt.tcp.TCP):  # 解包，判斷傳輸層協(xié)議是否是TCP，即當你只需要TCP時，可用來過濾
            #     continue
            if not isinstance(ip.data, dpkt.udp.UDP):#解包，判斷傳輸層協(xié)議是否是UDP
                continue
            transf_data = ip.data  # 傳輸層負載數(shù)據(jù)，基本上分析流量的人都是分析這部分數(shù)據(jù)，即應用層負載流量
            if not len(transf_data.data):  # 如果應用層負載長度為0，即該包為單純的tcp包，沒有負載，則丟棄
                continue

            if len( transf_data.data) != 1206:  # 長度過濾   todo 為什么會有512字節(jié)的數(shù)據(jù)
                continue

            all_pcap_data[ts] = transf_data.data  # 將時間戳與應用層負載按字典形式有序放入字典中，方便后續(xù)分析.
            points = unpack_udp(transf_data.data)
            if i % 76 != 0:  # vlp16 每75個UDP數(shù)據(jù)包形成一圈數(shù)據(jù)
                cir_point.append(points)
            else:
                cir_udp = np.vstack(cir_point)
                print(cir_udp.shape)            # 最后需要的一圈完整的點云數(shù)據(jù) 只包含了28000余個xyz坐標
                cloud_all = pcl.PointCloud(cir_udp[:, 0:3].astype(np.float32))   # 可視化
                viewer.AddPointCloud(cloud_all)
                viewer.SpinOnce(100)
                viewer.RemoveAllPointClouds(0)
                cir_point = []

            i += 1
        except Exception as err:
            print( "[error] %s" % err)
    f.close()






if __name__ == '__main__':
    #file_path="D:xxxxxx.pcap"
    file_path = "2020-10-21-10-13-57_Velodyne-VLP-16-Data.pcap"
    main(file_path)

這里其實都是數(shù)據(jù)格式解析，要知道怎么樣的數(shù)據(jù)格式，用什么方式能夠更好的解析。
當然好的工具使用起來也是很舒服，感謝開發(fā)這些庫的大佬們。

總結

本文僅僅簡單介紹了基于python的不同類型點云數(shù)據(jù)讀取、顯示方法，這些都比較簡單。
ubuntu下一般都是錄制bag 文件，并通過rviz顯示，后續(xù)可以擴展一下。文章來源地址http://www.zghlxwxcb.cn/news/detail-793763.html

到了這里，關于第二章 python-pcl、open3d讀取、顯示pcd、bin等格式點云數(shù)據(jù)的文章就介紹完了。如果您還想了解更多內容，請在右上角搜索TOY模板網以前的文章或繼續(xù)瀏覽下面的相關文章，希望大家以后多多支持TOY模板網！