4、点云数据处理：点云滤波与降采样、点云配准（ICP、NDT）、点云语义分割、地面点提取

点云数据，说白了就是激光雷达打出去的一堆点。每个点都带着三维坐标，有的还带着反射强度。但原始点云直接拿来用？那基本没法干活。噪声多、数据量大、还有各种乱七八糟的离群点。我刚开始接触高精地图时，就吃过这个亏——直接把原始点云扔进配准算法，结果跑了半小时还没收敛。

所以，点云处理的第一步，永远是清洗和瘦身。咱们一步步来。

4.1 点云滤波与降采样

滤波的目的很单纯：去掉噪声，保留有效信息。降采样呢？就是让数据量小一点，跑得快一点。你想想看，一帧64线激光雷达的数据，动辄几十万个点。全量处理，实时性根本扛不住。

4.1.1 常见滤波方法

我个人习惯，先做直通滤波。什么意思？就是根据坐标范围直接切一刀。比如我只关心车前50米、左右20米、高度-2到5米的范围。超出这个范围的点，直接扔掉。简单粗暴，但有效。

// 伪代码示例：直通滤波
PointCloud passThroughFilter(PointCloud input, float xMin, float xMax, float yMin, float yMax, float zMin, float zMax) {
    PointCloud output;
    for (auto point : input.points) {
        if (point.x >= xMin && point.x <= xMax &&
            point.y >= yMin && point.y <= yMax &&
            point.z >= zMin && point.z <= zMax) {
            output.push_back(point);
        }
    }
    return output;
}

然后是统计滤波。这个我特别喜欢用。它的原理是：每个点周围找K个邻居，算平均距离。如果某个点的平均距离明显大于整体均值，那它大概率是离群点。我在项目中遇到过，一辆车从旁边开过，溅起一堆尘土，统计滤波直接把这些噪点干掉了。

半径滤波也常用。指定一个半径，如果某个点周围邻居数量少于阈值，就干掉。这个对孤立噪点特别有效。

4.1.2 降采样策略

降采样，我首推体素滤波。把空间划分成一个个小立方体（体素），每个体素内只保留一个点——通常是体素内所有点的重心。体素大小很关键。设成0.1米，点云密度降为原来的十分之一，但几何特征基本不变。设成0.5米？嗯，车道线可能就没了。

体素大小	点云数量（原始10万点）	适用场景
0.05m	约5万点	精细建模、语义分割
0.10m	约1万点	配准、定位
0.20m	约3000点	快速预览、粗配准

4.2 点云配准：ICP与NDT

配准，就是把不同位置采集的点云对齐。L3自动驾驶里，车辆在移动，激光雷达不断扫到新的局部点云。怎么把这些局部点云拼成完整的地图？靠的就是配准。

4.2.1 ICP（迭代最近点）

ICP是最经典的配准算法。原理很简单：找两个点云中距离最近的点对，算一个旋转平移矩阵，让这些点对的距离最小。然后迭代，直到收敛。

但ICP有个大坑——对初始位置敏感。如果两帧点云初始偏差太大，ICP很容易掉进局部最优。我曾经调试一个项目，两帧点云差了5米，ICP死活配不上。后来加了初值估计，先用手动对齐一下，再跑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> final_cloud;
icp.align(final_cloud);
Eigen::Matrix4f transformation = icp.getFinalTransformation();

4.2.2 NDT（正态分布变换）

NDT是另一种思路。它不直接匹配点对，而是把点云用概率分布来描述。把空间划分成网格，每个网格内计算点云的正态分布。配准时，找的是让源点云在目标点云的概率分布上似然最大的变换。

NDT比ICP有什么好处？速度快，对初值不敏感。我个人的习惯是：先用NDT做粗配准，再用ICP做精配准。这样既快又准。

// PCL中NDT的使用示例
pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
ndt.setInputSource(source_cloud);
ndt.setInputTarget(target_cloud);
ndt.setResolution(1.0);           // 网格分辨率
ndt.setStepSize(0.1);             // 步长
ndt.setTransformationEpsilon(1e-8);
ndt.setMaximumIterations(30);

pcl::PointCloud<pcl::PointXYZ> final_cloud;
ndt.align(final_cloud);
Eigen::Matrix4f transformation = ndt.getFinalTransformation();

特性	ICP	NDT
对初值敏感度	高	低
计算速度	慢（点对搜索）	快（概率分布）
精度	高（收敛时）	中等
适用场景	精配准	粗配准、实时定位

4.3 点云语义分割

语义分割，就是给每个点打标签：这个是路面，那个是车辆，远处的是建筑。L3自动驾驶里，语义分割是理解环境的基础。

传统方法靠手工特征。比如用点云的局部法向量、曲率、高度差来区分地面和障碍物。但说实话，效果一般。现在主流都是用深度学习。

PointNet++是我用得最多的网络。它直接处理原始点云，不需要转成体素或图像。核心思想是：分层采样，局部聚合。先在最粗的尺度上提取全局特征，再逐步细化到局部细节。

还有一个叫RandLA-Net的，效率更高。它用随机采样代替最远点采样，速度提升了一个数量级。我在实车部署时就用它，能在嵌入式平台上跑到10Hz以上。

4.4 地面点提取

地面点提取，说白了就是把路面上的点单独拎出来。为什么重要？因为地面是参考平面，很多后续处理都依赖它——比如障碍物检测、可行驶区域判断。

最经典的方法是RANSAC平面拟合。随机选三个点，算一个平面，然后看有多少点在这个平面上。迭代多次，找到点数最多的那个平面，就是地面。

// PCL中RANSAC平面拟合提取地面
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.1);  // 点到平面的距离阈值
seg.setInputCloud(input_cloud);
seg.segment(*inliers, *coefficients);

// inliers中的索引就是地面点

但RANSAC有个问题——对坡度敏感。如果路面有上下坡，一个平面就搞不定了。我遇到过这种情况，在山区道路上，RANSAC把上坡路面当成了障碍物。后来改用区域生长法：从种子点开始，根据法向量和高度差，逐步向外生长，把连续的地面区域都找出来。

还有一个实用技巧：先做地面提取，再做语义分割。地面点提取出来后，剩下的点就是非地面点，语义分割只需要处理这些点，计算量直接减半。

好了，点云处理的这几个核心模块，咱们就聊到这儿。滤波降采样是基础，配准是建图的关键，语义分割和地面提取是理解环境的前提。下一章，咱们聊聊怎么把这些处理好的点云，真正用在高精地图的构建和更新中。