# -*- coding: utf-8 -*-#
# -------------------------------------------------------------------------------
# Name: 点云降维处理
# Author: yunhgu
# Date: 2021/8/23 10:05
# Description:
# -------------------------------------------------------------------------------
import copy
import open3d as o3d
from pyntcloud import PyntCloud
import numpy as np
import random
import matplotlib.pyplot as plt
from pandas import DataFrame
from pypcd import pypcd
def parse_pcd_data(pcd_file):
pcd_obj = pypcd.PointCloud.from_path(pcd_file)
data_list = []
for item in pcd_obj.pc_data:
data_list.append([item[0], item[1], item[2]])
return np.array(data_list)
def point_cloud_show(points):
fig = plt.figure(dpi=150)
ax = fig.add_subplot(111, projection='3d')
ax.scatter(points[:, 0], points[:, 1], points[:, 2], cmap="spectral", s=2, linewidths=0, alpha=1, marker='.')
plt.title("Point Cloud")
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')
plt.show()
# 显示二维点云
def point_show(pcd_point_cloud):
x, y = [], []
for i in range(pcd_point_cloud.shape[0]):
x.append(pcd_point_cloud[i][0])
y.append(pcd_point_cloud[i][1])
plt.scatter(x, y)
plt.show()
def voxel_filter(point_cloud, leaf_size, filter_mode):
:param point_cloud:点云
:param leaf_size:voxel尺寸
:param filter_mode:
:return:
filtered_points = []
# step1,计算边界值,计算x,y,z三个维度的最值
x_max, y_max, z_max = np.amax(point_cloud, axis=0)
x_min, y_min, z_min = np.amin(point_cloud, axis=0)
# step2,确定体素的尺寸
size_r = leaf_size
# step3,计算每个voxel的维度
dx = (x_max - x_min) / size_r
dy = (y_max - y_min) / size_r
dz = (z_max - z_min) / size_r
# step4,计算每个点voxel grid内每个维度的值
h = []
for i in range(len(point_cloud)):
hx = np.floor((point_cloud[i][0] - x_min) / size_r)
hy = np.floor((point_cloud[i][1] - y_min) / size_r)
hz = np.floor((point_cloud[i][2] - z_min) / size_r)
h.append(hx + hy * dx + hz * dx * dy)
# step5,对h值进行排序
h = np.array(h)
h_index = np.argsort(h) # 提取索引
h_sorted = h[h_index] # 升序
count = 0 # 用于维度的积累
# 将h值相同的点放到同一个grid,进行筛选
np.seterr(divide='ignore', invalid='ignore') # 忽略除法遇到无效值的问题
for i in range(len(h_sorted) - 1):
if h_sorted[i] == h_sorted[i + 1]:
continue
elif filter_mode == 'random': # 随机滤波
point_idx = h_index[count:i + 1]
random_points = random.choice(point_cloud[point_idx])
filtered_points.append(random_points)
count = i
for i in range(len(h_sorted) - 1):
if h_sorted[i] == h_sorted[i + 1]:
continue
elif filter_mode == 'centroid': # 随机滤波
point_idx = h_index[count:i + 1]
filtered_points.append(np.mean(point_cloud[point_idx], axis=0))
count = i
filtered_points = np.array(filtered_points, dtype=np.float64)
return filtered_points
PCD_BINARY_TEMPLATE = """VERSION 0.7
FIELDS x y z
SIZE 4 4 4
TYPE F F F
COUNT 1 1 1
WIDTH {}
HEIGHT 1
VIEWPOINT 0 0 0 1 0 0 0
POINTS {}
DATA binary
def to_pcd_binary(pcdpath, points):
f = open(pcdpath, 'wb')
shape = points.shape
header = copy.deepcopy(PCD_BINARY_TEMPLATE).format(shape[0], shape[0])
f.write(header.encode())
# f.write(binary)
import struct
for pi in points:
h = struct.pack('fff', pi[0], pi[1], pi[2])
f.write(h)
f.close()
def main():
pcd_file = r"F:\pythonProject\3D点云降维处理\1.pcd"
pcd_data = parse_pcd_data(pcd_file)
filter_cloud1 = voxel_filter(pcd_data, 0.05, "random")
to_pcd_binary("1_random.pcd", filter_cloud1)
if __name__ == '__main__':
main()
