点云处理－－voxel filter

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

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

# 功能：对点云进行voxel滤波
# 输入：
#     point_cloud：输入点云
#     leaf_size: voxel尺寸
def voxel_filter(point_cloud, leaf_size, mode):
    filtered_points = []
    data = point_cloud
    # 作业3
    # 屏蔽开始
    #1.计算点云的最大最小值
    D_min = data.min(0)
    D_max = data.max(0)
    #2.设定划分体素大小，计算空间划分份数1ｘ3
    D = (D_max - D_min) / leaf_size
    #3.每个点计算划分索引
    point_x, point_y, point_z = np.array(data.x), np.array(data.y), np.array(data.z)
    hx = np.floor((point_x - D[0]) / leaf_size)
    hy = np.floor((point_y - D[1]) / leaf_size)
    hz = np.floor((point_z - D[2]) / leaf_size)
    index = np.array(np.floor(hx + hy * D[0] + hz * D[0] * D[1]))   #Nｘ1

    #不进行排序，使用哈希映射进行点的筛选#

    
    #4.对索引进行排序
    data_index_point = np.c_[index, point_x, point_y, point_z]
    sort_idx = data_index_point[:, 0].argsort()
    data_index_point = data_index_point[sort_idx]
    size = data_index_point.shape[0]
    tem_point = []
    if mode == 1:
        #使用随机采样方法,索引相同的点选取最后一个为滤波输出点，相当于是随机采样了
        for i in range(size - 1):
            if(data_index_point[i][0] != data_index_point[i+1][0]):
                filtered_points.append(data_index_point[i][1:])
        #最后一个没有比较，加上
        filtered_points.append(data_index_point[size-1][1:])
        filtered_points = np.array(filtered_points)
    if mode == 2:
        #使用计算均值方法
        for i in range(size - 1):
            #判断前一个序号和后一个是否相等
            if data_index_point[i][0] == data_index_point[i+1][0]: #对于只有两个点的就会只保留一个点
                tem_point.append(data_index_point[i][1:])
                continue
            if tem_point == []:
                continue
            filtered_points.append(np.mean(tem_point, axis=0))
            tem_point = []
        filtered_points = np.array(filtered_points)
    
    #4.利用哈希表将点的索引映射到哈希容器中，注意排除冲突的点

    # 屏蔽结束

    # 把点云格式改成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 = "airplane_0001.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]) # 显示原始点云
    print('the original pointcloud size is:', point_cloud_pynt.points.shape[0])
    # 调用voxel滤波函数，实现滤波
    filtered_cloud = voxel_filter(point_cloud_pynt.points, 10, 2)
    print('the pointcloud size is:', filtered_cloud.shape[0])
    # point_cloud_o3d.points = o3d.utility.Vector3dVector(filtered_cloud)
    # 显示滤波后的点云
    # o3d.visualization.draw_geometries([point_cloud_o3d])

if __name__ == '__main__':
    main()