点云网格化处理【v1.2】

1.2增加功能：
x,y方向聚类为20m,点数5000
z轴方向聚类为3m，点数为5000
同时修改为先聚类在网格化，判断高程是否分层

问题：高程仍存在部分误检

from typing import List
import o3d_hdmap.open3d as o3d
import numpy as np
import glob
import pygal
import time
import ditu.topbind as tb

def read_point_cloud(pcds_path:List):
    clouds = [np.asarray(o3d.io.read_point_cloud(p).points) for p in pcds_path]
    cloud = np.vstack(clouds)
    return cloud

if __name__ == '__main__':
    start = time.time()
    # paths = glob.iglob('/data/hongyuan/work/test*.pcd', recursive=True)
    paths = glob.iglob('/data/mpcv_lspo_download_data/prod/PLEF35196-2021-11-10-17-48-39//mapping_results/lidar_features_world_opt*.pcd', recursive=True)
    points = read_point_cloud(paths)
    print(len(points))
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(points)
    # voxel_down_sample
    pcd = pcd.voxel_down_sample(voxel_size=1)
    points = np.asarray(pcd.points)
    print(len(points))
    # split_point_cloud
    box_min = points.min(axis=0)[:2]
    grid_size = 100
    indices = (np.floor(
            (points[:, :2] - box_min) / grid_size)).astype(int)
    deltas = []
    colors = np.zeros((len(points), 3))
    grids = sorted(set([tuple(t) for t in indices]))
    for ind in grids:
        indices_0 = np.where(np.logical_and(indices[:, 0] == ind[0], indices[:, 1] == ind[1]))[0]
        grid_points = points[indices_0,:]

        grid_z_points = grid_points[:,2].copy()
        grid_points[:,2] = 0
        vq = tb.VectorQuery()
        vq.add(positions=grid_points)
        segs = vq.segmentize(
            radius=20,
            max_cluster_capacity=5000,
        )
        for segment_index, s in enumerate(segs):
            grid_points_ = grid_points[s,:]
            grid_points_[:,2] = grid_z_points[s]
            grid_points_[:,:2] = np.array([0,0])
            vq1 = tb.VectorQuery()
            vq1.add(positions=grid_points_)
            segs1 = vq1.segmentize(
                radius=3,
                max_cluster_capacity=5000,
            )
            for segment_index1, s1 in enumerate(segs1):
                grid_points_1 = grid_points_[s1,:]
                z_min = np.amin(grid_points_1,0)[2]
                z_max = np.amax(grid_points_1,0)[2]
                if (z_max - z_min) > 3:
                    colors[indices_0[s[s1]],0] = 1
                else:
                    colors[indices_0[s[s1]],1] = 1
            # colors[indices_0[s], :] = (np.random.randint(0,255,3) / 255.0)
            # colors[s,:] = np.random.randint(0,256,3)
        
    pcd.points = o3d.utility.Vector3dVector(points)
    pcd.colors = o3d.utility.Vector3dVector(colors)
    o3d.io.write_point_cloud('test_1229.pcd',pcd) 
    end = time.time()
    print(f'total time：{end-start}(s)')
    print(time.strftime("%Y-%m-%d-%H_%M_%S", time.localtime()))
    print()