4. 深度感知与点云处理
在机器人6D位姿估计与抓取任务中，深度感知是连接“看见物体”与“操作物体”的关键桥梁。RGB图像提供纹理与颜色，而深度信息给出真实尺度与空间位置。只有把深度数据组织成点云，并经过滤波、配准、分割与特征提取，才能为后续的位姿求解和抓取规划提供稳定、可计算的几何基础。
一、深度感知的两类主力传感器
RGB-D相机与激光雷达是机器人系统中最常见的深度来源，但它们的成像机理、数据组织和适用场景差异明显。
- RGB-D相机：典型代表包括结构光相机与飞行时间（ToF）相机。结构光通过投射编码图案并观察形变恢复深度，近距离精度高，适合室内抓取；ToF通过测量光脉冲飞行时间获得深度，帧率高，但易受多路径干扰。RGB-D相机输出对齐的彩色图与深度图，深度图可逐像素反投影为有序点云。
- 激光雷达：通过旋转或固态扫描发射激光束，测量回波时间得到距离。机械式激光雷达视场大、测距远，适合室外导航与建图；固态激光雷达体积小、寿命长，逐渐用于近距离感知。激光雷达输出的是无序或按扫描线组织的三维点，密度随距离增加而下降。
核心区别：RGB-D相机提供“像素级有序深度”，适合稠密点云与纹理融合；激光雷达提供“大范围稀疏三维点”，适合全局环境感知与远距离定位。抓取任务中，桌面级操作常用RGB-D，移动抓取平台则常采用RGB-D与激光雷达融合。
二、从深度图到点云：坐标变换与组织
深度图转点云的核心是针孔相机模型。已知像素坐标 (u, v)、深度值 d、相机内参矩阵 K，可反投影得到相机坐标系下的三维点：
X = (u - cx) * d / fx
Y = (v - cy) * d / fy
Z = d
其中 fx、fy 为焦距，cx、cy 为主点。若深度图与彩色图已对齐，还可将RGB值赋给对应点，形成彩色点云。激光雷达则通常直接输出三维点，需通过外参标定变换到机器人基座或世界坐标系。
点云的组织方式直接影响处理效率。有序点云保留图像网格结构，便于邻域查询；无序点云常用KD-Tree或Octree建立空间索引。实际工程中，PCL、Open3D等库都提供了从深度图生成点云以及空间索引构建的接口。
三、点云滤波：去噪、降采样与范围裁剪
原始点云往往包含噪声、离群点和冗余数据，直接配准或分割会显著降低精度与速度。常用滤波方法如下：
| 滤波方法 | 作用 | 典型参数 | 适用场景 |
|---|---|---|---|
| 直通滤波 | 按坐标范围裁剪 | x/y/z 最小最大值 | 去除背景与远处干扰 |
| 体素栅格滤波 | 降采样，保持形状 | 体素边长 leaf_size | 加速后续配准与分割 |
| 统计滤波 | 去除离群点 | 邻域点数 k、标准差倍数 | 消除飞点与稀疏噪声 |
| 半径滤波 | 去除孤立点 | 半径 r、最少邻点数 | 保留稠密结构 |
| 双边滤波 | 平滑深度图 | 空间与颜色权重 | RGB-D深度图预处理 |
工程建议：先直通滤波裁剪工作空间，再体素降采样，最后统计滤波去离群点。体素边长通常取目标最小特征尺寸的1/5～1/10，过大会丢失细节，过小则加速有限。
四、配准：把多帧点云对齐到同一坐标系
配准的目标是求解两个点云之间的刚体变换，使它们在同一坐标系下重合。配准分为粗配准与精配准两个阶段。
- 粗配准：在未知初始位姿时，使用FPFH、SHOT等特征描述子建立对应点，再通过RANSAC或SAC-IA求解初始变换。适合多视角拼接与物体识别。
- 精配准：在已有较好初值的前提下，使用ICP（迭代最近点）及其变体（Point-to-Plane ICP、GICP）最小化对应点距离，逐步优化变换矩阵。
# Open3D 精配准示例
reg = o3d.pipelines.registration.registration_icp(
source, target, max_correspondence_distance=0.02,
init=initial_transform,
estimation_method=o3d.pipelines.registration.TransformationEstimationPointToPlane())
print(reg.transformation)
配准质量常用均方误差、内点比例和变换矩阵的收敛性评估。若初值偏差过大，ICP容易陷入局部最优，因此粗配准不可省略。
五、分割与特征提取：从场景中分离目标
分割是把点云划分为若干具有物理意义的子集。抓取场景中，最常用的是平面分割与聚类分割。
- RANSAC平面分割：拟合桌面、地面等大平面，将其移除后保留物体点云。参数包括距离阈值和迭代次数。
- 欧式聚类：根据点间距离将剩余点云聚成独立物体，参数为聚类容差和最小/最大点数。
- 区域生长：依据法线、曲率等几何一致性合并邻域点，适合分割复杂曲面。
分割后，需要提取能描述物体几何与位姿的特征。常用特征包括：
- 法线：估计局部表面朝向，是点对特征与ICP的重要输入。
- FPFH / SHOT：描述局部几何分布，用于粗配准与物体识别。
- 点对特征（PPF）：描述两点间的距离、法线夹角等关系，是6D位姿投票算法的核心。
- VFH / CVFH：全局或视点相关特征，用于快速物体检索与位姿初值估计。
常见误区：直接对原始点云做ICP而不做滤波和分割，会导致背景点参与匹配，位姿结果漂移；法线估计时邻域半径选择不当，会使法线方向混乱，进而影响PPF和配准精度。建议先降采样、去离群点，再估计法线，并检查法线朝向一致性。
六、典型抓取场景中的处理链路
以“桌面抓取杯子”为例，完整链路如下：RGB-D相机采集对齐的彩色图与深度图；裁剪工作空间并降采样；统计滤波去除飞点；RANSAC分割桌面并移除；欧式聚类得到杯子点云；估计法线与FPFH特征；与模型库粗配准后ICP精配准；输出6D位姿并送入抓取规划。激光雷达场景中，则先做地面分割与聚类，再对目标点云做配准与特征提取，最后与视觉结果融合。
整条链路中，每一步都会影响最终位姿精度。深度图质量决定点云基础，滤波与分割决定目标是否干净，配准决定坐标系是否统一，特征提取决定位姿求解是否稳定。工程实践中，应把每一步的参数与可视化结果纳入调试流程，而不是只关注最后的位姿数值。