点云聚类算法

摘要：本文整理欧氏聚类与体素网格聚类的原理、参数和实现过程，分别按处理步骤拆解两种算法的核心代码，说明 PCL 邻域搜索、连通扩展、体素质心与原始点回填，通过代码理解聚类阈值对目标粘连、过分割和计算量的影响。

1. 点云聚类的目标

地面分割后，将空间上连通的非地面点组织成点簇，为包围盒拟合、分类与跟踪提供输入。欧氏聚类输出的是几何分组，不保证每个点簇都对应一个真实物体。

参考：

https://autowarefoundation.github.io/autoware_universe/main/perception/autoware_euclidean_cluster/

2. Euclidean Cluster（欧氏聚类）

设两个点为 $p_i=(x_i,y_i,z_i)$ 与 $p_j=(x_j,y_j,z_j)$ ，三维欧氏距离为：

d_{ij}^{3D}=\sqrt{(x_i-x_j)^2+(y_i-y_j)^2+(z_i-z_j)^2}.

不使用高度时，距离改为：

d_{ij}^{2D}=\sqrt{(x_i-x_j)^2+(y_i-y_j)^2}.

用距离阈值 $\varepsilon$ 建立邻域连接，再通过连通扩展形成点簇。A 与 B 接近、B 与 C 接近时，即使 A 与 C 超过阈值，也可能进入同一个点簇。因此阈值限制的是邻域距离，并非点簇直径。

流程：

选取一个尚未访问的点作为种子，不要求随机选取
以它为起点，找附近距离小于阈值的点
把邻近的点加入同一类
再从这些新加入的点继续找邻居
扩散到找不到为止
连通的点组成一个点簇（cluster）
从下一个尚未访问的点继续，直到所有点均被访问

核心参数：

tolerance_m是默认距离阈值

min_points_per_cluster是有效物体最小点数，滤掉噪声

max_cluster_size 是有效点簇的最大点数；PCL 的 setMinClusterSize 控制最小点数。

use_height=false 时只按 x-y 距离搜索邻域，高度差不会阻止连接，但不能由此认定这些点属于同一真实物体。

Name	Type	Description	Default	Range
max_cluster_size	integer	如果一个聚类中的点数超过该值，通常认为它不是一个正常的独立目标，可能是墙面、建筑物、大面积植被，或者多个目标粘连在一起。	1000	N/A
min_points_per_cluster	integer	点数少于该值的聚类会被认为是噪声或无效目标。	10	N/A
tolerance_m	float	在 L2 欧氏空间中，如果两个点之间的距离小于该阈值，它们可能会被归入同一个 cluster。调大：更容易把点粘成一团，但不同物体可能被合并。调小：物体更容易分开，但同一个物体可能被拆成多个 cluster。	0.7	N/A
use_height	boolean	是否使用点的 z 坐标参与聚类。false：只根据 x-y 平面距离聚类，适合俯视障碍物分组。true：使用 x、y、z 三维距离聚类，高度差也会影响聚类结果。	false	N/A

2.1 代码讲解：准备搜索点云并建立 KdTree

以下按算法流程拆解 C++ 核心片段，并非完整 ROS 节点。cloud 是已去除地面与无效点的 pcl::PointCloud<pcl::PointXYZ> 输入，单位为米。

using PointT = pcl::PointXYZ;
using Cloud = pcl::PointCloud<PointT>;
Cloud::Ptr search_cloud(new Cloud(*cloud));
if (!use_height) {
    for (auto& p : search_cloud->points) p.z = 0.0f;
}
pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);
tree->setInputCloud(search_cloud);

use_height=false 时只压平搜索副本，以三维搜索接口实现二维距离判断。原始点云保留真实高度，且副本没有删除或重排点，因此索引仍然对应。KdTree 为半径邻域查询建立搜索结构，聚类扩展时可以反复查找附近点。

2.2 代码讲解：邻域扩展与点数约束

pcl::EuclideanClusterExtraction<PointT> ec;
ec.setInputCloud(search_cloud);
ec.setSearchMethod(tree);
ec.setClusterTolerance(tolerance_m);
ec.setMinClusterSize(min_points_per_cluster);
ec.setMaxClusterSize(max_cluster_size);
std::vector<pcl::PointIndices> clusters;
ec.extract(clusters);

extract 内部完成种子选取、邻域搜索、访问标记与连通扩展。tolerance_m 控制每次半径搜索的连接范围；点簇形成后，再按最少与最多点数决定是否输出。标准 PCL 超大簇会被排除，不会因 setMaxClusterSize 自动拆分。

也可以把其内部连通扩展理解为下面的队列过程。此片段用于解释原理，不应在调用 ec.extract 后再运行一遍：

std::vector<bool> visited(search_cloud->size(), false);
for (int seed = 0; seed < static_cast<int>(search_cloud->size()); ++seed) {
    if (visited[seed]) continue;
    std::vector<int> queue{seed};
    visited[seed] = true;
    for (std::size_t head = 0; head < queue.size(); ++head) {
        std::vector<int> neighbors;
        std::vector<float> squared_distances;
        tree->radiusSearch(search_cloud->points[queue[head]], tolerance_m,
                           neighbors, squared_distances);
        for (int index : neighbors) {
            if (!visited[index]) {
                visited[index] = true;
                queue.push_back(index);
            }
        }
    }
    // queue 为一个完整连通簇，此时再按点数判断是否保留。
}

新邻居入队后继续搜索，使连接具有传递性。入队时就标记 visited，可避免一个点被重复加入；radiusSearch 返回的距离数组保存平方距离，搜索半径本身仍以米传入。

2.3 代码讲解：恢复原始三维输出

std::vector<Cloud> output;
for (const auto& indices : clusters) {
    Cloud cluster_cloud;
    pcl::copyPointCloud(*cloud, indices.indices, cluster_cloud);
    output.push_back(cluster_cloud);
}

使用原始 cloud 和聚类索引复制点，可保留真实 z。若搜索前执行了裁剪、排序或下采样，这一索引对应关系就不再自动成立。主要头文件是 <pcl/search/kdtree.h>、<pcl/segmentation/extract_clusters.h> 与 <pcl/common/io.h>。

3. Voxel Grid Based Euclidean Cluster（体素网格聚类）

流程：

用 pcl::VoxelGrid 计算每个体素的质心
对质心使用 pcl::EuclideanClusterExtraction 聚类
根据聚类后的质心，把原始点分配回各个cluster

核心参数：

voxel_leaf_size_m 表示 x、y 方向的网格尺寸。本文参考的 Autoware 二维实现将不同高度合并到同一平面网格；普通 PCL VoxelGrid 是三维体素过滤器。

tolerance_m:质心距离是否在阈值内

min_points_per_voxel：单个体素至少包含的原始点数。

max_voxels_per_cluster：单个输出点簇的体素数上限，超限时按参考版本的切分逻辑处理。

Name	Type	Description	Default	Range
tolerance_m	float	当两个被占用 voxel 的 2D 质心，即 x-y 平面质心距离小于该值时，它们会被合并到同一个 cluster。该合并具有传递性：A 接近 B，B 接近 C，则 A、B、C 可能属于同一 cluster。聚类时使用 voxel 质心，并将 z 坐标压平为 0。	0.7	N/A
voxel_leaf_size_m	float	x 和 y 方向的 voxel 尺寸 [m]。z 方向的 voxel 尺寸等效为无限大，因此该 grid 本质上是 2D 网格。同一个 x-y cell 内的所有高度点都会被压到同一个 voxel 中。该值决定聚类和切分时的平面 footprint 分辨率。	0.3	N/A
min_points_per_voxel	integer	单个 voxel 至少需要包含的原始点数。点数少于该值的 voxel 会在聚类前被丢弃，用于过滤稀疏噪声点。	1	N/A
min_points_per_cluster	integer	输出 cluster 至少需要包含的原始点数。小于该点数的 cluster 会在最终过滤阶段被丢弃。	10	N/A
max_voxels_per_cluster	integer	单个 cluster 允许包含的最大 voxel 数量。如果某个 cluster 的 voxel 数超过该值，算法会沿其主方向，也就是最长轴方向，递归二分成更小的子 cluster，直到每个子 cluster 的 voxel 数都不超过该阈值。注意：大 cluster 不会因为过大被直接丢弃，而是会被切分后输出为多个有界部分。	800	≥1
large_cluster_voxel_count_threshold	integer	大 cluster 判定阈值。当某个 cluster 的 voxel 数超过该值时，会被认为是 large cluster，并启用每个 voxel 的点数上限控制。voxel 数不超过该值的 cluster 会保留全部原始点。	65	N/A
large_cluster_max_points_per_voxel	integer	large cluster 中每个 voxel 最多保留的点数。当 cluster 的 voxel 数超过 large_cluster_voxel_count_threshold 时生效。超出该数量的点会被丢弃，以限制后续处理计算量。点会以确定性打乱顺序进行处理，使保留下来的点尽量均匀采样每个 voxel。	10	N/A
use_height	boolean	是否使用点的 z 坐标参与聚类。当前仅实现了 2D 路径，即 use_height=false。因此聚类实际基于 x-y 平面距离，高度 z 不参与聚类判断。	false	N/A

3.1 配置版本与参数单位

原笔记中的 perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml 是旧版目录示例。旧版的 tolerance、voxel_leaf_size、min_points_number_per_voxel 与 min_cluster_size，不能直接与上表的新名称混用。尤其要区分原始点数与体素数，max_cluster_size 不等同于 max_voxels_per_cluster。表中的默认值仅作参考，部署前应核对节点实际声明的参数和 YAML 层级。

3.2 体素质心与原始点映射

体素 $V_k$ 包含 $n_k$ 个点，其质心为：

\bar{p}_k=\frac{1}{n_k}\sum_{p_i\in V_k}p_i.

聚类在质心上完成，但输出通常需要原始点。因此应保留“原始点索引 → 体素 → 点簇”的映射，不能把质心点云索引直接应用到原始点云。二维网格可以使用如下键：

k_x=\left\lfloor\frac{x}{l}\right\rfloor,\qquad k_y=\left\lfloor\frac{y}{l}\right\rfloor.

$l$ 为网格尺寸。代码中应使用 std::floor；浮点数强制转整数会向零截断，对负坐标产生不同的分桶结果。

3.3 代码讲解：二维体素分桶与原始索引

下面用显式二维分桶展示“体素质心聚类 → 原始点回填”的教学实现，便于看清索引关系。它不使用普通三维 VoxelGrid 替代二维网格，也没有复刻 Autoware 的大簇切分和点数采样策略。cloud 使用上一节的 PCL 点云类型，voxel_leaf_size_m 必须为正。

using Key = std::pair<int, int>;
std::map<Key, std::vector<int>> cells;
for (int i = 0; i < static_cast<int>(cloud->size()); ++i) {
    const auto& p = cloud->points[i];
    const int ix = static_cast<int>(std::floor(p.x / voxel_leaf_size_m));
    const int iy = static_cast<int>(std::floor(p.y / voxel_leaf_size_m));
    cells[{ix, iy}].push_back(i);
}

每个 cell 保存原始点索引，同一平面网格内的不同高度点被归到一起。floor 确保负坐标也按网格边界正确分组；直接转换为整数会向零截断。这里使用 map 明确表达网格键与索引列表的对应关系，实际工程也可用哈希表。

3.4 代码讲解：生成质心并聚类

Cloud::Ptr centroids(new Cloud);
std::vector<std::vector<int>> centroid_to_raw;
for (const auto& entry : cells) {
    const auto& indices = entry.second;
    if (indices.size() < static_cast<std::size_t>(min_points_per_voxel)) continue;
    double sx = 0.0, sy = 0.0;
    for (int index : indices) {
        sx += cloud->points[index].x;
        sy += cloud->points[index].y;
    }
    PointT center;
    center.x = static_cast<float>(sx / indices.size());
    center.y = static_cast<float>(sy / indices.size());
    center.z = 0.0f;
    centroids->push_back(center);
    centroid_to_raw.push_back(indices);
}

std::vector<pcl::PointIndices> voxel_clusters;
if (!centroids->empty()) {
    pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);
    tree->setInputCloud(centroids);
    pcl::EuclideanClusterExtraction<PointT> ec;
    ec.setInputCloud(centroids);
    ec.setSearchMethod(tree);
    ec.setClusterTolerance(tolerance_m);
    ec.setMinClusterSize(1);
    ec.setMaxClusterSize(static_cast<int>(centroids->size()));
    ec.extract(voxel_clusters);
}

min_points_per_voxel 先剔除稀疏网格。centroids[k] 与 centroid_to_raw[k] 同时追加，确保第 k 个质心能找到对应的原始点。z 设为零，让后续距离判断只涉及平面坐标。

此时 PCL 聚类器看到的是质心，一个质心代表一个有效体素，最小点数 1 表示允许单体素簇。不能把 min_points_per_cluster 直接传到这里，否则约束的将是体素数。示例允许所有质心组成一个连通簇，将原始点数过滤放到回填之后。

3.5 代码讲解：回填原始点并过滤输出

std::vector<Cloud> output;
for (const auto& voxel_cluster : voxel_clusters) {
    std::vector<int> raw_indices;
    for (int centroid_index : voxel_cluster.indices) {
        const auto& indices = centroid_to_raw[centroid_index];
        raw_indices.insert(raw_indices.end(), indices.begin(), indices.end());
    }
    if (raw_indices.size() < static_cast<std::size_t>(min_points_per_cluster))
        continue;
    Cloud cluster_cloud;
    pcl::copyPointCloud(*cloud, raw_indices, cluster_cloud);
    output.push_back(cluster_cloud);
}

voxel_cluster.indices 指向质心点云，经过 centroid_to_raw 才能得到原始索引。先回填再检查 min_points_per_cluster，约束的才是原始点数；从 cloud 复制可以保留原始高度。

体素方案减少的是参与邻域搜索的点数，通常能降低聚类开销，但分桶和原始点回填也有成本。max_voxels_per_cluster 对应参考版本的额外切分逻辑，不应简单映射成 PCL 的最大点数过滤；本教学片段没有实现该切分或大簇体素内采样。除上一节的 PCL 头文件外，分桶还需 <map>、<utility>、<vector> 和 <cmath>。

4. 调参与结果检查

现象	优先检查	调整代价
相邻目标粘连	地面残留、距离阈值过大	缩小阈值可能切碎远处稀疏目标
同一目标被拆开	遮挡、点云密度、阈值过小	增大阈值可能连接不同物体
远处目标消失	最小点数、最小体素点数	降低阈值也会保留更多噪声
耗时增加	输入点数、网格尺寸、邻域密度	增大网格可能丢失细小结构

固定阈值难以同时适配近处稠密与远处稀疏点云。应分别检查近、中、远距离目标，并记录耗时和输出簇数。二维聚类可能合并上下重叠结构，三维聚类则可能拆开同一目标的高度稀疏部分。

参考：PCL 欧氏聚类教程、Autoware 聚类文档。

本篇文章受 CC BY-NC-SA 4.0 协议保护，转载请注明出处。

1. 点云聚类的目标​

2. Euclidean Cluster（欧氏聚类）​

2.1 代码讲解：准备搜索点云并建立 KdTree​

2.2 代码讲解：邻域扩展与点数约束​

2.3 代码讲解：恢复原始三维输出​

3. Voxel Grid Based Euclidean Cluster（体素网格聚类）​

3.1 配置版本与参数单位​

3.2 体素质心与原始点映射​

3.3 代码讲解：二维体素分桶与原始索引​

3.4 代码讲解：生成质心并聚类​

3.5 代码讲解：回填原始点并过滤输出​

4. 调参与结果检查​