python 点云下采样

一个简单的点云下采样程序

import open3d as o3d
import numpy as np
import random


def voxel_filter(point_cloud, leaf_size, filter_mode):
    filtered_points = []
    # step1 计算边界点
    x_max, y_max, z_max = np.amax(point_cloud, axis=0)  # 计算 x,y,z三个维度的最值
    x_min, y_min, z_min = np.amin(point_cloud, axis=0)
    # step2 确定体素的尺寸
    size_r = leaf_size
    # step3 计算每个 volex的维度
    Dx = (x_max - x_min) / size_r
    Dy = (y_max - y_min) / size_r
    Dz = (z_max - z_min) / size_r
    # step4 计算每个点在volex grid内每一个维度的值
    h = list()
    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_indice = np.argsort(h)  # 提取索引
    h_sorted = h[h_indice]  # 升序
    count = 0  # 用于维度的累计
    np.seterr(divide='ignore', invalid='ignore')  # 忽略除法遇到无效值的问题
    # 将h值相同的点放入到同一个grid中，并进行筛选
    for i in range(len(h_sorted) - 1):  # 数据点
        if h_sorted[i] == h_sorted[i + 1]:  # 当前的点与后面的相同，放在同一个volex grid中
            continue
        else:
            filter_mode == "random"  # 随机滤波
            point_idx = h_indice[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]:  # 当前的点与后面的相同，放在同一个volex grid中
            continue
        else:
            filter_mode == "centroid"  # 均值滤波
            point_idx = h_indice[count: i + 1]
            filtered_points.append(np.mean(point_cloud[point_idx], axis=0))  # 取同一个grid的均值
            count = i

    # 把点云格式改成array，并对外返回
    filtered_points = np.array(filtered_points, dtype=np.float64)
    return filtered_points


def main():
    # 加载自己的点云文件
    pcd = o3d.io.read_point_cloud("bunny.pcd")
    points = np.asarray(pcd.points)
    # 调用voxel滤波函数，实现滤波
    filtered_cloud_1 = voxel_filter(points, 0.01, "random")  # random
    point_cloud_o3d_filter_1 = o3d.geometry.PointCloud()
    point_cloud_o3d_filter_1.points = o3d.utility.Vector3dVector(filtered_cloud_1)
    point_cloud_o3d_filter_2 = o3d.geometry.PointCloud()
    filtered_cloud_2 = voxel_filter(points, 0.01, "centroid")  # centroid
    point_cloud_o3d_filter_2.points = o3d.utility.Vector3dVector(filtered_cloud_2)
    # 显示滤波前后的点云
    o3d.visualization.draw_geometries([pcd])  # 显示原始点云
    o3d.visualization.draw_geometries([point_cloud_o3d_filter_1])
    o3d.visualization.draw_geometries([point_cloud_o3d_filter_2])


if __name__ == '__main__':
    main()