3D点云降维处理

# -*- coding: utf-8 -*-#
# -------------------------------------------------------------------------------
# Name:         点云降维处理
# Author:       yunhgu
# Date:         2022/1/26 11:17
# Description: 
# -------------------------------------------------------------------------------
import copy
import logging
import random
import struct
from pathlib import Path
from time import strftime, localtime, time
from traceback import format_exc

import numpy as np
from progress.bar import Bar

from pypcd import pypcd

PCD_BINARY_TEMPLATE = """VERSION 0.7
FIELDS x y z rgb
SIZE 4 4 4 4
TYPE F F F U
COUNT 1 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())
    for pi in points:
        h = struct.pack('fffI', pi[0], pi[1], pi[2], int((pi[3] * 256 + pi[4]) * 256 + pi[5]))
        f.write(h)
    f.close()


def check_exist(path):
    """
    @param path: 文件或者文件夹路径
    @return: True/False
    """
    return Path(path).exists() and path != ""


def log(log_name: str, p_type=""):
    """
    @param log_name:log文件名字
    @param p_type:输出类型
    @return:日志对象
    """
    journal = logging.getLogger(log_name)
    journal.setLevel(logging.INFO)
    log_file = f"{log_name}{strftime('%Y%m%d%H', localtime(time()))}.log"
    format_content = '%(message)s'
    if p_type == "time":
        format_content = '%(asctime)s - %(levelname)s: %(message)s'
    formatter = logging.Formatter(format_content)
    # 创建日志文件对象
    handler = logging.FileHandler(log_file, encoding="utf-8", mode="w")
    handler.setLevel(logging.INFO)
    handler.setFormatter(formatter)
    # 创建日志流对象
    console = logging.StreamHandler()
    console.setLevel(logging.INFO)
    console.setFormatter(formatter)
    # 添加文件和输出流到日志对象
    journal.addHandler(handler)
    journal.addHandler(console)
    return journal


logger = log("点云降维处理", p_type="time")


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)[:3]
    x_min, y_min, z_min = np.amin(point_cloud, axis=0)[:3]
    # 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


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 convert(file, output_file):
    pcd_data = pypcd.PointCloud.from_path(file)
    data = np.array([list(line) for line in pcd_data.pc_data])
    filter_cloud1 = voxel_filter(data, 0.5, "centroid")
    to_pcd_binary(output_file, filter_cloud1)


def main(input_path, output_path):
    pcd_file_list = [file for file in input_path.rglob("*.pcd")]
    with Bar(max=len(pcd_file_list), suffix='%(index)d/%(max)d in %(elapsed)ds (eta:%(eta_td)s)') as bar:
        for file in pcd_file_list:
            try:
                output_file = output_path.joinpath(file.relative_to(input_path))
                output_file.parent.mkdir(parents=True, exist_ok=True)
                convert(file, output_file)
            except Exception as e:
                logger.error(f"{file}运行失败，跳过这个文件。{e}\n{format_exc()}")
            finally:
                bar.next()


if __name__ == '__main__':
    while True:
        print("**** start    ****")
        # input_folder = input("请输入平台标注结果文件夹:").strip("\"")
        # output_folder = input("请输入结果保存文件夹:").strip("\"")
        input_folder = r"C:\Users\pc\Desktop\试标P\平移后pcd"
        output_folder = r"C:\Users\pc\Desktop\降低采样"
        if check_exist(input_folder) and check_exist(output_folder):
            try:
                main(Path(input_folder), Path(output_folder))
            except Exception as ee:
                logger.error(f"{format_exc()}:{ee}")
            finally:
                print("**** finished ****")
                c = input("请输入q(不区分大小写)退出,按其他任意键回车继续:")
                if c.lower() == "q":
                    break
        else:
            logger.error("输入的路径不存在,请检查后重新输入!!!")
            continue