3D点云降维处理

#! /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:pythonProject3D点云降维处理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()