📚 课程目录第4章点云与深度图处理

4. 点云与深度图处理

在无人机三维避障与局部规划链路中，深度相机、激光雷达或双目视觉输出的原始数据往往带有噪声、密度不均、地面点混杂等问题。若直接送入规划器，不仅计算量巨大，还会因地面点被误判为障碍而频繁触发急停或绕行。因此，点云与深度图处理是连接感知与规划的关键预处理环节，其目标是把“原始稠密数据”转化为“稀疏、干净、带语义与几何特征的障碍物表示”。

本节核心任务链：原始点云/深度图 → 滤波去噪 → 降采样 → 地面分割 → 聚类 → 法向量估计 → 障碍物特征提取 → 供局部规划使用。

一、核心原理：从原始数据到障碍物特征

深度图本质是二维图像，每个像素值表示该方向上的距离；点云则是三维空间中的离散点集合。两者可互相转换：深度图通过相机内参反投影得到点云，点云也可投影回深度图。处理流程通常按以下顺序进行：

滤波：去除离群点、飞点、深度缺失造成的噪声，常用统计滤波、半径滤波、双边滤波。
降采样：降低点云密度，减少后续计算量，常用体素栅格降采样。
地面分割：分离地面点与非地面点，避免地面被当作障碍，常用 RANSAC 平面拟合、渐进形态学滤波。
聚类：将非地面点按空间连通性分组，形成独立障碍物簇，常用欧式聚类、DBSCAN。
法向量估计：计算每个点或每个簇的局部表面法向，用于判断可通行性、坡度和障碍面朝向。
障碍物特征提取：输出簇的质心、包围盒、尺寸、高度、法向分布、点密度等，供局部规划器评估代价。

这一链条并非固定不变。例如在算力受限的机载平台上，可先降采样再滤波；在室内平坦环境中，地面分割可简化为高度阈值法；在室外复杂地形中，则需更鲁棒的地面拟合与法向量分析。

二、方法流程与关键参数

下面给出一段典型的 ROS/ PCL 风格处理流程伪代码，展示各步骤的调用顺序与关键参数：

// 1. 读取原始点云
pcl::PointCloud<pcl::PointXYZ>::Ptr raw(new ...);
// 2. 统计滤波去离群点
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
sor.setMeanK(30);              // 邻域点数
sor.setStddevMulThresh(1.0);   // 标准差倍数
// 3. 体素降采样
pcl::VoxelGrid<pcl::PointXYZ> vg;
vg.setLeafSize(0.10f, 0.10f, 0.10f); // 10cm 体素
// 4. RANSAC 地面分割
pcl::SACSegmentation<pcl::PointXYZ> seg;
seg.setDistanceThreshold(0.05); // 地面厚度 5cm
// 5. 欧式聚类
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance(0.25);   // 簇间最小距离
ec.setMinClusterSize(10);
ec.setMaxClusterSize(5000);
// 6. 法向量估计
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setKSearch(20);
// 7. 特征输出：质心、AABB、高度、法向均值

参数选择直接影响避障效果。体素过大，小障碍物会被“降没”；聚类容差过小，同一障碍被拆成多簇；地面阈值过大，斜坡会被误判为地面。工程上建议先离线用 rosbag 回放调参，再固化到机载配置。

三、案例场景：室内走廊避障

假设无人机在室内走廊飞行，深度相机输出 640×480 深度图，帧率 30fps。原始点云约 30 万点，直接聚类耗时超过 80ms，无法满足 10Hz 规划频率。经过处理：

统计滤波去除飞点，点数降至 28 万；
体素降采样至 5cm，点数降至 3 万；
RANSAC 分割地面，保留非地面点约 1.2 万；
欧式聚类得到 3～5 个障碍簇，分别对应两侧墙壁与前方行人；
法向量估计显示墙壁法向水平、地面法向垂直，行人簇法向杂乱；
输出障碍物包围盒与质心，局部规划器据此生成绕行轨迹。

处理后单帧耗时约 12ms，满足实时性要求。可见，降采样与地面分割是性能提升的主要贡献者。

四、常见误区与复盘

误区一：只做降采样不做滤波，导致离群点被聚类成“幽灵障碍”。
误区二：地面分割阈值固定不变，遇到斜坡或台阶时地面点残留，规划器误判为不可通行。
误区三：聚类容差与体素尺寸不匹配，体素 10cm 而容差 5cm，同一物体被拆散。
误区四：忽略法向量估计，导致把可跨越的小坡当作垂直墙壁。

复盘建议：建立“感知-规划”联合调试日志，记录每帧各步骤点数、耗时与最终障碍物数量。若障碍物数量突变，优先检查滤波与地面分割参数；若规划轨迹抖动，检查聚类稳定性与法向量平滑度。

五、关键步骤与输出特征对照表

处理步骤	常用方法	关键参数	输出/作用
滤波	统计滤波、半径滤波	MeanK、StddevMulThresh	去除离群点与飞点
降采样	体素栅格	LeafSize	降低密度，提升速度
地面分割	RANSAC、形态学	DistanceThreshold	分离地面与非地面
聚类	欧式聚类、DBSCAN	ClusterTolerance、Min/MaxSize	形成独立障碍簇
法向量估计	PCA、KSearch	KSearch、RadiusSearch	判断表面朝向与坡度
特征提取	质心、AABB、OBB	尺寸、高度、密度	供规划器评估代价

综上，点云与深度图处理不是单一算法，而是一条“去噪—减量—分地—聚类—定向—提特征”的流水线。每一步都需结合机载算力、传感器特性与飞行场景调参。只有把原始数据转化为稳定、轻量、带几何语义的障碍物表示，局部规划器才能在三维空间中安全、平滑地避障。