3. 点云降采样

任务目标

本案例实现基于体素网格（Voxel Grid）的点云降采样算法。输入高密度点云后，通过将空间划分为固定尺寸的立方体网格，每个网格内仅保留一个代表点（重心或中心），从而在保持物体整体轮廓与关键几何特征的前提下，显著减少点云数量，降低下游处理（如配准、分割、曲面重建）的计算负载。

核心原理与实现

体素网格滤波的核心参数为体素边长 leaf_size。算法流程：

1. 计算点云整体包围盒，按 leaf_size 划分三维网格；
2. 为每个点计算其所属体素的索引 (hx, hy, hz)；
3. 将同一体素内的点求重心（或取第一个点），作为该体素的输出点。

重心法对噪声更鲁棒，本实现采用重心法。代码不依赖任何第三方点云库，仅使用 Python 标准库与 NumPy，便于直接运行验证。

完整代码

import numpy as np

def voxel_grid_filter(points, leaf_size):
    """
    体素网格降采样
    :param points: (N, 3) 输入点云
    :param leaf_size: 体素边长
    :return: (M, 3) 降采样后点云，M <= N
    """
    if points.shape[0] == 0:
        return points.copy()
    if leaf_size <= 0:
        raise ValueError("leaf_size 必须为正数")

    # 计算体素索引
    voxel_indices = np.floor(points / leaf_size).astype(np.int64)

    # 将三维索引编码为一维，便于分组
    # 使用线性编码：idx = hx * max_yz + hy * max_z + hz
    # 为避免溢出，使用字典分组（更安全）
    from collections import defaultdict
    groups = defaultdict(list)
    for i, idx in enumerate(voxel_indices):
        groups[(idx[0], idx[1], idx[2])].append(i)

    # 对每个体素计算重心
    filtered_points = []
    for indices in groups.values():
        cluster = points[indices]
        centroid = np.mean(cluster, axis=0)
        filtered_points.append(centroid)

    return np.array(filtered_points)

if __name__ == "__main__":
    # 生成测试数据：两个平面 + 随机噪声，共 5000 点
    np.random.seed(42)
    # 平面1：z=1 的 10x10 区域
    x1 = np.random.uniform(0, 10, 2500)
    y1 = np.random.uniform(0, 10, 2500)
    z1 = np.ones(2500) * 1.0
    # 平面2：z=5 的 10x10 区域
    x2 = np.random.uniform(0, 10, 2500)
    y2 = np.random.uniform(0, 10, 2500)
    z2 = np.ones(2500) * 5.0

    cloud = np.vstack([
        np.column_stack([x1, y1, z1]),
        np.column_stack([x2, y2, z2])
    ])
    print(f"输入点云数量: {cloud.shape[0]}")

    # 执行降采样
    leaf = 0.5  # 体素边长 0.5m
    downsampled = voxel_grid_filter(cloud, leaf)
    print(f"降采样后点云数量: {downsampled.shape[0]}")

    # 验证：检查两个平面的 z 值是否仍被保留
    z_vals = np.unique(np.round(downsampled[:, 2], 6))
    print(f"保留的 z 值层级: {z_vals}")
    assert np.allclose(z_vals, [1.0, 5.0]), "关键特征丢失！"
    print("验证通过：两个平面的关键高度特征均被保留。")

运行与验证

环境依赖：仅需 Python 3.6+ 与 NumPy（pip install numpy）。

运行方式：将代码保存为 voxel_filter.py，在终端执行：

python voxel_filter.py

预期输出：

输入点云数量: 5000
降采样后点云数量: 约 800~900（具体值取决于随机分布）
保留的 z 值层级: [1. 5.]
验证通过：两个平面的关键高度特征均被保留。

判定标准：

• 程序无异常退出；
• 输出点云数量显著小于输入（本例应小于 1500）；
• 断言通过，说明 z=1 与 z=5 两个关键特征层在降采样后依然存在，证明滤波未破坏核心几何结构。

参数影响说明

leaf_size 决定降采样强度：值越大，点云越稀疏，细节丢失越多；值越小，保留细节越多但降采样效果有限。实际使用时需根据点云密度与后续算法需求调整，通常取点云平均点间距的 2~5 倍作为初始值。