第四章 点云预处理：从原始数据到可用特征

各位同学，欢迎来到高精地图构建的核心环节——点云预处理。说实话，这一章的内容在很多人看来可能有点“枯燥”，但我要告诉你，预处理做得好不好，直接决定了后面地图建得稳不稳。我在项目里见过太多因为预处理没到位，导致整个地图拼接失败、定位漂移的案例。所以，这一章咱们得好好聊聊。

4.1 点云滤波：去噪与降采样

原始激光雷达点云数据量巨大，一帧64线激光雷达的数据可能包含十几万个点。而且，受限于传感器物理特性，数据里总会有一些“脏东西”——比如悬浮的灰尘、雨滴、传感器自身的噪声。滤波的目的，就是把这些没用的点去掉，同时把数据量降下来，让后续处理跑得更快。

4.1.1 体素滤波（Voxel Filter）

体素滤波是我最常用的降采样方法。它的原理很简单：把三维空间划分成一个个小立方体（体素），每个体素内只保留一个点（通常是体素内所有点的重心）。

为什么要用体素滤波？你想想看，如果直接用原始点云做配准，几十万个点算一次ICP，耗时可能几百毫秒。但经过体素滤波后，点云数量可能降到几千个，计算量直接下降一个数量级。

核心参数：体素大小

体素大小决定了降采样的程度。我个人习惯：

  • 粗配准阶段：体素大小设为 0.5m - 1.0m，快速得到大致位姿
  • 精配准阶段：体素大小设为 0.1m - 0.2m，保留更多细节
  • 地图构建：体素大小设为 0.05m - 0.1m，保证地图精度
// PCL 中的体素滤波实现示例
pcl::VoxelGrid<pcl::PointXYZ> voxel_filter;
voxel_filter.setInputCloud(input_cloud);
voxel_filter.setLeafSize(0.1f, 0.1f, 0.1f);  // 体素边长 10cm
voxel_filter.filter(*filtered_cloud);

我的经验：体素大小不要设得太小，否则降采样效果不明显。我曾经在一个项目中把体素设为 0.02m，结果点云数量只减少了 20%，处理速度几乎没有提升。后来改成 0.1m，点云数量降到了原来的 1/10，配准速度提升了 5 倍以上。

4.1.2 统计滤波（Statistical Outlier Removal）

统计滤波是用来去除离群点的。它的原理是：计算每个点到其 k 个最近邻的平均距离，如果这个平均距离大于全局均值加上若干倍标准差，就认为它是离群点。

为什么会存在离群点？激光雷达打到玻璃、水面或者某些高反射表面时，会产生一些“飞点”。这些点位置明显偏离真实物体表面，如果不处理，配准时会产生很大的误差。

关键参数：

  • k（近邻数量）：通常设为 50-100
  • std_dev_mult（标准差倍数）：通常设为 1.0-3.0
// PCL 统计滤波示例
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
sor.setInputCloud(input_cloud);
sor.setMeanK(50);          // 考虑 50 个近邻
sor.setStddevMulThresh(2.0); // 2 倍标准差阈值
sor.filter(*filtered_cloud);

注意：统计滤波的参数需要根据实际场景调整。在隧道等封闭环境中，点云分布比较均匀，标准差倍数可以设小一些（1.5-2.0）。在开阔道路场景，点云分布稀疏，标准差倍数要设大一些（2.5-3.0），否则容易把正常点误删。

4.2 点云分割：理解场景结构

滤波之后，我们得到了相对干净的点云。但这时候的点云还是“一锅粥”——地面点、建筑物、车辆、树木全都混在一起。分割的目的，就是把不同物体分开，方便后续处理。

4.2.1 地面分割

地面分割是高精地图构建的第一步，也是最重要的一步。为什么？因为地面是自动驾驶车辆的主要参考平面，地图的坐标系、高程信息都依赖地面。

常用的地面分割方法有两种：

  • RANSAC平面拟合：随机采样三个点拟合平面，迭代找到包含最多内点的平面。速度快，但对坡度变化敏感。
  • 射线法（Ray Ground Filter）：按激光雷达的扫描线方向，逐条射线判断地面点。适合非平坦地面，但计算量稍大。

我个人更倾向于在高速场景用RANSAC，在城区复杂场景用射线法。曾经在一个有连续坡道的项目中，RANSAC把坡道上的点全当成了非地面点，导致地图高程信息完全错误。后来改用射线法，问题才解决。

// 基于RANSAC的地面分割（PCL实现）
pcl::SACSegmentation<pcl::PointXYZ> seg;
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);

seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations(100);
seg.setDistanceThreshold(0.2);  // 距离平面 20cm 以内的点视为地面
seg.setInputCloud(filtered_cloud);
seg.segment(*inliers, *coefficients);

4.2.2 物体分割

去掉地面后，剩下的点云就是障碍物。物体分割的目标是把这些障碍物点云聚类成一个个独立的物体——比如一辆车、一棵树、一个行人。

最常用的方法是欧几里得聚类（Euclidean Cluster Extraction）。它的原理很简单：如果两个点之间的距离小于某个阈值，就认为它们属于同一个物体。

聚类参数设置：

  • 聚类容忍度（Cluster Tolerance）：通常设为 0.5m - 1.0m。太小会把一个物体分成多块，太大会把多个物体合并。
  • 最小点数：过滤掉点数太少的噪声簇，通常设为 10-50。
  • 最大点数：过滤掉点数太多的簇（比如整面墙），通常设为 50000-100000。
// 欧几里得聚类示例
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance(0.5);  // 50cm 内的点属于同一物体
ec.setMinClusterSize(50);
ec.setMaxClusterSize(50000);
ec.setInputCloud(cloud_without_ground);
ec.extract(cluster_indices);

避坑指南：我曾经在雨雾天气测试时，发现聚类结果特别差——远处的车辆和背景的雨雾点被合并成了一个簇。后来我在聚类前加了一步：先根据点云的反射强度（Intensity）做一次粗分类，高反射的点（如车辆、路牌）和低反射的点（如雨雾）分开聚类，效果好了很多。

4.3 点云配准：对齐多帧数据

高精地图需要融合多帧激光雷达数据。配准的目的，就是找到两帧点云之间的旋转和平移关系，把它们对齐到同一个坐标系下。

4.3.1 ICP（Iterative Closest Point）

ICP是最经典的点云配准算法。它的核心思想：对于源点云中的每个点，在目标点云中找到最近邻点，然后求解一个刚体变换，使得这些点对的距离平方和最小。迭代这个过程，直到收敛。

ICP的优点是精度高，缺点是对初始位姿敏感。如果两帧点云的初始位姿偏差太大（比如超过 30° 旋转或 1m 平移），ICP很容易陷入局部最优。

// PCL 中 ICP 配准示例
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputSource(source_cloud);
icp.setInputTarget(target_cloud);
icp.setMaxCorrespondenceDistance(1.0);  // 最大对应点距离
icp.setMaximumIterations(50);           // 最大迭代次数
icp.setTransformationEpsilon(1e-8);     // 变换矩阵收敛阈值
icp.setEuclideanFitnessEpsilon(1e-5);   // 均方误差收敛阈值

pcl::PointCloud<pcl::PointXYZ> aligned_cloud;
icp.align(aligned_cloud);

Eigen::Matrix4f transformation = icp.getFinalTransformation();

注意：ICP的收敛条件要设合理。我曾经见过有人把最大迭代次数设为 1000，结果每次配准耗时 2 秒以上，完全无法满足实时性要求。实际工程中，迭代次数 30-50 次就够了，配合粗配准（如NDT）先给个好的初值，效果更好。

4.3.2 NDT（Normal Distributions Transform）

NDT是另一种常用的配准算法，它和ICP的思路完全不同。NDT先把点云空间划分成网格，每个网格内计算点云的高斯分布（均值和协方差）。配准时，不是找点对点对应关系，而是最大化源点云落在目标点云网格分布中的概率。

NDT的优点：对初始位姿不敏感，计算速度比ICP快。缺点：精度略低于ICP，且网格大小对结果影响很大。

NDT vs ICP 选择建议：

场景 推荐算法 原因
已知初始位姿（如GPS+IMU提供初值） ICP 精度高，收敛快
初始位姿未知或偏差较大 NDT 鲁棒性好，不易陷入局部最优
实时性要求高（>10Hz） NDT 计算效率更高
地图构建（离线） ICP + 回环检测 精度优先，可以接受较慢速度 
// PCL 中 NDT 配准示例
pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
ndt.setInputSource(source_cloud);
ndt.setInputTarget(target_cloud);
ndt.setResolution(1.0);           // 网格大小 1m
ndt.setStepSize(0.1);             // 步长
ndt.setTransformationEpsilon(1e-8);
ndt.setMaximumIterations(50);

pcl::PointCloud<pcl::PointXYZ> aligned_cloud;
ndt.align(aligned_cloud);

Eigen::Matrix4f transformation = ndt.getFinalTransformation();

我的经验：在实际项目中，我通常采用“粗配准+精配准”的策略。先用NDT做粗配准（网格设大一些，比如 2m），得到一个不错的初始位姿。然后用ICP做精配准（对应点距离设小一些，比如 0.5m），得到最终的精确位姿。这样既保证了鲁棒性，又保证了精度。

好了，这一章的内容就到这里。点云预处理看似基础，但每一步都藏着不少坑。滤波参数调不好，分割结果一团糟，配准初值给不对——这些我都踩过。希望你们在实际项目中，能少走一些弯路。

下一章，我们会进入高精地图的核心——车道线提取与道路要素建模。到时候见。

← 上一章 📖 返回目录 下一章 →