2. 3D视觉基础
3D视觉的核心任务是获取场景中每个像素对应的三维空间坐标，从而将物理世界转化为可计算、可交互的几何数据。本章作为课程的第二块基石，将深入剖析三种主流深度相机的工作原理——结构光、飞行时间（ToF）与双目立体视觉，并详细讲解点云数据的标准存储格式（PCD/PLY/XYZ）以及贯穿整个机器人引导抓取流程的坐标系与刚体变换基础。理解这些内容，是后续进行点云预处理、特征提取、位姿估计与手眼标定的前提。下图概括了深度获取、数据表示与空间变换三者之间的逻辑关系：
上图清晰地展示了从物理世界到机器人可执行指令的完整链路。下面我们将分三部分展开：首先剖析三种深度相机的原理与选型要点；其次讲解点云数据的存储规范与读写注意事项；最后重点阐述坐标系间的刚体变换数学基础及其在抓取定位中的核心作用。
2.1 深度相机原理：三种主流技术路线
2.1.1 核心原理与差异对比
深度相机通过主动或被动方式测量物体到相机的距离。目前工业与机器人领域应用最广的三种方案为：结构光、飞行时间（ToF）和双目立体视觉。它们的本质区别在于“如何建立对应关系”以及“如何计算深度”。
- 结构光（Structured Light）：投影仪向场景投射已知编码图案（如红外散斑、条纹），相机拍摄被物体表面调制后的图案。通过图案的畸变程度，利用三角法原理逐像素计算深度。典型代表：Intel RealSense D435i（主动立体）、iPhone Face ID（散斑结构光）。
- 飞行时间（ToF）：向场景发射连续或脉冲光波（通常为850nm或940nm红外光），传感器测量光发射到接收的相位差或时间差，直接换算距离。典型代表：Microsoft Kinect v2、Basler ToF相机。
- 双目立体视觉（Stereo Vision）：使用两个水平放置的普通RGB相机模拟人眼，通过匹配左右图像中的同名点，计算视差（disparity），再依据三角测量原理求取深度。典型代表：ZED 2、RealSense T265（仅追踪）。
| 对比维度 | 结构光 | ToF | 双目 |
|---|---|---|---|
| 测距原理 | 编码光斑三角测量 | 光飞行时间/相位差 | 视差三角测量 |
| 工作距离 | 0.2m - 3m（近距离精度高） | 0.5m - 10m（中远距离） | 0.3m - 20m（依赖基线） |
| 精度 | 亚毫米级（近距离） | 厘米级（受多路径干扰） | 毫米级（受纹理影响） |
| 光照敏感度 | 受环境红外干扰 | 受环境光影响较大 | 依赖纹理，弱纹理失效 |
| 计算负载 | 低（硬件解码） | 低（直接输出深度） | 高（需立体匹配算法） |
| 典型场景 | 近距离精密测量、人脸识别 | AGV避障、动态抓取 | 户外导航、大范围场景 |
2.1.2 判断方法：如何快速评估相机输出质量
在实际项目中，拿到深度相机后，不能直接信任其输出。需要从以下三个维度进行快速验证：
- 深度图空洞率：在目标距离放置标准平面板，统计深度值为0或无效像素的比例。空洞率超过5%则需调整相机角度或补光。
- 时间噪声：固定场景连续采集30帧，观察同一点深度值的标准差。标准差大于5mm时，需考虑多帧融合或滤波。
- 温度漂移：相机开机后每10分钟记录一次深度值，连续1小时。若漂移超过3mm，需在算法中加入温度补偿或定期标定。
2.1.3 案例拆解：结构光相机在金属工件抓取中的调优
背景与约束：某汽车零部件厂需要抓取堆叠的铝合金支架，工件表面有油污且反光严重。初始使用某国产散斑结构光相机，深度图出现大量“飞点”（即深度值跳变到背景）。
分析过程：通过查看红外散斑图，发现油污区域导致散斑对比度下降，局部解码失败。进一步检查相机参数，发现红外投影功率设置为默认的60%。
操作步骤：
- 将投影功率提升至90%，增强散斑纹理强度。
- 开启相机的“HDR模式”，针对高反光区域进行多次曝光融合。
- 在点云处理端，增加基于深度的离群点滤波器（半径搜索法，半径5mm，邻域点数<10则剔除）。
结果对比：调整后，深度图空洞率从12%降至2.3%，抓取成功率从78%提升至96%。代价是功耗增加约1.5W，帧率从30fps降至25fps，但满足节拍要求。
复盘经验：结构光相机对表面材质极其敏感，现场调试时优先检查红外投影质量，而非直接修改算法参数。
2.2 点云数据格式：PCD/PLY/XYZ
2.2.1 格式定义与适用场景
点云是三维空间点的集合，每个点通常包含三维坐标（x,y,z），并可附加颜色（RGB）、强度（intensity）、法向量等属性。不同格式在存储效率、可读性和扩展性上差异显著。
| 格式 | 文件结构 | 优点 | 缺点 | 适用场景 |
|---|---|---|---|---|
| PCD | ASCII或二进制头文件+数据 | PCL原生支持，支持自定义字段 | 头文件解析稍复杂 | PCL算法处理、科研 |
| PLY | 头文件+元素列表 | 支持网格与点云，可含颜色 | 二进制格式需注意字节序 | 三维重建、模型交换 |
| XYZ | 纯文本，每行“x y z” | 极简，任何语言可解析 | 无头信息，文件较大 | 快速验证、跨平台传输 |
以PCD格式为例，其头部必须包含以下关键字段：
# .PCD v0.7 - Point Cloud Data file format
VERSION 0.7
FIELDS x y z rgb
SIZE 4 4 4 4
TYPE F F F F
COUNT 1 1 1 1
WIDTH 640
HEIGHT 480
VIEWPOINT 0 0 0 1 0 0 0
POINTS 307200
DATA ascii
0.123 0.234 0.345 4.2108e+06
...2.2.2 格式转换与数据校验
在实际工程中，经常需要将相机输出的原始数据（如16bit深度图）转换为点云。核心步骤包括：
- 利用相机内参（fx, fy, cx, cy）将像素坐标(u,v)和深度值d转换为相机坐标系下的三维坐标：
X = (u - cx) * d / fx，Y = (v - cy) * d / fy，Z = d - 若需要RGB信息，则通过对齐后的彩色图获取对应像素颜色。
- 将点云数据写入PCD或PLY文件，并检查点云包围盒是否合理（例如，Z值不应出现负数或异常大值）。
2.3 坐标系与变换基础
2.3.1 核心概念：四大坐标系
在机器人引导抓取中，至少涉及以下四个坐标系：
- 像素坐标系 (u,v)：图像左上角为原点，单位为像素。
- 相机坐标系 (Xc, Yc, Zc)：光心为原点，Z轴指向场景前方，单位为米。
- 机器人基座坐标系 (Xb, Yb, Zb)：机器人底座中心，Z轴垂直向上。
- 工具坐标系 (Xt, Yt, Zt)：安装在法兰盘上的抓具中心，Z轴通常指向抓取方向。
从相机坐标系到机器人基座坐标系的变换，通常由一个4x4齐次变换矩阵T_cam_to_base表示，该矩阵由旋转矩阵R（3x3）和平移向量t（3x1）组成：
T = | R t |
| 0 1 |其中R描述了相机相对于机器人基座的姿态，t描述了相机光心在基座坐标系下的位置。求解这个矩阵的过程，就是手眼标定。
2.3.2 变换运算与点云应用
假设点云中某点在相机坐标系下的坐标为P_cam，要将其变换到机器人基座坐标系，只需左乘变换矩阵：
P_base = T_cam_to_base * P_cam在PCL中，这通常通过pcl::transformPointCloud函数实现。需要注意的是，变换矩阵必须是刚体变换（即R为正交矩阵，行列式为1），否则会导致点云形变。
| 变换类型 | 数学表达 | 物理意义 |
|---|---|---|
| 旋转 | R (3x3) | 绕X/Y/Z轴旋转，或任意轴旋转 |
| 平移 | t (3x1) | 沿X/Y/Z方向移动 |
| 齐次变换 | T (4x4) | 旋转+平移的复合 |
2.3.3 实战场景：从点云到抓取点的完整变换链
场景描述：使用固定安装的深度相机引导六轴机器人抓取传送带上的随机摆放的齿轮。已知齿轮在相机坐标系下的位姿为P_cam（位置+姿态），需要计算机器人抓取时工具坐标系的目标位姿。
操作流程：
- 离线完成手眼标定，得到T_cam_to_base。
- 在线获取点云，通过聚类与模板匹配算法，得到齿轮在相机系下的位姿P_cam。
- 计算齿轮在机器人基座系下的位姿：P_base = T_cam_to_base * P_cam。
- 根据工具坐标系的偏移量（即抓具中心到法兰盘中心的固定变换T_tool_to_flange），计算法兰盘目标位姿：T_flange_target = P_base * inv(T_tool_to_flange)。
- 将T_flange_target发送给机器人控制器执行抓取。
风险与代价：若传送带在运动，还需考虑传送带编码器与机器人的同步，否则会产生动态跟踪误差。此时，需要引入传送带跟踪标定，将编码器读数映射到机器人坐标系。
复盘：整个变换链中，任何一环的误差都会累积。建议在项目验收时，使用激光跟踪仪或高精度探针测量实际抓取点与理论点的偏差，若偏差大于5mm，应逐级排查标定板平面度、相机畸变校正和机器人定位精度。