4. 点云基础
在机器人三维语义地图构建中，点云是最原始、最直接的空间观测形式。激光雷达、深度相机、结构光传感器输出的都是一批离散的三维点，每个点通常包含坐标 (x, y, z)，还可能附带强度、颜色、法向量、时间戳等属性。理解点云的数据结构、滤波、降采样、特征提取与配准方法，是后续语义分割、目标识别与地图融合的基础。本节将围绕这条处理链路展开，帮助读者建立从“原始点”到“可用地图”的完整认知。
一、点云数据结构：从无序集合到有序组织
点云最朴素的形式是无组织的点列表，即 N×3 或 N×4 的数组。这种结构简单，但查询邻域、计算法向量、做配准都很低效。工程中常用以下几种组织方式：
- 无序点云：仅存储点坐标与属性，适合小规模或一次性处理。
- 有序点云：按激光雷达扫描线、深度图像素排列，保留行列索引，便于快速邻域访问。
- KD-Tree：将空间递归划分，支持最近邻、半径搜索，是配准与特征提取的常用索引。
- Octree：八叉树分层压缩，适合大场景存储、体素查询与地图更新。
- Voxel Grid：将空间划分为规则体素，每个体素保留一个代表点，是降采样与占据栅格的基础。
选择数据结构时，要权衡内存、查询速度与精度。例如，KD-Tree 适合静态点云配准，Octree 更适合动态地图维护，而 Voxel Grid 在降采样与语义地图的体素化表达中非常常见。
二、滤波与降采样：让点云更干净、更轻量
原始点云往往包含噪声、离群点、地面点、动态物体以及冗余密度。滤波的目标是去除干扰，降采样的目标是减少点数但保留几何结构。常见方法如下：
| 方法 | 作用 | 典型参数 | 适用场景 |
|---|---|---|---|
| 直通滤波 | 按坐标范围裁剪 | x/y/z 最小最大值 | 去除远处背景、车体自身点 |
| 统计滤波 | 去除离群噪声 | 邻域点数 k、标准差倍数 | 激光雷达稀疏噪声 |
| 半径滤波 | 去除孤立点 | 半径 r、最少邻居数 | 深度相机飞点 |
| 体素降采样 | 降低密度 | 体素边长 leaf size | 配准前加速、地图压缩 |
| 随机降采样 | 随机抽点 | 采样比例 | 快速预览、非关键处理 |
实际工程中，滤波顺序很重要。一般先做直通滤波去除明显无关区域，再用统计滤波或半径滤波去噪，最后做体素降采样。体素边长通常取 0.05～0.2 m，过大会丢失小物体，过小则加速效果有限。
# 以 Open3D 为例的滤波与降采样流程
pcd = o3d.io.read_point_cloud("frame.pcd")
# 1. 直通滤波：保留 z 在 0.2~3.0 m
pcd = pcd.crop(o3d.geometry.AxisAlignedBoundingBox(
min_bound=(-10, -10, 0.2), max_bound=(10, 10, 3.0)))
# 2. 统计滤波：去除离群点
pcd, ind = pcd.remove_statistical_outlier(nb_neighbors=20, std_ratio=2.0)
# 3. 体素降采样：体素边长 0.1 m
down = pcd.voxel_down_sample(voxel_size=0.1)
print(down)
三、特征提取：描述局部几何的关键
点云特征用于描述局部表面的几何形态，是配准、识别与语义分割的核心输入。常见特征包括：
- 法向量：描述局部平面朝向，常用于地面分割、表面重建。
- FPFH：快速点特征直方图，描述点与其邻域的法向量角度关系，对刚体变换具有不变性。
- SHOT：方向直方图签名，结合法向量与空间分布，区分度更高。
- ISS：内在形状签名，用于提取关键点，减少配准计算量。
- 曲率：反映局部弯曲程度，可用于边缘、角点检测。
特征提取通常先估计法向量，再计算特征描述子。法向量估计依赖邻域半径或 k 近邻，半径过小会受噪声影响，过大则平滑掉细节。FPFH 的维度为 33，计算效率较高，适合作为粗配准的特征。
四、配准方法：把多帧点云对齐到同一坐标系
配准的目标是求解两帧或多帧点云之间的刚体变换，使它们在统一坐标系下重合。配准分为粗配准与精配准：
- 粗配准：在未知初始位姿时，利用特征匹配与 RANSAC 估计初始变换。常用 FPFH + RANSAC。
- 精配准：在初始变换基础上，迭代优化点到点或点到面距离。常用 ICP、GICP、NDT。
- 多帧配准：将多帧点云依次对齐，或使用位姿图优化全局一致性。
ICP 对初始位姿敏感，容易陷入局部最优；NDT 将点云表示为概率分布，对初始误差更鲁棒，常用于激光雷达里程计。GICP 结合点到点与点到面，精度较高但计算量更大。
配准质量评估指标：均方误差 RMSE、内点比例 inlier ratio、变换矩阵的平移与旋转误差。工程中通常要求 RMSE 小于 0.05 m，内点比例大于 60%。
五、典型场景与误区复盘
以室内机器人建图为例：深度相机输出 640×480 有序点云，先做直通滤波去除 3 m 外背景，再用半径滤波去除飞点，体素降采样到 0.05 m。随后估计法向量，计算 FPFH，与上一帧做粗配准，再用 ICP 精配准，最后将点云变换到地图坐标系并拼接。整个过程需要控制单帧点数在 1 万～5 万之间，否则实时性难以保证。
常见误区：一是降采样过早，导致特征提取时邻域点不足；二是法向量估计半径与体素边长不匹配，造成法向量方向混乱；三是配准时只依赖 ICP，忽略粗配准，导致对齐失败；四是把动态物体点云直接拼入地图，造成“鬼影”。建议先做动态点剔除，再进行配准与地图更新。
总结来看，点云处理是一条“结构组织—滤波降采样—特征提取—配准融合”的链路。每一步都会影响后续语义地图的质量。工程中应优先保证数据结构的合理性、滤波顺序的正确性、特征参数与体素尺度的一致性，以及配准策略的鲁棒性。只有把这条链路打通，机器人才能在复杂环境中稳定地构建出可理解、可复用的三维语义地图。