python读点云pcd的方法
点云数据的常见存储格式
激光点云是一些空间点的集合,常见于XYZI的
(
N
∗
4
)
(N*4)
(N∗4)数组。
对于这些点云数据的存储无非就那么些方式(二进制、ASCII码、序列化及其他我也不清楚的方式),如果按后缀名来说
.
p
c
d
、
.
b
i
n
、
.
n
p
y
、
.
p
k
l
.pcd、.bin、.npy、.pkl
.pcd、.bin、.npy、.pkl等等。
最常见的还是二进制格式跟ASCII码,也主要是介绍这两种存储方式的pcd点云的读取,比较推荐二进制的方式,读写速度快。
读写pcd
读取header
def read_pcd_binary(pcd_file):
with open(pcd_file, 'rb') as f:
# 读取pcd文件头信息
header = ""
while True:
line = f.readline().decode('utf-8')
header += line
if line.startswith("DATA"):
break
print(header)
# 从文件内容中读取三维坐标点云数据
dtype = np.dtype([('x', np.float32), ('y', np.float32), ('z', np.float32)])
data = np.fromfile(f, dtype=dtype)
return header, data
read/readlines
# bin
def read_bin(self, pcd_file):
## struct read binary
import struct
list = []
with open(pcd_file, 'rb') as f:
data = f.read()
pc_iter = struct.iter_unpack('ffff', data)
for i, point in enumerate(pc_iter):
list.append(point)
return np.array(list)
# ASCII
def read_pcd(self, pcd_file):
data = []
with open(pcd_file, 'r') as f:
lines = f.readlines()[11:]
for line in lines:
line = list(line.strip('\n').split(' '))
# line = np.array(line.strip('\n').split(' '), dtype=np.float32)
x = float(line[0])
y = float(line[1])
z = float(line[2])
i = float(line[3])
r = math.sqrt(x**2 + y**2 + z**2) * i
data.append(np.array([x,y,z,i,r]))
# points = list(map(lambda line: list(map(lambda x: float(x), line.split(' '))), lines))
points = np.array(data)
return points
pclpy
def pclpy_read_pcd(self, pcd_file):
## pclpy read pcd
import pclpy
from pclpy import pcl
cloud = pcl.PointCloud.PointXYZI()
# reader = pcl.io.PCDReader()
# reader.read(pcd_file, cloud)
pcl.io.loadPCDFile(pcd_file, cloud)
cloud = np.asarray(cloud.points)
open3d
def o3d_read_pcd(self, pcd_file):
## open3d read pcd
pcd = o3d.io.read_point_cloud(pcd_file)
points = np.asarray(pcd.points) # x, y, z
numpy
import numpy as np
data = np.array()
np.save('***.npy', data) #保存
# np保存为bin文件
data.tofile('data.bin')
load_data = np.load(npy_path, allow_pickle=True)
## np.fromfile read pcd
# 必须是二进制格式的pcd
cloud = np.fromfile(str(pcd_file), dtype=np.float32, count=-1)
# points = np.fromfile(lidar_path, dtype=np.float32).reshape(-1, 4)
# 去除Nan值
isnan = np.isnan(points[:,:3])
points = np.delete(points, np.where(isnan)[0], axis=0)
pickle
# read
with open(pkl_file, 'rb') as f:
data = pickle.load(f, encoding='latin1')
# write
with open(pkl_file, 'wb') as f:
pickle.dump(tensors, f, pickle.HIGHEST_PROTOCOL) # 写入tensor格式数据
python-pcl
# # pip3 install python-pcl
import pcl
pcd_ndarray = pcl.load(args.pcd_path).to_array()[:, :3] #不要intensity
# pcd_ndarray = pcl.load_XYZI(args.pcd_path).to_array()[:, :4]
point_num = pcd_ndarray.shape[0]
pypcd
from pypcd import pypcd
import numpy as np
def read_pcd(pcd_path):
pcd = pypcd.PointCloud.from_path(pcd_path)
pcd_np_points = np.zeros((pcd.points, 4), dtype=np.float32)
pcd_np_points[:, 0] = np.transpose(pcd.pc_data['x'])
pcd_np_points[:, 1] = np.transpose(pcd.pc_data['y'])
pcd_np_points[:, 2] = np.transpose(pcd.pc_data['z'])
pcd_np_points[:, 3] = np.transpose(pcd.pc_data['intensity'])
del_index = np.where(np.isnan(pcd_np_points))[0]
pcd_np_points = np.delete(pcd_np_points, del_index, axis=0)
return pcd_np_points
python读取pcd
点云存储形式为:
- binary_compressed
- ascii
# coding: utf-8
import cv2
import os
import numpy as np
import math, struct
import lzf
numpy_pcd_type_mappings = [(np.dtype('float32'), ('F', 4)),
(np.dtype('float64'), ('F', 8)),
(np.dtype('uint8'), ('U', 1)),
(np.dtype('uint16'), ('U', 2)),
(np.dtype('uint32'), ('U', 4)),
(np.dtype('uint64'), ('U', 8)),
(np.dtype('int16'), ('I', 2)),
(np.dtype('int32'), ('I', 4)),
(np.dtype('int64'), ('I', 8))]
numpy_type_to_pcd_type = dict(numpy_pcd_type_mappings)
pcd_type_to_numpy_type = dict((q, p) for (p, q) in numpy_pcd_type_mappings)
def build_dtype(metadata):
""" Build numpy structured array dtype from pcl metadata.
Note that fields with count > 1 are 'flattened' by creating multiple
single-count fields.
*TODO* allow 'proper' multi-count fields.
"""
fieldnames = []
typenames = []
for f, c, t, s in zip(metadata['FIELDS'],
metadata['COUNT'],
metadata['TYPE'],
metadata['SIZE']):
np_type = pcd_type_to_numpy_type[(t, s)]
if c == 1:
fieldnames.append(f)
typenames.append(np_type)
else:
fieldnames.extend(['%s_%04d' % (f, i) for i in xrange(c)])
typenames.extend([np_type]*c)
dtype = np.dtype(list(zip(fieldnames, typenames)))
return dtype
def parse_binary_compressed_pc_data(f, dtype, metadata):
""" Parse lzf-compressed data.
Format is undocumented but seems to be:
- compressed size of data (uint32)
- uncompressed size of data (uint32)
- compressed data
- junk
"""
fmt = 'II'
compressed_size, uncompressed_size =\
struct.unpack(fmt, f.read(struct.calcsize(fmt)))
compressed_data = f.read(compressed_size)
# TODO what to use as second argument? if buf is None
# (compressed > uncompressed)
# should we read buf as raw binary?
buf = lzf.decompress(compressed_data, uncompressed_size)
if len(buf) != uncompressed_size:
raise IOError('Error decompressing data')
# the data is stored field-by-field
pc_data = np.zeros(metadata['WIDTH'], dtype=dtype)
ix = 0
for dti in range(len(dtype)):
dt = dtype[dti]
bytes = dt.itemsize * metadata['WIDTH']
column = np.frombuffer(buf[ix:(ix+bytes)], dt)
pc_data[dtype.names[dti]] = column
ix += bytes
return pc_data
def read_pcd_binary(pcd_file):
# pcd = pypcd.PointCloud.from_path(pcd_file)
# print(pcd)
with open(pcd_file, 'rb') as f:
# 读取pcd文件头信息
header_dict = {}
while True:
line = f.readline().decode('utf-8').strip()
key, value = line.split(" ", 1)
header_dict[key] = value
if key in ('FIELDS', 'TYPE'):
header_dict[key] = value.split()
if key in ('SIZE', 'COUNT'):
header_dict[key] = map(int, value.split())
# header_dict[key] = [int(i) for i in value.split()]
elif key in ('WIDTH', 'HEIGHT', 'POINTS'):
header_dict[key] = int(value)
elif key == 'VIEWPOINT':
header_dict[key] = map(float, value.split())
# header_dict[key] = [float(i) for i in value.split()]
if line.startswith("DATA"):
break
if header_dict['DATA'] == 'binary_compressed':
# 从文件内容中读取三维坐标点云数据
# dtype = np.dtype([('x', np.float32), ('y', np.float32), ('z', np.float32), ('intensity', np.uint8),
# ('ring', np.uint16), ('timestamp', np.float64)])
dtype = build_dtype(header_dict)
data = parse_binary_compressed_pc_data(f, dtype, header_dict)
points = []
for id, p in enumerate(data):
x, y, z, i = p[0], p[1], p[2], p[3]
r = math.sqrt(x ** 2 + y ** 2 + z ** 2) * i
points.append(np.array([x, y, z, i, r]))
points = np.array(points)
isnan = np.isnan(points[:, :3])
points = np.delete(points, np.where(isnan)[0], axis=0)
elif header_dict['DATA'] == 'ascii':
points = read_pcd(pcd_file)
return points
def read_data(path):
for i in os.listdir(path):
if i.split('.')[-1] == "png":
file = os.path.join(path, i)
print(file)
data = cv2.imread(file, cv2.IMREAD_GRAYSCALE)
print(data)
print(np.unique(data), data.shape)
if i.split('.')[-1] == 'pcd':
file = os.path.join(path, i)
# print(file)
# read_pcd(file)
read_pcd_binary(file)
def read_pcd(pcd_file, readline=11):
data = []
with open(pcd_file, 'r') as f:
lines = f.readlines()[readline:]
for line in lines:
line = np.array(line.split(' '), dtype=np.float32)
x, y, z, i = line[0], line[1], line[2], line[3]
r = math.sqrt(x ** 2 + y ** 2 + z ** 2) * i
data.append(np.array([x, y, z, i, r]))
points = np.array(data)
isnan = np.isnan(points[:, :3])
points = np.delete(points, np.where(isnan)[0], axis=0)
print(points.shape)
return points
if __name__ == "__main__":
path = r"test_data/pcd"
read_data(path)