4. 点云预处理：点云滤波、配准与分割

各位同学，欢迎来到第四章。这一章我们聊聊点云预处理。说实话，这是整个高精地图建图流程里最“脏”最“累”的活，但也是决定后续算法成败的关键。我见过太多项目，前面定位算法写得再花哨，点云预处理没做好，最后建出来的地图全是噪点，根本没法用。

点云预处理说白了就三件事：滤波、配准、分割。咱们一个一个来拆解。

4.1 点云滤波：把“脏数据”洗干净

激光雷达扫出来的点云，原始数据里啥都有。有噪声点、有离群点、有冗余点。你想想看，一帧64线激光雷达的数据可能有几十万个点，要是全拿来算，计算量直接爆炸。所以滤波的目的很明确：去噪、降采样、保留有效信息。

4.1.1 体素滤波（Voxel Filter）

体素滤波是我个人最常用的降采样方法。它的思路很简单：把三维空间切成一个个小立方体（体素），每个体素里只保留一个点（通常是重心点）。

举个例子，一帧点云有10万个点，你设置体素大小为0.1米，可能就只剩下1万个点了。计算量直接降一个数量级。

// C++ 示例：PCL 体素滤波
pcl::VoxelGrid<pcl::PointXYZ> voxel_filter;
voxel_filter.setInputCloud(cloud_input);
voxel_filter.setLeafSize(0.1f, 0.1f, 0.1f); // 体素边长0.1米
voxel_filter.filter(*cloud_filtered);

4.1.2 统计滤波（Statistical Outlier Removal）

统计滤波是用来干掉离群点的。什么叫离群点？就是那些孤零零飘在空中的噪点。比如激光打到玻璃上产生的反射噪点，或者打到远处小物体上的零星点。

它的原理是：计算每个点周围k个邻居的平均距离，如果这个距离明显大于全局平均值（超过几个标准差），就判定为离群点，直接干掉。

// C++ 示例：PCL 统计滤波
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
sor.setInputCloud(cloud_filtered);
sor.setMeanK(50);          // 邻居点数
sor.setStddevMulThresh(1.0); // 标准差倍数
sor.filter(*cloud_clean);

4.2 点云配准：把碎片拼成完整地图

激光雷达一帧只能扫到局部区域。要建完整的高精地图，得把多帧点云拼到一起。这个过程就叫配准。说白了就是：找到两帧点云之间的旋转和平移矩阵。

4.2.1 ICP（Iterative Closest Point）

ICP是最经典的配准算法。它的逻辑很直观：对于源点云中的每个点，在目标点云中找到最近邻点，然后计算一个变换矩阵让这些点对的距离之和最小。迭代直到收敛。

嗯，这里要注意：ICP对初始位姿非常敏感。如果两帧点云初始位置差太远，它很容易陷入局部最优。我一般先用IMU或轮速计给个粗略的初始位姿，再用ICP做精配准。

// C++ 示例：PCL ICP 配准
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputSource(cloud_source);
icp.setInputTarget(cloud_target);
icp.setMaxCorrespondenceDistance(0.5);  // 最大匹配距离
icp.setMaximumIterations(100);          // 最大迭代次数
icp.setTransformationEpsilon(1e-8);     // 收敛阈值
icp.align(*cloud_registered);

4.2.2 NDT（Normal Distributions Transform）

NDT是另一种配准方法，我个人更喜欢它。它不直接匹配点，而是把点云用正态分布来表示。说白了，就是把空间分成网格，每个网格里计算点的均值和协方差，然后通过优化让两帧点云的概率分布对齐。

NDT比ICP快，而且对初始位姿不那么敏感。我在城市道路场景里基本都用NDT，ICP只在需要高精度的小范围场景里用。

// C++ 示例：PCL NDT 配准
pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
ndt.setInputSource(cloud_source);
ndt.setInputTarget(cloud_target);
ndt.setResolution(1.0);          // 网格分辨率
ndt.setStepSize(0.1);            // 优化步长
ndt.setMaximumIterations(50);    // 最大迭代次数
ndt.align(*cloud_registered);

4.3 点云分割：从点云中“抠”出地面和物体

配准完的点云，里面啥都有：地面、建筑物、车辆、行人、树木...要做高精地图，得先把地面分割出来，再把动态物体（车、人）去掉，只保留静态结构。

最常用的方法是RANSAC平面拟合。它的思路是：随机选三个点拟合一个平面，然后统计有多少点在这个平面附近。重复这个过程，找到包含点数最多的那个平面——那就是地面。

// C++ 示例：PCL RANSAC 平面分割
pcl::SACSegmentation<pcl::PointXYZ> seg;
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);

seg.setInputCloud(cloud_registered);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setDistanceThreshold(0.1);  // 点到平面的距离阈值
seg.segment(*inliers, *coefficients);

4.4 本章知识体系

下面这张图是我自己画的，把本章的核心逻辑串起来了。你仔细看看，就能明白预处理流程的上下游关系。

你看，整个流程是串行的：原始点云先滤波去噪降采样，再配准拼接成完整场景，最后分割提取出地面和静态物体。每一步的输出都是下一步的输入。我在实际项目中，经常在滤波和配准之间来回调参，因为滤波参数直接影响配准精度。

好了，这一章就到这里。点云预处理是基础中的基础，你把这些搞透了，后面建图和定位才能稳。记住：数据质量决定算法上限。