3. 点云数据预处理

激光雷达输出的原始点云并不是“拿来就能配准”的数据。机械式或固态激光雷达在扫描过程中，载体本身在运动，每个点被采集的时刻不同，因此同一帧点云里不同点的参考坐标系并不一致；同时，雨雾、扬尘、反光材质、多路径反射会带来离群噪声；地面点数量往往占整帧的 40%～60%，却对配准贡献很小；而单帧点云动辄数万到数十万点，直接送入配准会严重拖慢实时性。因此，去畸变 → 滤波去噪 → 地面分割 → 体素栅格下采样 → 数据组织构成了 SLAM 前端一条标准且必要的预处理流水线。

一、核心原理：为什么必须按这个顺序处理

预处理顺序不是随意的，它由“误差传播”和“计算代价”共同决定。去畸变必须最先做，因为它改变的是点的几何位置；如果先下采样再去畸变，被保留下来的代表点位置本身就是错的，后续所有几何计算都会继承这个误差。滤波去噪紧随其后，用于剔除明显不可能来自真实结构的点，避免它们污染地面分割的法向与高度统计。地面分割放在下采样之前，是因为地面点密度高、分布连续，先分割可以显著减少后续处理点数，同时保留下采样时容易被“平均掉”的稀疏障碍物边缘。体素下采样放在最后，是为了在保持几何结构的前提下统一降低点密度，为配准提供规模可控、分布均匀的输入。

二、方法流程与关键参数

去畸变的本质是运动补偿。以常见的 10 Hz 机械激光雷达为例，一帧扫描持续约 100 ms，若车辆以 10 m/s 行驶，帧首帧尾之间位移可达 1 m，若不补偿，墙面会被“拉斜”。工程上通常用 IMU 或轮速计提供的位姿，对每个点按其时间戳插值到帧起始时刻或帧中间时刻，再做坐标变换。若没有高频 IMU，也可用匀速模型近似。

# 去畸变核心逻辑（伪代码）
for each point p in scan:
    t = p.timestamp                      # 点采集时刻
    T_w_l = interpolate_pose(t)          # 插值得到该时刻雷达到世界系位姿
    p_world = T_w_l * p.local            # 变换到统一参考时刻
    p_undistorted = T_ref_inv * p_world  # 再变换回参考帧

滤波去噪常用三类手段：统计滤波（Statistical Outlier Removal）根据 K 近邻平均距离剔除离群点；半径滤波剔除邻域内点数过少的孤立点；条件滤波按高度、距离、强度阈值裁剪无效区域。地面分割主流方法有 RANSAC 平面拟合、基于射线坡度的方法，以及 Patchwork++ 这类分块地面拟合算法。体素下采样则把空间划分为固定边长立方体，每个体素内用质心或最近点代表，体素边长是精度与速度的核心权衡参数。

三、典型场景：城市道路与地下车库的差异

在城市道路场景中，地面近似平面但存在坡度与路拱，RANSAC 单平面拟合容易把缓坡误判为障碍物，此时更适合 Patchwork++ 的分块拟合。体素边长可取 0.2～0.3 m，既保留路沿和车辆轮廓，又能把单帧点数从 10 万降到 2 万左右。在地下停车场，地面平整但存在大量立柱和低矮减速带，地面分割阈值要收紧，避免把减速带并入地面；同时由于结构重复性强，下采样后仍需保留足够特征点，体素边长不宜大于 0.2 m，否则立柱边缘会被过度平滑，导致配准退化。

四、误区与复盘

误区一：跳过去畸变直接配准。在低速、低分辨率雷达上问题不明显，一旦车速提高或使用 32/64 线雷达，畸变会直接表现为配准残差偏大、建图墙面重影。复盘时应检查每帧点云的时间戳是否被正确使用，而不是只取帧头时间。

误区二：先下采样再去畸变。这是顺序性错误。下采样会丢失点的时间信息与空间细节，再去畸变只能对代表点做近似补偿，误差被固化。正确做法永远是去畸变在最前。

误区三：地面分割阈值一刀切。同一套 RANSAC 距离阈值在平地与坡道上表现差异巨大。复盘方法是统计分割后地面点的高度方差与法向分布，若地面点中混入明显非地面结构，说明阈值过松；若障碍物底部被大量削掉，说明阈值过紧。

误区四：体素边长只追求快。体素边长从 0.2 m 放大到 0.5 m，点数可能下降 80%，但路沿、栏杆、行人等细结构会被抹平，配准容易陷入局部最优。建议按场景做参数扫描，记录配准残差与耗时的折中曲线，而不是凭感觉设定。

完成上述五步后，点云已经从“原始测量”转变为“结构清晰、规模可控、索引就绪”的配准输入。去畸变保证几何正确，滤波保证干净，地面分割保证语义分层，下采样保证实时性，数据组织保证近邻搜索效率。这五者共同决定了后续 ICP、NDT、GICP 等配准算法的精度上限与实时表现。