3D點雲降維處理
阿新 • • 發佈:2021-08-23
不論你在什麼時候開始,重要的是開始之後就不要停止。 不論你在什麼時候結束,重要的是結束之後就不要悔恨。#! /usr/bin/env python # -*- 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()