第 4 章 立体视觉与深度¶
第 2 章从两视图恢复的是带任意尺度的结构，本章讲怎么把"米"这个尺度真正拿到手：靠基线已知的立体。双目、结构光、ToF 是三条主流主动/被动测距路线，产出的深度图再经后处理变成可用的三维点。深度相机驱动与话题侧承接 ROS2-note 第 7 章，本章聚焦算法、判据与坑。代码基于 OpenCV 5.0.0。
视差到深度的主干与后处理位置：
flowchart LR
rect["极线校正"] --> disp["块匹配求视差 d"]
disp --> depth["Z = f·B / d"]
depth --> post["后处理：滤波 / 孔洞填充"]
post --> pc["深度图 → 点云"]1. 双目视差与深度¶
平行光轴的双目（左右相机同高、内参相同、基线 \(B\) 已知）里，同一物点在左右图的横坐标差即视差（disparity） \(d=x_L-x_R\)。由相似三角形：
深度与视差成反比——这是立体视觉一切特性的根源：近处视差大、深度分辨率高；远处视差小、同样的 1 px 视差误差对应越来越大的深度误差（\(\partial Z/\partial d=-fB/d^2\propto Z^2\)）。
cv2.stereoRectify 输出的 \(Q\)（重投影矩阵）把上面三条公式打包成一次齐次乘法：对 \((u,v,d,1)\) 左乘 \(Q\)、除以第四项即得 \((X,Y,Z)\)——
Q = np.array([[1,0,0,-cx],[0,1,0,-cy],[0,0,0,fx],[0,0,1/B,0]], float)
XYZ = (np.stack([u,v,d,np.ones_like(u)],-1) @ Q.T)
XYZ = XYZ[...,:3] / XYZ[...,3:4] # 齐次除法
齐次式与解析式是同一组方程的两种写法，数值上除舍入外完全等价，差别只在乘法次数与坐标约定的书写位置。
要把 \(Z=fB/d\) 用起来，前提是极线校正：两张图各自畸变校正后，再按外参把像平面转成共面、行对齐，同名点才落在同一行上，匹配从二维搜索降为一维。stereoRectify 干的正是这件事，输出的 \(R_1,R_2,P_1,P_2,Q\) 一并决定后续所有反投影。校正误差会直接变成视差偏差：残差 0.5 px 在 2 m 处就是几厘米的深度偏差，所以立体标定关注的是两相机之间的相对姿态，而非各自内参的标定精度。
固定视差误差下的深度误差随距离平方增长（取 f·B = 60 px·m、视差误差 0.5 px，与正文「2 m 处几厘米」一致；数值按 \(Z^2\delta d/(fB)\) 直算）：
xychart-beta
title "视差误差 0.5 px 对应的深度误差"
x-axis "深度 Z (m)" [1, 2, 3, 4, 5, 6, 8, 10]
y-axis "深度误差 (cm)" 0 --> 90
line [0.8, 3.3, 7.5, 13.3, 20.8, 30.0, 53.3, 83.3]2. 主动与被动三种测距¶
| 路线 | 原理 | 适用面 | 典型坑 |
|---|---|---|---|
| 被动双目 | 两灰度图匹配求视差 | 室外、强光、远距离 | 无纹理区匹配失败、计算量大 |
| 主动结构光（RealSense 一类） | 投射已知红外图案，单/双目解码 | 室内中近距、纹理无关 | 强光冲刷图案、室外失效 |
| ToF（time-of-flight） | 测光往返时间/相位差 | 大视场、实时、中距 | 多径、飞行像素（边缘混叠）、分辨率低 |
形态与话题回引 ROS2-note 第 7 章；三者的具体驱动包名与分支可用性（如某型 RealSense 在 ROS 2 Humble 的驱动）标「待核实」。判据：室内近距桌面操作选结构光，室外/强光选被动双目或 ToF，动态快、要低延迟选 ToF，要高分辨率近距离选结构光。
3. 视差窗口与块匹配¶
StereoSGBM 在左右图沿行搜索最优视差（半全局匹配，Semi-Global Block Matching），输出 16 倍定点的整数视差。关键工程含义：视差搜索窗口 = 可测深度窗口。取 numDisparities=160、基线 0.12 m、\(f_x=600\)，视差上限 160 px 对应 \(Z>B f_x/160=0.45\) m；2 m 处视差约 36 px（在窗口内），8 m 处约 9 px（逼近亚像素，匹配易失）。即小于 0.45 m 的物体视差超出窗口、测不到；远处的视差落入亚像素、精度崩塌。纹理充分的合成场景里，SGBM 的视差中位误差可压到亚像素内；纹理稀疏处则靠后处理与多帧融合。
SGBM 主要参数：numDisparities（决定深度窗口，须为 16 的倍数）、blockSize（匹配窗口，奇数，越大越平滑但丢细节）、P1/P2（相邻视差变化惩罚，P2>P1 鼓励大片一致）、uniquenessRatio/speckleRange（剔歧义与噪块）。
其中两个细节直接影响可用性：左右一致性检查（disp12MaxDiff）用右视差图反查左图，不一致的像素标为无效——它把遮挡区与误匹配一并剔掉，代价是边缘处有效点减少；亚像素插值在整数视差之外给出小数部分，远距下视差本身就小，这一项直接决定深度精度。P1/P2 没有通用最优值，常按 P1=8*blockSize²、P2=32*blockSize² 起步再按场景调；P2 增大使弱纹理区更连续，也更容易把真实阶跃抹平。
4. 深度图后处理¶
原始深度图有三类典型缺陷，对应三类后处理：
- 空洞（invalid / holes）：无纹理或遮挡处视差无效。填充用邻域膨胀、
cv2.inpaint（Telea），或直接标记为无效而非填 0——把 0 当真实深度会污染下游。 - 飞点（flying pixels）：物体边缘一个像素同时含前景与背景深度，落在真实轮廓外。抑制靠左右一致性检查、深度梯度阈值、或形态学开运算。
- 边缘保持滤波：平滑噪声但不能抹平深度阶跃。
空洞数量级判据（第 3 节场景）：无纹理区与遮挡边界会出现成片无效像元，可占全图约三成；cv2.inpaint 能补出正值，但补出来的不是真测量，下游要能区分"测得"与"补得"。
边缘保持对照：跨深度阶跃处，高斯滤波把阶跃抹平（前景背景深度被平均），双边滤波用值域核保护大梯度、几乎完整保留阶跃。深度/视差图后处理默认选双边或引导滤波（guidance filter），不用高斯。
后处理效果要落到下游指标上看：点云密度、平面拟合残差、法向估计的抖动。空洞补得漂亮但补出来的深度系统性偏一个常量，平面拟合就会整体倾斜；飞点没剔干净，法向估计会在轮廓处随机翻转——这两类问题在深度图上不显眼，到配准与抓取环节才放大成失败。
5. RGB-D 对齐与标定回引¶
结构光/ToF 相机通常同时给红外深度图和 RGB 图，两者光心不重合，需配准（registration / alignment）：把深度像素逐点变换到 RGB 像素系（驱动侧用对齐选项，如 realsense2_camera 的 align_depth.enable；或软件用两者相对 \(R,t\) + 内外参重投影）。对齐后深度图才与彩色逐像素对应，反投影成彩色点云才不会错位。
深度与 RGB 的对齐关系、以及深度图 → 点云的反投影（第 5 章谱系图的转换边）依赖第 1 章的 \(K\) 与该相对外参；标定操作侧回引 ROS2-note 第 7、8 章。
对齐有方向之分：深度到彩色（把深度重投影到 RGB 像素网格）会在彩色视角看不到的位置留下空洞，彩色到深度则在深度缺失处产生无意义的彩色采样。多数驱动默认前者，因为下游通常要"彩色图逐像素带深度"。两种方向都会在遮挡边界产生错配，转换采样用最近邻而不是双线性，避免把前景背景插成中间值。
6. 单目深度的不可判性¶
第 2 章的尺度不可观在深度估计上表现为单目无法直接测距。把场景整体放大 2 倍、同时把相机平移也放大 2 倍，投影逐像素完全一致（浮点误差内）——
## X, t 同乘 λ：u = K[R | t](λX,1) 与 K[R | t]X 归一化后相同
assert np.abs(proj(R, t, X).max() - proj(R, 2*t, 2*X).max()) < 1e-12
单张像无法区分"近处小物体"与"远处大物体"，因此单目不能凭几何直接测距。单目深度靠先验（学习式网络的绝对/相对深度先验、已知物体尺寸、相机高度 + 地面假设）恢复，本质是把歧义用统计或约束消掉，而非几何可解；单目里程计的尺度靠 IMU、轮速或已知基线锚定（第 6 章、control-note 第 9 章）。
7. 要点¶
- \(Z=fB/d\) 反比关系决定近处精度高、远处随 \(Z^2\) 崩塌；视差搜索窗口就是可测深度窗口。
- 被动双目靠纹理、结构光/ToF 靠主动投射各有适用面；强光、无纹理、远距离是各自的失效边界。
- 齐次 \(Q\) 矩阵与解析反投影数学等价；按 \(Q\) 的数学定义直接用 numpy 计算，便于核对每一步的坐标约定。
- 深度后处理三分工：空洞补/标记要能区分测得与补得、飞点靠左右一致性与梯度、平滑必须用双边类保阶跃。
- 单目深度本质是尺度歧义，图像在"场景×λ + 平移×λ"下不变，只能靠先验或外部锚定消歧。