体素滤波算法python实现

# 实现voxel滤波，并加载数据集中的文件进行验证

import open3d as o3d 
import os
import numpy as np
from pyntcloud import PyntCloud
import random

# 功能：对点云进行voxel滤波
# 输入：
#     point_cloud：输入点云
#     leaf_size: voxel尺寸
def voxel_filter(point_cloud, leaf_size,use_centroid = True):
    filtered_points = []
    # 作业3
    # 屏蔽开始
    point_cloud = np.array(point_cloud)
    #列最大
    uppers = np.max(point_cloud,axis=0)
    lowers = np.min(point_cloud,axis=0)
    print("uppers:", uppers)
    print("lowers:",lowers)
    print("(uppers - lowers)/leaf_size\n",(uppers - lowers)/leaf_size)
    #三个维度各有多少个点
    dims = np.ceil((uppers - lowers)/leaf_size)
    print("dims", dims)

    indices = (point_cloud - lowers)//leaf_size
    print("indices", indices.shape)

    #线性索引
    h_indices = indices[:,0] + indices[:,1]*dims[0] + indices[:,2]*dims[0]*dims[1];
    print("h_indices",h_indices.shape)

    for h_index in np.unique(h_indices):
        points = point_cloud[h_indices == h_index]
        if use_centroid:
            filtered_points.append(np.mean(points,axis = 0))
        else:
            filtered_points.append(random.choice(points))
    # 屏蔽结束

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

def main():
    # # 从ModelNet数据集文件夹中自动索引路径，加载点云
    # cat_index = 10 # 物体编号，范围是0-39，即对应数据集中40个物体
    # root_dir = '/Users/renqian/cloud_lesson/ModelNet40/ply_data_points' # 数据集路径
    # cat = os.listdir(root_dir)
    # filename = os.path.join(root_dir, cat[cat_index],'train', cat[cat_index]+'_0001.ply') # 默认使用第一个点云
    # point_cloud_pynt = PyntCloud.from_file(file_name)

    # 加载自己的点云文件
    file_name = "/data/home/a123/cgw/三维物体检测/数据集/ply_data_points/airplane/test/airplane_0627.ply"
    point_cloud_pynt = PyntCloud.from_file(file_name)

    # 转成open3d能识别的格式
    point_cloud_o3d = point_cloud_pynt.to_instance("open3d", mesh=False)
    # o3d.visualization.draw_geometries([point_cloud_o3d]) # 显示原始点云

    # 调用voxel滤波函数，实现滤波
    filtered_cloud = voxel_filter(point_cloud_pynt.points, 50.0,True)
    point_cloud_o3d.points = o3d.utility.Vector3dVector(filtered_cloud)
    # 显示滤波后的点云
    o3d.visualization.draw_geometries([point_cloud_o3d])

if __name__ == '__main__':
    main()