Point cloud processing voxel filter

#  Realization voxel wave filtering , Load and verify the data set 

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

#  function ： Point cloud voxel wave filtering 
#  Input ：
# point_cloud： Enter the point cloud 
# leaf_size: voxel Size 
def voxel_filter(point_cloud, leaf_size, mode):
    filtered_points = []
    data = point_cloud
    #  Homework 3
    #  Shielding starts 
    #1. Calculate the maximum and minimum value of the point cloud 
    D_min = data.min(0)
    D_max = data.max(0)
    #2. Set partition voxel size , Calculate the number of space partitions 1ｘ3
    D = (D_max - D_min) / leaf_size
    #3. Calculate the partition index for each point 
    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

    # No sorting , Use hash mapping to filter points #

    
    #4. Sort the index 
    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:
        # Use random sampling , Select the last point with the same index as the filter output point , It is equivalent to random sampling 
        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:])
        # The last one has no comparison , add 
        filtered_points.append(data_index_point[size-1][1:])
        filtered_points = np.array(filtered_points)
    if mode == 2:
        # Use the calculated mean method 
        for i in range(size - 1):
            # Judge whether the previous serial number is equal to the next one 
            if data_index_point[i][0] == data_index_point[i+1][0]: # For those with only two points, only one point will be reserved 
                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. Use the hash table to map the index of the point into the hash container , Be careful to exclude conflicting points 

    #  End of shielding 

    #  Change the point cloud format to array, And return to 
    filtered_points = np.array(filtered_points, dtype=np.float64)
    return filtered_points
    

def main():
    # #  from ModelNet Auto index path in dataset folder , Load point cloud 
    # cat_index = 10 #  Object number , The scope is 0-39, That is, the corresponding data set 40 An object 
    # root_dir = '/Users/renqian/cloud_lesson/ModelNet40/ply_data_points' #  Dataset path 
    # cat = os.listdir(root_dir)
    # filename = os.path.join(root_dir, cat[cat_index],'train', cat[cat_index]+'_0001.ply') #  The first point cloud is used by default 
    # point_cloud_pynt = PyntCloud.from_file(file_name)

    #  Load your own point cloud file 
    file_name = "airplane_0001.ply"
    point_cloud_pynt = PyntCloud.from_file(file_name)

    #  Turn into open3d Recognizable format 
    # point_cloud_o3d = point_cloud_pynt.to_instance("open3d", mesh=False)
    # o3d.visualization.draw_geometries([point_cloud_o3d]) #  Displays the original point cloud 
    print('the original pointcloud size is:', point_cloud_pynt.points.shape[0])
    #  call voxel The filter function , Realize filtering 
    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)
    #  Displays the filtered point cloud 
    # o3d.visualization.draw_geometries([point_cloud_o3d])

if __name__ == '__main__':
    main()