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()