接上一篇地面分割忙活了大半天我终于把激光雷达点云里的地面点给拦住了剩下的非地面点看上去干净了不少。可只要打开可视化界面一瞧头还是大的远处那几十米到底是站了个人还是停了辆车又或者是绿化带完全是一锅粥。每个点都只知道自己坐标是多少并不知道自己和旁边哪个点属于同一个目标。激光雷达点云障碍物检测做到这一步拼的已经不是“去噪”和“去掉地面”了而是要把同一目标上的点归堆把不同目标切开。我这一系列笔记写到现在前两篇分别讲了点云预处理和地面分割这篇专门来写欧式聚类Euclidean Cluster Extraction。这是点云感知里最经典、最直观、也最适合入门的聚类方案几乎所有做激光雷达避障、目标检测的开发者都绕不开它。文章内容基于我常用的一套点云处理Pipeline适用于无人小车、机器人、安防雷达等场景。如果你正打算把“一锅粥”一样的非地面点云切成一个一个的障碍物这篇应该能帮你少踩几个坑。1. 聚类在点云障碍物检测链路中的位置把一锅粥变成一个个目标1.1 为什么做完地面分割还不够很多新手容易产生一个误解地面分割之后剩下的点不就是障碍物了吗还真不是。地面分割只是把道路平面附近的点剔除掉剩下的点云仍然是无组织的散点集合。一辆车可能由车顶、车窗、侧面、轮眉等多块反射点组成一个行人从上到下反射点也散落在不同高度。如果不做聚类你面对的是几万个彼此之间没有归属关系的点后续无论是求目标尺寸、算质心还是做跟踪匹配都无从下手。我习惯把整个点云障碍物检测链路拆成这样数据采集 → 去噪滤波 → 地面分割 → 障碍物聚类 → 目标属性提取 → 帧间跟踪。欧式聚类在链路中扮演的是“分拣员”角色把地面分割后的非地面点按照空间上的远近亲疏拆成若干个有独立ID的点簇每个点簇对应一个候选障碍物。1.2 聚类的输入输出到底是什么说得直白点欧式聚类的输入是一堆无组织的点云输出是一组“点索引集合”。每个集合代表一个障碍物例如cluster[0]是行人、cluster[1]是左侧的车、cluster[2]是垃圾桶。用代码语言来描述就是输入pcl::PointCloudpcl::PointXYZ::Ptr杂乱的非地面点云。输出std::vectorpcl::PointIndices其中PointIndices内部是一个std::vectorint存的是原始点云里的索引号。为什么不是直接输出子点云因为索引方式零拷贝、省内存我们想提取某个障碍物的点云时按索引挑出来就行不必复制整片点云。这个习惯在后续做目标级处理时特别方便。1.3 为什么选欧式聚类而不是别的聚类我第一次接触点云聚类的时候其实也纠结过要不要用KMeans或者别的算法后来发现大部分工程实践都落在欧式聚类的框架内。原因很实在。KMeans要求预先知道目标个数K这在无人驾驶场景里根本不可行——你永远不知道前面有几辆车、几个人。KMeans对初始中心敏感输出结果极不稳定点云这种密度不均的数据它很难吃住。而真正的“同类竞品”其实是DBSCAN和区域生长。欧式聚类在思路上就是DBSCAN的简化特例固定半径搜索邻域按密度连通性把点抱团成簇。PCL里实现的EuclideanClusterExtraction本质上就是“固定半径区域生长 最小/最大点数过滤”。它的好处是没有全局密度假设点稀疏的地方照样能处理实现又足够简单实时性有保证。如果你问我什么时候不用欧式聚类只有一种情况点云密度极度不均匀远距离点稀疏到根本连不成簇的时候。这种情况需要先做距离自适应阈值或者干脆换更复杂的栅格聚类、深度图分割方案。但在绝大多数近中距离车载/机器人场景欧式聚类依然是性价比最高的选择。2. 欧式聚类的核心原理KD-Tree加速的近邻搜索加区域生长2.1 一句话理解以点会友抱团成簇欧式聚类的核心思想用四个字就能概括抱团成簇。它的具体做法是从点云里随便挑一个种子点找到它半径r范围内的所有邻居然后把这些邻居也当作种子继续找邻居直到再也找不到新邻居为止。这一整套流程走完所有被“传染”到的点就构成一个点簇。这个过程很像朋友圈扩散你认识了AA认识了BB又认识了C最终你会发现一个社交圈子。点云聚类也一样空间上离得近的点会通过层层传递连接到一起而中间隔着很大空隙的两群点则永远不会被连起来。2.2 KD-Tree在这里到底加速了什么我得单独说说KD-Tree因为很多教程把它当成点缀提一嘴就过去了但它在欧式聚类里其实是命根子。你想干“找半径r内所有邻居”这件事最笨的办法是把当前种子点和点云里每一个点都算一遍距离。假设点云有n个点每次搜索都要遍历全图整个聚类过程复杂度会到O(n^2)甚至更高。我实测过几万点的场景这种纯暴力做法在CPU上能卡到几十毫秒级别而且点越多越离谱。KD-Tree解决的就是“加速邻居搜索”的问题。它按坐标维度不断切分空间把点云组织成一颗二叉树。搜索某个点半径范围内的邻居时只需要沿着树的分支往下走大部分不相关的分支可以直接剪掉平均查询复杂度降到O(log n)整体聚类流程变成O(n log n)。说句大实话没有KD-Tree欧式聚类是不可能在实时系统里用的。PCL里对应的是pcl::search::KdTreepcl::PointXYZ基于FLANN实现。需要注意不同版本的PCL里有的叫pcl::KdTreeFLANN新版本统一到pcl::search::KdTree都能用。2.3 区域生长的完整流程拆解完整流程我拆成六步你可以对照着理解为输入点云建一棵KD-Tree方便后续快速查邻居。准备一个“访问标记”数组长度为点云点数初始全部为未访问。从点云中取一个未访问点作为种子开一个新的簇。以该种子为中心通过KD-Tree搜索半径r内的所有邻居点遍历这些邻居凡是未访问的标记为已访问并加入当前簇同时塞进一个队列。从队列中取出下一个点重复第4步直到队列为空。当前簇生长结束回到第3步找下一个未访问点作为新种子直到所有点都有归属。第4步和第5步是经典BFS广度优先搜索写法也可以用DFS递归只是要注意递归深度可能导致栈溢出我一般用队列稳一点。下面这段Python伪代码更直白适合你快速理解def euclidean_cluster(points, radius, min_points, max_points): tree build_kdtree(points) visited [False] * len(points) clusters [] for i in range(len(points)): if visited[i]: continue queue [i] visited[i] True cluster [] while queue: idx queue.pop() cluster.append(idx) neighbors tree.radius_search(points[idx], radius) for nb in neighbors: if not visited[nb]: visited[nb] True queue.append(nb) if min_points len(cluster) max_points: clusters.append(cluster) return clusters看到没真正的算法核心就这么点东西。但就是这点东西把“人和人之间隔了1米”和“人和车之间隔了5米”这种几何距离信息变成了目标切分的依据。3. 三个参数的工程取值距离容差、最少点数、最多点数PCL的pcl::EuclideanClusterExtraction有三个核心参数我逐一说说它们到底管什么以及我平时怎么取值。3.1 距离容差setClusterTolerance这是最要命的参数。它决定“多近才算一个目标”单位是米。阈值设小了同一个障碍物上的点会碎成好几块阈值设大了两辆并排停着的车会被连成一个巨型簇。我踩过一个很经典的坑把tolerance设成0.2m结果一辆2米宽的车被切成了五六块——车头和车尾的点距离超过了0.2m聚不到一起。后来放宽到0.5m才勉强整。但0.5m在近处又容易把行人、自行车这种间距小的目标黏在一起。不同雷达、不同距离下这个值的合理范围差很多。我的建议是从小到大试先0.3m试再0.5m、0.8m肉眼观察聚类结果在哪一档“既不碎也不黏”。3.2 最少点数setMinClusterSize和最多点数setMaxClusterSize这两个参数是按点数过滤作用完全不同。最少点数主要是滤噪点。激光雷达点云里总会有个别孤立点可能是灰尘、飞鸟也可能是传感器本身的一个坏点。把这些孤立点聚类常常就是一个1~3点的超级小簇这显然不是目标物。设一个最小点数阈值比如5点或者10点这些小簇直接扔掉。最多点数主要是防止“把整面墙截成一个目标”。比如你的车正好停在路边一堵很长的墙旁边墙和车可能在空间上连成一整片生成一个上万点的大簇。如果不对最大点数做限制后处理做的目标框就会变成一个横跨几十米的长条严重影响判断。把最大点数设成50000之类超过就直接不输出。3.3 参数速查表我常用的一组初始取值我以16线机械式激光雷达、检测距离50米内的无人车场景为例给一组可以直接抄作业的初始值参数初值说明聚类半径0.5 m中近距离通用需按雷达线数和安装高度微调最小点数5滤掉孤立噪点最大点数50000避免墙面/大型物体被聚成一个巨型簇体素降采样前0.05 m数据量太大时先用小体素降采样体素降采样后0.1~0.2 m降采样程度会影响聚类半径见第5章这组参数对大多数室内外机器人、园区无人车都够用。如果你用的是32线、64线雷达近距离点非常密聚类半径可以收窄到0.2~0.3m如果你用的是单线雷达点密度低半径往往要放到0.5~1m才有效。4. 跑通欧式聚类的完整代码与结果解读4.1 基于PCL的C实现开箱即用如果你直接基于PCL开发代码大概长这样#include pcl/point_types.h #include pcl/point_cloud.h #include pcl/search/kdtree.h #include pcl/segmentation/extract_clusters.h #include vector std::vectorpcl::PointCloudpcl::PointXYZ::Ptr segmentClusters(const pcl::PointCloudpcl::PointXYZ::Ptr input_cloud) { // 1. 建KD-Tree pcl::search::KdTreepcl::PointXYZ::Ptr tree( new pcl::search::KdTreepcl::PointXYZ); tree-setInputCloud(input_cloud); // 2. 配置欧式聚类 std::vectorpcl::PointIndices cluster_indices; pcl::EuclideanClusterExtractionpcl::PointXYZ ec; ec.setClusterTolerance(0.5); // 聚类半径, 单位米 ec.setMinClusterSize(5); // 最少点数 ec.setMaxClusterSize(50000); // 最多点数 ec.setSearchMethod(tree); ec.setInputCloud(input_cloud); ec.extract(cluster_indices); // 3. 按索引提取每个簇的点云 std::vectorpcl::PointCloudpcl::PointXYZ::Ptr clusters; for (const auto indices : cluster_indices) { pcl::PointCloudpcl::PointXYZ::Ptr cluster( new pcl::PointCloudpcl::PointXYZ); for (int idx : indices.indices) { cluster-points.push_back(input_cloud-points[idx]); } cluster-width cluster-points.size(); cluster-height 1; cluster-is_dense true; clusters.push_back(cluster); } return clusters; }这套代码我在几个项目里反复用过稳定可靠。唯一要注意的是新版PCL里setSearchMethod接收的是pcl::search::Search的派生类如果遇到编译报错多半是头文件没包含全把pcl/search/kdtree.h换成pcl/search/impl/kdtree.hpp之类的情况很少见大概率是你引用的头文件路径不对。4.2 手写Python实现便于理解与调试我不太建议在正式项目里用Python做实时聚类但用它来理解算法、调试参数非常友好。基于numpy和scipy的cKDTree几十行就能实现一个够用的欧式聚类import numpy as np from scipy.spatial import cKDTree from collections import deque def euclidean_cluster(points, radius0.5, min_points5, max_points50000): points: (N, 3) numpy数组 返回: 每个簇的点索引列表 tree cKDTree(points) visited np.zeros(len(points), dtypebool) clusters [] for i in range(len(points)): if visited[i]: continue queue deque([i]) visited[i] True cluster [] while queue: idx queue.popleft() cluster.append(idx) neighbors tree.query_ball_point(points[idx], radius) for nb in neighbors: if not visited[nb]: visited[nb] True queue.append(nb) if min_points len(cluster) max_points: clusters.append(cluster) return clusters这里scipy.spatial.cKDTree的query_ball_point作用就是查询半径内的邻居点索引底层同样是一棵KD-Tree。直接读这份代码你对欧式聚类的理解会比看任何博客都深刻。4.3 聚类结果怎么验证与可视化跑通聚类只是第一步关键是怎么快速判断结果对不对。我常用的验证手段就两个一是可视化。把聚类结果用不同颜色随机渲染出来观察有没有“一个障碍物被切成两半”或者“两个障碍物被连成一体”。这一步可比看数值参数直观多了。二是统计量检查。对每个簇打印点数、质心、能装下该簇的最小立方体尺寸。行人簇点数通常在几十到两三百车簇可能上千如果有一个簇的最小立方体长宽超过10米大概率是阈值过宽把墙面和树连一起了。关于可视化工具实时调试我用PCL Visualizer或者rviz离线分析用Open3D都很方便。关键是要养成“每次改完参数先可视化验证再跑批处理”的习惯。5. 实战调优三个最典型的坑和对应解法这部分算是这篇文章里我最想聊的干货。欧式聚类写起来简单调起来全是细节而且很多坑不看效果图根本想不到。5.1 固定半径的远近视差距离自适应阈值怎么定上面给的固定0.5m阈值只适合中等距离。我用16线雷达时发现一个明显规律10米内的点相邻点间距只有0.1~0.3米0.5m阈值能把车聚得很好但到了40米开外相邻光束间的点间距能拉大到0.8~1.2米0.5m的阈值根本连不到邻居远处的车会直接碎成几个小簇甚至被当作噪点滤掉。解决思路是用“距离自适应阈值”不再用固定半径而是让半径随着目标点到雷达的水平距离增大而线性增大。模糊的规律就是“近处密远处疏阈值跟着距离走”。我常用的一组经验公式是r 0.3 0.02 * distance其中distance是当前种子点到雷达中心的水平距离单位米。近处0.3~0.5m50米外约1.3m基本能适配大多数16线雷达的密度变化。注意这个“距离自适应”的修改在PCL原生EuclideanClusterExtraction里没法直接配置因为它内部用的是一棵静态树加固定阈值。我在实际项目里是换了更灵活的做法把点云按距离分带或者自己写一个支持动态半径的聚类循环。还有一种折中方案就是把阈值设成1m近处靠降采样把密度降下来避免过度合并。5.2 地面分割残留带来的“连体婴”问题地面分割不可能100%干净。贴近地面的路沿、缓坡、反光路面残留点往往会在障碍物脚下形成一层“尾巴”。这在聚类里很讨厌车和路沿在空间上通过这层尾巴连在一起聚类结果直接把路沿也拉进了车的点簇形成“连体婴”。我碰到的典型情况是一辆靠边停的车聚类之后包围框比实际长度大了快两米——里面混进了几十个路沿点。治本的办法是把地面分割参数调严比如提高RANSAC地面拟合的次数阈值但更稳健的工程做法是聚类之前对非地面点做高度过滤把传感器安装高度附近比如离地高度低于0.1m的残留点直接剔除。聚类前的“用高度再清一次”是我强烈建议加进Pipeline里的步骤成本极低效果立竿见影。5.3 体素降采样和聚类半径的连带效应点云数据量一大免不了先用体素网格降采样跑一遍。但降采样参数和聚类半径是强耦合的很多人在这里翻车。体素太大近处密集的点也被抽稀一辆车可能被分成好几块体素太小数据不降反增聚类耗时爆炸。我通常先在0.05~0.1m之间选体素尺寸然后把聚类半径和体素尺寸配平体素0.1m时聚类半径不要小于0.4m体素0.2m时聚类半径至少0.6m。降采样越狠聚类半径就要越大否则远处目标一定碎。另外提醒一句体素降采样会改变点密度分布最终参数一定是在“降采样后的点云”上标定而不是降采样前。5.4 性能瓶颈KD-Tree重建开销与多帧复用最后说性能。欧式聚类的耗时大头其实在KD-Tree构建而不是搜索本身。一帧5万点建KD-Tree大约耗时几毫秒到十几毫秒如果放到嵌入式平台会更紧张。我的优化建议是如果点云帧率很高且场景没大变可以考虑每隔几帧重建一次KD-Tree中间帧用上一帧的树做近邻查询代价是边界精度略有下降收益是CPU占用明显降低。多线程并行处理不同区域按距离带或扇形区分开聚类最后再合并能吃到多核红利。提前用体素降采样减少点数比任何算法优化都直接。我实测过一帧3万点的典型园区场景优化前耗时20ms左右降采样到1万点后耗时压到5ms以内实时性完全不一样。6. 聚类之后接什么包围框、目标属性与帧间跟踪聚类永远不是终点。拿到一个个点簇之后下一步我一般做三件事6.1 从点簇到包围框AABB还是OBB最简单的是AABB轴对齐包围盒直接求点簇在x/y/z三个方向上的最大值和最小值。PCL里一句pcl::getMinMax3D就能算。但AABB的问题在于不会跟随目标朝向旋转——一辆斜着停在路边的车AABB会长出一个很大的“斜切角”测出来的尺寸比实际车身大很多。更实用的是OBB有向包围盒。做法是对点簇做PCA主成分分析取前三个主方向作为包围盒的轴再投影求长宽高。对于车这种结构比较规则的目标OBB的尺寸和真实车身非常接近。PCL里有pcl::MomentOfInertiaEstimation可以方便计算思路清晰代码也不长。6.2 目标属性质心、尺寸、点数每个簇的质心就是目标的粗略位置把簇内所有点坐标取平均即可。这里的坑是点云反射不均匀车的近侧面点比远侧面密很多简单平均出来的质心会偏向近侧面。如果想要更准的位置估计建议先求OBB中心而不是点云平均。目标尺寸用OBB的长宽高点数大小则可以作为目标类别的辅助判断特征。不要把点数直接当置信度但它和体积、距离组合起来能帮你过滤掉非常多的误检。6.3 为帧间跟踪做的准备最后聚类后我还会给每个簇分配一个稳定ID临时ID然后把它和上一帧的簇做最近邻匹配或匈牙利匹配从而得到跨帧稳定的目标ID为后续速度估计和轨迹预测做准备。这里有一个经验之谈聚类结果的质量直接影响跟踪系统的上限。如果聚类经常碎、经常连体那么后面无论跟踪算法多高级都会一帧一个跳变根本稳不住。所以千万不要跳过第5章那几个调优步骤参数调不顺后患无穷。最后分享两个个人心得一个是欧式聚类看起来是“最没技术含量”的一步但它在整个点云感知里扮演的角色有点像掌勺师傅手里的盐。盐放少了菜没味放多了齁嗓子你必须根据食材雷达型号、锅灶算力平台、火候场景复杂度来一遍遍地试。我到现在也不敢说一组参数能吃遍所有场景每次换传感器或者换车位都要重新花半小时做参数标定。另一个是别把欧式聚类当成“标准步骤”就闭眼用。你只要理解了它背后的近邻搜索和区域生长就可以按自己的场景任意魔改加距离自适应阈值、做多尺度聚类、甚至把点云先投影到栅格再做领域生长。我在做某些稀疏雷达项目时就是在欧式聚类框架上做了不少改造才保住性能。如果这篇笔记能让你少走一点弯路那我自己踩过的那些坑也算值了。