【双目相机 深度估计】PixelXYZ 3D 双目相机标定与实时深度估计全流程

zh路西法2026-07-29 9:06

前言

  • 最近项目之间的闲暇间隙在实验室瞎逛，摸到一个 PixelXYZ 3D 双目相机，于是有了本文
  • 刚好手头有一套棋盘格，那就从头走一遍：标定 → 矫正 → 匹配 → 深度，完整闭环
  • 本文将带你从零完成双目相机的标定与深度估计，核心链路如下：
    • 标定 --- 张正友棋盘格法，标定出左右目的内参、畸变、外参
    • 矫正 --- 利用标定结果做极线矫正（Bouguet 算法）
    • 匹配 --- StereoSGBM 半全局匹配计算视差
    • 滤波 --- WLS 加权最小二乘滤波去噪保边
    • 深度 --- 视差转深度，2x2 画面实时显示，中心像素深度实时输出

文章目录

    • 前言
    • [1 双目介绍](#1 双目介绍)
        • [1-1 什么是双目相机](#1-1 什么是双目相机)
        • [1-2 本相机类型](#1-2 本相机类型)
        • [1-3 双目相机的典型用途](#1-3 双目相机的典型用途)
    • [2 相机标定](#2 相机标定)
        • [2-1 为什么要标定](#2-1 为什么要标定)
        • [2-2 标定原理](#2-2 标定原理)
          • [2-2-1 针孔相机模型](#2-2-1 针孔相机模型)
          • [2-2-2 畸变模型](#2-2-2 畸变模型)
          • [2-2-3 张正友标定法](#2-2-3 张正友标定法)
          • [2-2-4 双目外参标定](#2-2-4 双目外参标定)
          • [2-2-5 极线矫正（Bouguet 算法）](#2-2-5 极线矫正（Bouguet 算法）)
        • [2-3 标定代码](#2-3 标定代码)
        • [2-4 运行](#2-4 运行)
        • [2-5 标定输出的 npz 文件](#2-5 标定输出的 npz 文件)
    • [3 深度估计](#3 深度估计)
        • [3-1 三角测量原理](#3-1 三角测量原理)
        • [3-2 视差图计算方法概览](#3-2 视差图计算方法概览)
        • [3-3 SGBM 半全局匹配](#3-3 SGBM 半全局匹配)
        • [3-4 WLS 滤波原理](#3-4 WLS 滤波原理)
        • [3-5 完整代码](#3-5 完整代码)
        • [3-6 对比](#3-6 对比)
    • [4 辅助内容](#4 辅助内容)
    • 总结

1 双目介绍

1-1 什么是双目相机
  • 双目相机模仿人眼：两个摄像头水平排列，间距称为 基线（baseline）
  • 同一物体在左右图像中的成像位置有水平偏移------这就是 视差（disparity）
  • 基线越大、物体越近，视差越大
  • 说人话就是：

两只眼睛看同一个物体，左眼和右眼看到的水平位置不一样------近的东西偏移大（伸手看手指），远的东西偏移小（看远处的楼）。根据这个偏移量，就能反推距离。

1-2 本相机类型
  • 我手上的 PixelXYZ 3D 双目相机通过 USB 连接，在 Linux 下被识别为 UVC 设备
  • 一条总线出两个 sensor 的图像，硬件内部已经将左右画面横向拼接成一帧------省去了我们自己同步左右帧的麻烦
  • 查看设备：
bash 复制代码 
ls /dev/video*
# /dev/video0  /dev/video1  (笔记本自带摄像头)
# /dev/video2  /dev/video3  /dev/video4  /dev/video5  (PixelXYZ 3D 双目相机)

  • 用 v4l2-ctl --list-devices 可以看到：

    PixelXYZ 3d Cam: PixelXYZ 3d Ca (usb-0000:00:14.0-1):
    /dev/video2
    /dev/video3
    /dev/video4
    /dev/video5

    Integrated Camera: Integrated C (usb-0000:00:14.0-11):
    /dev/video0
    /dev/video1

  • 从上面可以看到：/dev/video0 和 /dev/video1 被笔记本自带的 Integrated Camera 占用了，PixelXYZ 3D 作为第二个插入的 USB 摄像头被分配到 /dev/video2 ~ /dev/video5（一个物理相机可能有多个逻辑设备，对应不同的功能通道------主视频流、metadata 等）

  • 所以用 cv2.VideoCapture(2)（整数索引 2 即对应 /dev/video2）

  • 注意：必须用整数索引而不是字符串路径 "/dev/video2"------后者在某些 OpenCV 版本的 V4L2 后端会导致帧读取异常

  • 相机输出 YUYV 格式 ，分辨率 2560x720 ：左半边 [:, :1280] 是左目，右半边 [:, 1280:] 是右目，每帧只需简单切分即可

1-3 双目相机的典型用途
  • 深度估计：获取场景的稠密深度图
  • 3D 重建：结合深度 + RGB 做点云重建
  • SLAM / VIO：视觉里程计、ORB-SLAM 等
  • 避障：无人机/机器人近距离障碍物检测
  • 手势识别：利用深度信息做 3D 手势交互

2 相机标定

2-1 为什么要标定
  • 未标定的相机直接用默认参数算深度，误差巨大------你不知道真实的焦距和基线，算出来的深度就是"看着像那么回事"
  • 标定的目的就是获得以下几样东西：
参数 含义 示例（本相机左目）
内参矩阵 K K K 焦距 ( f x , f y ) (f_x, f_y) (fx,fy) + 主点 ( c x , c y ) (c_x, c_y) (cx,cy) f x = 928.3 , f y = 926.3 , c x = 659.2 , c y = 307.8 f_x=928.3,\ f_y=926.3,\ c_x=659.2,\ c_y=307.8 fx=928.3, fy=926.3, cx=659.2, cy=307.8
畸变系数 D D D 径向畸变 k 1 , k 2 , k 3 k_1,k_2,k_3 k1,k2,k3 + 切向畸变 p 1 , p 2 p_1,p_2 p1,p2 0.059 , − 0.186 , − 0.005 , 0.005 , 0.177 0.059, -0.186, -0.005, 0.005, 0.177 0.059,−0.186,−0.005,0.005,0.177
旋转矩阵 R R R 右目相对于左目的旋转 接近单位阵（两镜头几乎平行）
平移向量 T T T 右目相对于左目的平移（基线） T x ≈ − 60.4 mm T_x \approx -60.4\text{mm} Tx≈−60.4mm
  • 说人话就是：

标定 = 给相机做一次"视力体检"，查出它真实的焦距、光心位置、镜头畸变程度、以及两个镜头之间的位置关系。有了这些，后续算深度才有底气。

2-2 标定原理
2-2-1 针孔相机模型
  • 世界坐标系中的 3D 点 P w = ( X w , Y w , Z w ) P_w = (X_w, Y_w, Z_w) Pw=(Xw,Yw,Zw) 经过以下变换投影到像素坐标 ( u , v ) (u, v) (u,v)：

s u v 1 = K R ∣ t X w Y w Z w 1 s \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = K \begin{bmatrix} R \mid t \end{bmatrix} \begin{bmatrix} X_w \\ Y_w \\ Z_w \\ 1 \end{bmatrix} s uv1 =KR∣t XwYwZw1

  • 其中内参矩阵：

K = f x 0 c x 0 f y c y 0 0 1 K = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} K= fx000fy0cxcy1

  • 说人话： K K K 矩阵描述的是"从相机坐标系到像素坐标系的映射"------焦距决定缩放比例，主点决定画面中心偏移
2-2-2 畸变模型
  • 实际镜头存在畸变------直线的东西拍出来弯了
  • 径向畸变由 k 1 , k 2 , k 3 k_1, k_2, k_3 k1,k2,k3 描述（桶形/枕形），切向畸变由 p 1 , p 2 p_1, p_2 p1,p2 描述（镜头和传感器不平行导致）
类型 无畸变 桶形 k 1 > 0 k_1 > 0 k1>0 枕形 k 1 < 0 k_1 < 0 k1<0 切向 p 1 , p 2 ≠ 0 p_1, p_2 \neq 0 p1,p2=0
特征 方格笔直 边缘向外膨胀 边缘向内收缩 梯形/平行四边形扭曲
  • 设 ( x , y ) (x, y) (x,y) 为归一化无畸变坐标， ( x ~ , y ~ ) (\tilde{x}, \tilde{y}) (x~,y~) 为畸变后坐标， r 2 = x 2 + y 2 r^2 = x^2 + y^2 r2=x2+y2：

{ x ~ = x ( 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 ) + 2 p 1 x y + p 2 ( r 2 + 2 x 2 ) y ~ = y ( 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 ) + p 1 ( r 2 + 2 y 2 ) + 2 p 2 x y \begin{cases} \tilde{x} = x(1 + k_1 r^2 + k_2 r^4 + k_3 r^6) + 2p_1 xy + p_2(r^2 + 2x^2) \\ \tilde{y} = y(1 + k_1 r^2 + k_2 r^4 + k_3 r^6) + p_1(r^2 + 2y^2) + 2p_2 xy \end{cases} {x~=x(1+k1r2+k2r4+k3r6)+2p1xy+p2(r2+2x2)y~=y(1+k1r2+k2r4+k3r6)+p1(r2+2y2)+2p2xy

2-2-3 张正友标定法
  • 我们用的就是张正友标定法------OpenCV 的 cv2.calibrateCamera 和 cv2.stereoCalibrate 内部实现正是此方法
  • 使用棋盘格（已知方格物理尺寸 20mm），从不同角度、位置拍摄多帧
  • 核心思路：利用 单应性矩阵 建立平面标定板与图像平面的约束关系
  • 具体步骤：
    • 每帧检测棋盘格角点（cv2.findChessboardCorners），建立 3D-2D 对应
    • 亚像素精化（cv2.cornerSubPix）
    • 通过单应性 H H H 求解内参的封闭解
    • 用 Levenberg-Marquardt 优化最小化重投影误差：

min ⁡ K , D , R i , t i ∑ i ∑ j ∥ m i j − m ^ ( K , D , R i , t i , M j ) ∥ 2 \min_{K,D,R_i,t_i} \sum_{i} \sum_{j} \left\| m_{ij} - \hat{m}(K, D, R_i, t_i, M_j) \right\|^2 K,D,Ri,timini∑j∑∥mij−m^(K,D,Ri,ti,Mj)∥2

  • 说人话就是：

拿一块已知尺寸的棋盘格，在不同位置、角度、距离给相机拍几十张。因为棋盘格每个格子的真实大小我们提前知道，所以可以通过匹配"图像上检测到的角点 ↔ 真实物理坐标"来反推相机的所有参数。

  • 完整的闭式解推导（单应性 H H H → K − T K − 1 K^{-T}K^{-1} K−TK−1 → Cholesky 分解 → K K K）不是本文重点，有兴趣的读者请参考张正友原论文 A Flexible New Technique for Camera Calibration（IEEE TPAMI, 2000）
2-2-4 双目外参标定
  • 在单目标定基础上，双目联合标定求解右目相对左目的 R R R 和 T T T
  • 本质矩阵 E = T × R E = T_\times R E=T×R，基础矩阵 F = K r − T E K l − 1 F = K_r^{-T} E K_l^{-1} F=Kr−TEKl−1
  • 两者的区别：
    • E E E（Essential Matrix，本质矩阵） ：工作在归一化相机坐标系下（不考虑内参），编码了两个相机之间的纯几何关系------平移 T T T 和旋转 R R R。它描述的是"左相机坐标系中的一条射线，在右相机坐标系中对应哪条极线"
    • F F F（Fundamental Matrix，基础矩阵） ：工作在像素坐标系下（考虑内参），是 E E E 乘上左右目内参的逆。它描述的是"左图上的一个像素点，在右图上对应哪条极线"------这就是实际匹配时用的东西
  • 说人话： E E E 是"两个相机的物理位置关系"， F F F 是"两个相机的照片之间的像素对应关系"。 E E E 乘上内参就变 F F F------相当于把几何关系翻译成了像素关系
  • 最小化左右目的联合重投影误差
  • 说人话就是：

单目标定只是给每只眼睛做了验光（知道各自的度数和散光）。双目标定是量两只眼睛之间的距离和它们有没有歪------ T T T 就是瞳距（基线）， R R R 就是有没有斜视。有了这两个，后面才能算出"左眼看到的这个东西在右眼里应该出现在哪一行"。

2-2-5 极线矫正（Bouguet 算法）
  • 标定完成后，利用 cv2.stereoRectify 计算旋转矩阵 R 1 , R 2 R_1, R_2 R1,R2 和投影矩阵 P 1 , P 2 P_1, P_2 P1,P2
  • 目的是让左右图像的 行对齐（同名点在同一水平线上），把后续匹配的二维搜索降为一维
  • 核心代码：
python 复制代码 
R1, R2, P1, P2, Q, _, _ = cv2.stereoRectify(
    mtx_l, dist_l, mtx_r, dist_r, (w, h), R, T, alpha=0)
  • 参数说明：
参数 作用
alpha=0 裁剪到有效区域，无黑边
alpha=1 保留全部原始像素（边缘可能有黑边）
Q Q Q 矩阵 将视差直接映射到 3D 坐标的透视变换矩阵
  • 这里重点说一下 P 1 , P 2 P_1, P_2 P1,P2：它们是 3 × 4 3 \times 4 3×4 的投影矩阵，将矫正后的 3D 相机坐标映射到 2D 像素坐标。形式为：

P 1 = f x ′ 0 c x ′ 0 0 f y ′ c y ′ 0 0 0 1 0 , P 2 = f x ′ 0 c x ′ f x ′ ⋅ ∣ T x ∣ 0 f y ′ c y ′ 0 0 0 1 0 P_1 = \begin{bmatrix} f_x' & 0 & c_x' & 0 \\ 0 & f_y' & c_y' & 0 \\ 0 & 0 & 1 & 0 \end{bmatrix}, \quad P_2 = \begin{bmatrix} f_x' & 0 & c_x' & f_x' \cdot |T_x| \\ 0 & f_y' & c_y' & 0 \\ 0 & 0 & 1 & 0 \end{bmatrix} P1= fx′000fy′0cx′cy′1000 ,P2= fx′000fy′0cx′cy′1fx′⋅∣Tx∣00

  • 其中 f x ′ , f y ′ f_x', f_y' fx′,fy′ 是矫正后的等效焦距（alpha=0 时会调整来消除黑边）， c x ′ , c y ′ c_x', c_y' cx′,cy′ 是矫正后的主点

  • P 1 P_1 P1 和 P 2 P_2 P2 唯一不同的是 P 2 P_2 P2 的最后一列多了 f x ′ ⋅ ∣ T x ∣ f_x' \cdot |T_x| fx′⋅∣Tx∣------这个偏移恰好把基线距离编码进投影过程，让同一个 3D 点在左右像素坐标之间恰好差了一个视差 d d d

  • 说人话： P 1 P_1 P1 是左眼的"视网膜坐标系统"， P 2 P_2 P2 是右眼的------两者参数几乎一样，只是右眼比左眼往右偏了 ∣ T x ∣ |T_x| ∣Tx∣ 的距离，所以右眼的投影矩阵里多了一个水平偏移项

  • 注意：深度计算用的焦距 f x ′ = P 1 0 , 0 f_x' = P_10,0 fx′=P10,0 是矫正后的等效焦距 。当 alpha=0 时，矫正过程可能对图像有缩放裁剪，导致 f x ′ f_x' fx′ 与标定表里的原始 f x = 928.3 f_x = 928.3 fx=928.3 略有差异------这是正常的，以代码中 P1[0,0] 为准

  • Q Q Q 矩阵与 P 1 , P 2 P_1, P_2 P1,P2 的关系： Q Q Q 本质上是 P 1 , P 2 P_1, P_2 P1,P2 的逆变换，把像素坐标 ( u , v , d ) (u, v, d) (u,v,d) 直接射回 3D 世界坐标：

  • Q Q Q 矩阵形式：

Q = 1 0 0 − c x 0 1 0 − c y 0 0 0 f 0 0 − 1 T x c x − c x ′ T x Q = \begin{bmatrix} 1 & 0 & 0 & -c_x \\ 0 & 1 & 0 & -c_y \\ 0 & 0 & 0 & f \\ 0 & 0 & -\frac{1}{T_x} & \frac{c_x - c'_x}{T_x} \end{bmatrix} Q= 10000100000−Tx1−cx−cyfTxcx−cx′

  • 最后通过 cv2.initUndistortRectifyMap 生成查找表（remap），运行时直接查表做矫正，无需每帧重复计算
  • 说人话：矫正就是把两个带着畸变的画面"掰正"成完美对齐的标准画面，让后续匹配只需要在同一行上从左到右搜，速度飞起
  • 矫正前：左右目各自有畸变，同名点 p l p_l pl 和 p r p_r pr 不在同一行------要在这两帧之间找个对应点，得搜一整片二维区域
  • 矫正后：畸变消除 + 行对齐， p l p_l pl 和 p r p_r pr 严格落在同一行上------匹配只需沿这一行从左扫到右，计算量从 O ( H × W ) O(H \times W) O(H×W) 降到 O ( W ) O(W) O(W)
2-3 标定代码
python 复制代码 
#!/usr/bin/env python3
"""
双目相机标定 --- 仿 ROS2 cameracalibrator
自动采集 + X/Y/Size/Skew 四维进度条
棋盘格: 8x11 内角点, 20mm
"""

import cv2
import numpy as np
import os
import sys
import time

# ===== 参数 =====
CHESSBOARD = (8, 11)       # 内角点数
SQUARE_SIZE = 0.020        # 格子大小 (米)
BINS = 9                   # 每个维度的分箱数
MIN_INTERVAL = 1.0         # 自动采集最小间隔 (秒)
MIN_SAMPLES = 15

SAVE_DIR = "/home/lzh/postgraduate0/stereo_ws/src"
CALIB_FILE = os.path.join(SAVE_DIR, "stereo_calib.npz")

BAR_WIDTH = 40  # 进度条字符宽度


def compute_params(corners, img_w, img_h):
    """根据角点计算 X, Y, Size, Skew（0~1 归一化）"""
    pts = corners.reshape(-1, 2)

    # 中心位置
    cx, cy = np.mean(pts, axis=0)
    x = cx / img_w
    y = cy / img_h

    # Size: 用棋盘格覆盖面积占比
    bbox_w = np.max(pts[:, 0]) - np.min(pts[:, 0])
    bbox_h = np.max(pts[:, 1]) - np.min(pts[:, 1])
    size = np.sqrt(bbox_w * bbox_h) / np.sqrt(img_w * img_h)

    # Skew: 棋盘格相对水平线的倾角
    row_vec = pts[CHESSBOARD[0] - 1] - pts[0]
    angle = np.abs(np.arctan2(row_vec[1], row_vec[0]))
    skew = angle / (np.pi / 2)

    return np.clip(x, 0, 1), np.clip(y, 0, 1), np.clip(size, 0, 1), np.clip(skew, 0, 1)


def make_bins():
    """每个维度 BINS 个箱子，统计每个箱子的采样数"""
    return [0] * BINS


def update_bins(bins_list, values):
    """values: [(x,y,size,skew), ...]"""
    xb, yb, sb, kb = bins_list
    for x, y, s, k in values:
        xb[min(int(x * BINS), BINS - 1)] += 1
        yb[min(int(y * BINS), BINS - 1)] += 1
        sb[min(int(s * BINS), BINS - 1)] += 1
        kb[min(int(k * BINS), BINS - 1)] += 1


def draw_progress_bar(img, x, y, w, h, filled, total, label):
    """在 img 上画一个进度条"""
    cv2.putText(img, label, (x, y - 8),
                cv2.FONT_HERSHEY_SIMPLEX, 0.55, (255, 255, 255), 1)

    bar_h = h
    bar_y = y
    bin_w = w // total

    for i in range(total):
        bx = x + i * bin_w
        color = (0, 200, 0) if filled[i] > 0 else (60, 60, 60)
        cv2.rectangle(img, (bx, bar_y), (bx + bin_w - 2, bar_y + bar_h), color, -1)

    cv2.rectangle(img, (x, bar_y), (x + w, bar_y + bar_h), (100, 100, 100), 1)
    f_count = sum(1 for v in filled if v > 0)
    text = f"{f_count}/{total}"
    cv2.putText(img, text, (x + w + 5, bar_y + bar_h - 4),
                cv2.FONT_HERSHEY_SIMPLEX, 0.45, (200, 200, 200), 1)


def main():
    cap = cv2.VideoCapture(2)
    cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*"YUYV"))
    cap.set(cv2.CAP_PROP_FRAME_WIDTH, 2560)
    cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720)

    if not cap.isOpened():
        print("Cannot open /dev/video2")
        sys.exit(1)

    half_w = 1280
    img_w, img_h = 1280, 720
    criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)

    objp = np.zeros((CHESSBOARD[0] * CHESSBOARD[1], 3), np.float32)
    objp[:, :2] = np.mgrid[0:CHESSBOARD[0], 0:CHESSBOARD[1]].T.reshape(-1, 2)
    objp *= SQUARE_SIZE

    obj_points = []
    left_pts = []
    right_pts = []

    # 四个维度的分箱
    bins_x, bins_y, bins_size, bins_skew = make_bins(), make_bins(), make_bins(), make_bins()
    all_params = []

    last_capture = 0
    auto_mode = True

    print("=" * 60)
    print(f"  双目标定 | {CHESSBOARD[0]}x{CHESSBOARD[1]} 内角点 | {SQUARE_SIZE*1000:.0f}mm")
    print(f"  自动采集模式 (间隔 >= {MIN_INTERVAL}s)")
    print("  SPACE = 暂停/恢复自动采集")
    print("  c     = 手动采集一帧")
    print("  r     = 删除最后一帧")
    print("  q     = 完成标定")
    print("=" * 60)

    while True:
        ret, frame = cap.read()
        if not ret:
            continue

        left = frame[:, :half_w]
        right = frame[:, half_w:]

        gray_l = cv2.cvtColor(left, cv2.COLOR_BGR2GRAY)
        gray_r = cv2.cvtColor(right, cv2.COLOR_BGR2GRAY)

        ret_l, corners_l = cv2.findChessboardCorners(gray_l, CHESSBOARD)
        ret_r, corners_r = cv2.findChessboardCorners(gray_r, CHESSBOARD)

        detected = ret_l and ret_r

        # 自动采集
        now = time.time()
        if auto_mode and detected and (now - last_capture) >= MIN_INTERVAL:
            last_capture = now
            c_l = cv2.cornerSubPix(gray_l, corners_l, (11, 11), (-1, -1), criteria)
            c_r = cv2.cornerSubPix(gray_r, corners_r, (11, 11), (-1, -1), criteria)
            obj_points.append(objp)
            left_pts.append(c_l)
            right_pts.append(c_r)

            x, y, s, k = compute_params(c_l, img_w, img_h)
            all_params.append((x, y, s, k))
            update_bins((bins_x, bins_y, bins_size, bins_skew), [(x, y, s, k)])
            print(f"  [AUTO #{len(obj_points):2d}] x={x:.2f} y={y:.2f} "
                  f"size={s:.2f} skew={k:.2f}")

        # 绘制左目
        disp_l = left.copy()
        if ret_l:
            cv2.drawChessboardCorners(disp_l, CHESSBOARD, corners_l, ret_l)

        if detected:
            color = (0, 255, 0)
            text = "DETECTED"
            if auto_mode and (now - last_capture) < MIN_INTERVAL:
                text = f"WAIT {MIN_INTERVAL - (now - last_capture):.1f}s"
        else:
            color = (0, 0, 255)
            text = "L" if ret_l else ("R" if ret_r else "L:NO R:NO")
        cv2.putText(disp_l, text, (10, 30),
                    cv2.FONT_HERSHEY_SIMPLEX, 1, color, 2)

        mode_text = "AUTO" if auto_mode else "MANUAL"
        cv2.putText(disp_l, f"{mode_text} | Samples: {len(obj_points)}", (10, 65),
                    cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 255, 0), 2)

        # 进度条面板
        panel = np.zeros((180, img_w, 3), dtype=np.uint8)
        bar_y_start = 15
        draw_progress_bar(panel, 10, bar_y_start, 600, 20, bins_x, BINS, "X")
        draw_progress_bar(panel, 10, bar_y_start + 40, 600, 20, bins_y, BINS, "Y")
        draw_progress_bar(panel, 10, bar_y_start + 80, 600, 20, bins_size, BINS, "Size")
        draw_progress_bar(panel, 10, bar_y_start + 120, 600, 20, bins_skew, BINS, "Skew")
        cv2.putText(panel, f"Samples: {len(obj_points)}", (620, bar_y_start + 160),
                    cv2.FONT_HERSHEY_SIMPLEX, 0.7, (200, 200, 200), 1)

        disp_r = right.copy()
        if ret_r:
            cv2.drawChessboardCorners(disp_r, CHESSBOARD, corners_r, ret_r)
        top = np.hstack([disp_l, disp_r])

        if panel.shape[1] < top.shape[1]:
            pad = np.zeros((panel.shape[0], top.shape[1] - panel.shape[1], 3), dtype=np.uint8)
            panel = np.hstack([panel, pad])

        combined = np.vstack([top, panel])
        cv2.imshow("Stereo Calibration", combined)

        key = cv2.waitKey(1) & 0xFF

        if key == ord(' '):
            auto_mode = not auto_mode
            print(f"  {'[AUTO]' if auto_mode else '[MANUAL]'} 模式")

        elif key == ord('c') and detected:
            last_capture = now
            c_l = cv2.cornerSubPix(gray_l, corners_l, (11, 11), (-1, -1), criteria)
            c_r = cv2.cornerSubPix(gray_r, corners_r, (11, 11), (-1, -1), criteria)
            obj_points.append(objp)
            left_pts.append(c_l)
            right_pts.append(c_r)
            x, y, s, k = compute_params(c_l, img_w, img_h)
            all_params.append((x, y, s, k))
            update_bins((bins_x, bins_y, bins_size, bins_skew), [(x, y, s, k)])
            print(f"  [MANUAL #{len(obj_points)}] x={x:.2f} y={y:.2f} "
                  f"size={s:.2f} skew={k:.2f}")

        elif key == ord('r') and obj_points:
            obj_points.pop()
            left_pts.pop()
            right_pts.pop()
            all_params.pop()
            bins_x, bins_y, bins_size, bins_skew = make_bins(), make_bins(), make_bins(), make_bins()
            update_bins((bins_x, bins_y, bins_size, bins_skew), all_params)
            print(f"  [DEL] 剩余 {len(obj_points)} 对")

        elif key == ord('q'):
            if len(obj_points) < MIN_SAMPLES:
                print(f"  至少需要 {MIN_SAMPLES} 对，当前 {len(obj_points)} 对")
                continue
            break

    cap.release()
    cv2.destroyAllWindows()

    if len(obj_points) < MIN_SAMPLES:
        print(f"帧数不足 ({len(obj_points)}), 退出")
        return

    print(f"\n标定中 ({len(obj_points)} 对)...")

    # 单目标定
    ret_l, mtx_l, dist_l, _, _ = cv2.calibrateCamera(
        obj_points, left_pts, (img_w, img_h), None, None)
    ret_r, mtx_r, dist_r, _, _ = cv2.calibrateCamera(
        obj_points, right_pts, (img_w, img_h), None, None)

    # 双目标定
    ret, mtx_l, dist_l, mtx_r, dist_r, R, T, E, F = cv2.stereoCalibrate(
        obj_points, left_pts, right_pts,
        mtx_l, dist_l, mtx_r, dist_r, (img_w, img_h),
        criteria=criteria,
        flags=cv2.CALIB_FIX_INTRINSIC + cv2.CALIB_USE_INTRINSIC_GUESS)

    baseline = np.linalg.norm(T)

    print(f"\n{'='*50}")
    print(f"  RMS: {ret:.4f}")
    print(f"  Baseline: {baseline*1000:.1f} mm")
    print(f"  Left  fx={mtx_l[0,0]:.0f} fy={mtx_l[1,1]:.0f} "
          f"cx={mtx_l[0,2]:.0f} cy={mtx_l[1,2]:.0f}")
    print(f"  Right fx={mtx_r[0,0]:.0f} fy={mtx_r[1,1]:.0f} "
          f"cx={mtx_r[0,2]:.0f} cy={mtx_r[1,2]:.0f}")

    np.savez(CALIB_FILE,
             mtx_l=mtx_l, dist_l=dist_l,
             mtx_r=mtx_r, dist_r=dist_r,
             R=R, T=T, E=E, F=F,
             img_size=(img_h, img_w),
             baseline=baseline,
             square_size=SQUARE_SIZE,
             chessboard=CHESSBOARD)
    print(f"  已保存: {CALIB_FILE}")


if __name__ == "__main__":
    main()
  • 标定脚本支持 四维覆盖度进度条 （ X X X 位置、 Y Y Y 位置、 S i z e Size Size 大小、 S k e w Skew Skew 倾斜），引导你把棋盘格摆满整个视野
  • 为什么恰好是这四个维度？这得从标定的数学本质说起：
    • X X X / Y Y Y 位置 ：张正友法本质上是在拟合一个从棋盘格平面到图像平面的单应性 H H H，而 H H H 对棋盘格的位置敏感------如果你的棋盘格始终只在画面正中间，那 H H H 只能描述"正前方那一小块区域"的映射关系，边缘像素的畸变根本约束不到。标定出来的 k 1 , k 2 k_1, k_2 k1,k2 对画面边缘几乎不生效
    • S i z e Size Size 大小 ：即棋盘格离相机的远近。不同距离的棋盘格给出不同深度的约束------近处的大棋盘格约束焦距 f f f 的缩放，远处的小棋盘格约束主点 c x , c y c_x, c_y cx,cy 的偏移。如果只在一个距离拍， f f f 和 c c c 会耦合在一起（数学上叫"欠定"），随便给一组 ( f , c ) (f, c) (f,c) 都能拟合，但换个距离就全错了
    • S k e w Skew Skew 倾斜 ：棋盘格绕 X X X 或 Y Y Y 轴旋转。这是约束畸变参数最关键的维度------正对着拍的棋盘格几乎没有畸变信息（直线投影后还是直线），只有把棋盘格倾斜到画面边缘，径向畸变 k 1 , k 2 k_1, k_2 k1,k2 和切向畸变 p 1 , p 2 p_1, p_2 p1,p2 才会在角点位移中"暴露"出来
  • 说人话就是：

标定就像给相机做验光------你得上下左右（X/Y）、远近（Size）、歪着看（Skew）都测一遍，才能配出准的度数。如果只正对着看一张视力表，医生永远测不出你有散光（切向畸变）。

  • 四个进度条各分 9 个格子，当每一格至少被一帧覆盖时点亮。compute_params() 函数负责每帧把棋盘格角点转成这四个归一化参数，然后 update_bins() 累加到对应格子里------实现细节都在上面的完整代码中
2-4 运行
  • 启动标定：
bash 复制代码 
cd /home/lzh/postgraduate0/stereo_ws/src
python3 stereo_calibrate.py
  • 操作说明：
按键 功能
自动模式 检测到棋盘格后每隔 1s 自动采集
SPACE 暂停 / 恢复自动采集
c 手动采集一帧
r 删除最后一帧
q 完成标定并保存
2-5 标定输出的 npz 文件
  • 标定完成后生成 stereo_calib.npz，包含以下字段：
字段 含义 形状
mtx_l / mtx_r 左右目内参矩阵 ( 3 , 3 ) (3, 3) (3,3)
dist_l / dist_r 左右目畸变系数 ( 1 , 5 ) (1, 5) (1,5)
R 右目→左目的旋转矩阵 ( 3 , 3 ) (3, 3) (3,3)
T 右目→左目的平移向量 ( 3 , 1 ) (3, 1) (3,1)
E 本质矩阵 ( 3 , 3 ) (3, 3) (3,3)
F 基础矩阵 ( 3 , 3 ) (3, 3) (3,3)
img_size 单目图像尺寸 ( h , w ) (h, w) (h,w)
baseline 基线长度（米） 标量
square_size 棋盘格大小（米） 标量
chessboard 棋盘格内角点数 ( r o w s , c o l s ) (rows, cols) (rows,cols)
  • 如何查看内容：
python 复制代码 
import numpy as np
data = np.load("stereo_calib.npz")
print("Keys:", list(data.keys()))
print("左目内参:\n", data["mtx_l"])
print("基线:", data["baseline"] * 1000, "mm")
  • 如何在深度估计中使用：加载 .npz → 计算 remap 表 → 每帧做 cv2.remap 矫正 → 匹配 → 深度

踩坑记录：标定文件保存 img_size 时用了 ( i m g h , i m g w ) = ( 720 , 1280 ) (img_h, img_w) = (720, 1280) (imgh,imgw)=(720,1280)，但 OpenCV 的 stereoRectify 和 initUndistortRectifyMap 要求 ( w i d t h , h e i g h t ) = ( 1280 , 720 ) (width, height) = (1280, 720) (width,height)=(1280,720)。加载时需交换顺序：

python 复制代码 
h, w = self.calib["img_size"]   # 标定文件存的是 (height, width)
cv2.stereoRectify(..., (w, h), ...)  # OpenCV 需要 (width, height)

3 深度估计

3-1 三角测量原理
  • 极线矫正后，左右图像行对齐
  • 设空间中一点 P P P 在左右图像上的投影为 ( x l , y ) (x_l, y) (xl,y) 和 ( x r , y ) (x_r, y) (xr,y)，视差 d = x l − x r d = x_l - x_r d=xl−xr，基线 b b b，焦距 f f f，则深度 Z Z Z 为：

Z = f ⋅ b d Z = \frac{f \cdot b}{d} Z=df⋅b

  • 推导：由相似三角形 △ P O l O r ∼ △ P P l P r \triangle P O_l O_r \sim \triangle P P_l P_r △POlOr∼△PPlPr：

b − ( x l − x r ) Z − f = b Z ⇒ b − d Z − f = b Z ⇒ Z = f ⋅ b d \frac{b - (x_l - x_r)}{Z - f} = \frac{b}{Z} \quad\Rightarrow\quad \frac{b - d}{Z - f} = \frac{b}{Z} \quad\Rightarrow\quad Z = \frac{f \cdot b}{d} Z−fb−(xl−xr)=Zb⇒Z−fb−d=Zb⇒Z=df⋅b

  • 说人话就是：

同一个物体在左图靠右、右图靠左，离你越近偏移越大（视差大），离你越远偏移越小（视差小）。用"两只眼睛的间距（基线）× 焦距 ÷ 视差"就能算出精确距离。

  • 关键性质：
    • 视差 d d d 与深度 Z Z Z 成反比 → 近处物体视差大、远处物体视差小
    • 深度分辨率随距离下降 → 远处的不确定性更高
    • 视差为 0 0 0 或负 → 无有效深度（被遮挡或超出匹配范围）
  • 代码对应：
python 复制代码 
def depth(self, disparity):
    d = np.where(disparity <= 0, 1e-6, disparity)  # 避免除零
    return self.fx * self.baseline / d
  • 3-1 节讲的是"已知一个点的 x l , x r x_l, x_r xl,xr，怎么算它的深度"。但一个新问题来了：怎么为画面中每一个像素都找到它在右图中的对应点？
  • 这个问题的答案就是 视差图（Disparity Map） ------一张和原图同样大小的灰度图，每个像素的值不是亮度，而是该像素的视差 d d d。拿到视差图后，用上面那句 self.fx * self.baseline / d 逐像素一除，就是深度图
  • 为什么不能跳过视差、直接生成深度？因为立体匹配算法天然工作在 像素偏移 空间------"左图这个像素在右图往左偏了几个像素"才是算法能直接优化的量。而深度 Z Z Z 需要乘上 f f f 和 b b b 这两个标定参数，是"带尺子的物理量"，不是算法直接输出的东西
  • 说人话就是：

视差图是"左右眼看到的画面差了几个像素"，纯图像问题。深度图是"这个东西离你几米"，需要把像素差换算成物理距离。所以流程一定是：左右图 → 匹配算法 → 视差图 → 乘 f ⋅ b f \cdot b f⋅b → 深度图。

3-2 视差图计算方法概览
  • 视差图的计算是双目视觉的核心难题------给左图的每一个像素，在右图中找到它的"另一半"
  • 主流方法分为三大类：
方法 代表 特点 适用场景
局部匹配 BM（Block Matching） 逐像素取窗口做 NCC/SAD，速度最快但噪声大 实时性要求极高、精度不敏感
半全局匹配 SGBM 多方向代价聚合 + 平滑约束，速度与精度平衡 本文的选择，实时双目深度
全局匹配 GC（Graph Cut） 全图 MRF 能量最小化，精度最高但极慢 离线 3D 重建、精度优先
  • 近年来，基于深度学习的方法也大量涌现：
    • PSMNet、RAFT-Stereo 等用卷积网络直接回归视差，精度远超传统方法
    • 代价是推理需要 GPU，实时性依赖硬件。嵌入式 / 树莓派 / 飞控上的双目系统目前还是以 SGBM 为主
  • 本文选择 SGBM：精度足够日常场景使用，CPU 实时跑，代码仅一行 cv2.StereoSGBM_create
3-3 SGBM 半全局匹配
  • StereoSGBM 就是干这个的------逐像素在右图中搜索最佳匹配，输出一张稠密视差图
  • OpenCV 的 StereoSGBM 实现了 Heiko Hirschmüller 的 Semi-Global Block Matching 算法
  • 相比传统 BM（Block Matching，块匹配），SGBM 在多个方向（代码指定 MODE_SGBM_3WAY，即 3 方向）上做代价聚合，对无纹理区域和边缘更鲁棒
  • 核心思想：能量函数 E ( D ) E(D) E(D) 包含数据项 + 平滑项：

E ( D ) = ∑ p C ( p , D p ) + ∑ q ∈ N ( p ) P 1 ⋅ 1 ∣ D p − D q ∣ = 1 + ∑ q ∈ N ( p ) P 2 ⋅ 1 ∣ D p − D q ∣ \> 1 E(D) = \sum_p C(p, D_p) + \sum_{q \in \mathcal{N}(p)} P_1 \cdot \mathbb{1}\|D_p - D_q\| = 1 + \sum_{q \in \mathcal{N}(p)} P_2 \cdot \mathbb{1}\|D_p - D_q\| \> 1 E(D)=p∑C(p,Dp)+q∈N(p)∑P1⋅1∣Dp−Dq∣=1+q∈N(p)∑P2⋅1∣Dp−Dq∣\>1

  • 以防你忘记：
    • C ( p , D p ) C(p, D_p) C(p,Dp)：像素 p p p 在视差 D p D_p Dp 下的匹配代价（BT，Birchfield-Tomasi）
    • P 1 P_1 P1：小视差跳变的惩罚（适应倾斜表面）
    • P 2 P_2 P2：大视差跳变的惩罚（处理深度不连续，如物体边缘）
    • 沿 8 8 8 或 16 16 16 个方向的 1 D 1D 1D 路径做动态规划，近似 2 D 2D 2D 全局优化
  • 说人话：

SGBM 不光看单个像素像不像，还看周围的像素关系------如果邻近视差差不多，加小罚分（允许平滑过渡）；如果视差突变，加大罚分（但刚好说明可能是物体边缘，允许这种跳变）。这样出来的视差图边缘清晰、平面区域不花。

  • 本项目的 SGBM 参数配置：
python 复制代码 
self.stereo = cv2.StereoSGBM_create(
    minDisparity=0,           # 最小视差
    numDisparities=128,       # 视差搜索范围（16 的倍数）
    blockSize=7,              # 匹配块大小
    P1=8 * 3 * 7 ** 2,        # 小跳变惩罚
    P2=32 * 3 * 7 ** 2,       # 大跳变惩罚
    disp12MaxDiff=1,          # 左右一致性检查最大容忍差
    uniquenessRatio=10,       # 唯一性约束
    speckleWindowSize=100,    # 散斑滤波窗口
    speckleRange=32,          # 散斑滤波范围
    preFilterCap=63,          # 预处理截断
    mode=cv2.STEREO_SGBM_MODE_SGBM_3WAY)  # 三方向代价聚合

  • 视差图中为什么有些像素是空的（ d ≤ 0 d \le 0 d≤0）？原因有四个：

    • 遮挡：物体边缘处，左图能看到但右图被前景挡住------这个像素在右图中根本不存在，匹配必然失败
    • 超出搜索范围 ：numDisparities=128 意味着最大视差就是 128 128 128 像素。如果物体离相机太近（视差 > 128），算法搜不到，直接放弃
    • 左右一致性检查失败 ：disp12MaxDiff=1 要求左→右和右→左两个方向的匹配结果相差不超过 1 1 1 个像素。如果差异过大，说明这个匹配不可靠------宁缺毋滥
    • 唯一性约束失败 ：uniquenessRatio=10 要求最佳匹配的代价至少比次优匹配低 10 % 10\% 10%。如果两个候选匹配得分差不多（比如在重复纹理区域），算法无法确定哪一个是对的，直接标为空

  • 这些空值在后续深度计算中会被赋一个极小值 10 − 6 10^{-6} 10−6（避免除零），对应的深度 Z Z Z 会趋近于无穷------深度图上显示为黑色/远处

  • 注意：返回的视差图需要除以 16 16 16（SGBM 内部用 4 4 4 位小数精度存储）：

python 复制代码 
disparity = self.stereo.compute(left, right).astype(np.float32) / 16.0
3-4 WLS 滤波原理

  • SGBM 输出的视差图在弱纹理区域和边缘处仍存在噪声

  • WLS（Weighted Least Squares，加权最小二乘） 滤波器利用原始左图的颜色引导图对原始视差做加权最小二乘平滑，在平滑噪声的同时 保留边缘

    !\[wls_principle.png]

  • 流程：

    • SGBM 计算 左→右视差 disp_l
    • 用 createRightMatcher 计算 右→左视差 disp_r（不是简单反过来的，需要单独算）
    • WLS 滤波以左图灰度图为引导，融合两个方向的视差

  • 本质就是解这个优化问题：

min ⁡ D ′ ∑ p ( D p ′ − D p ) 2 + λ ∑ q ∈ N ( p ) w p q ( I ) ⋅ ( D p ′ − D q ′ ) 2 \min_{D'} \sum_p \left (D'_p - D_p)\^2 + \\lambda \\sum_{q \\in \\mathcal{N}(p)} w_{pq}(I) \\cdot (D'_p - D'_q)\^2 \\right D′minp∑ (Dp′−Dp)2+λq∈N(p)∑wpq(I)⋅(Dp′−Dq′)2

  • 其中权重 w p q ( I ) w_{pq}(I) wpq(I) 由左图颜色梯度决定：
    • 颜色相似 → 权重高 → 平滑
    • 颜色差异大（边缘）→ 权重低 → 保持不连续
参数 作用 本项目值
lambda 平滑强度，越大越平滑 8000
sigmaColor 颜色敏感度，边缘检测阈值 1.5
  • 代码实现：
python 复制代码 
def enable_wls(self):
    self.wls = cv2.ximgproc.createDisparityWLSFilter(self.stereo)
    self.right_matcher = cv2.ximgproc.createRightMatcher(self.stereo)
    self.wls.setLambda(8000)
    self.wls.setSigmaColor(1.5)

def compute_disparity(self, left, right):
    if self.wls is None:
        return self.stereo.compute(left, right).astype(np.float32) / 16.0
    disp_l = self.stereo.compute(left, right)
    disp_r = self.right_matcher.compute(right, left)
    return self.wls.filter(disp_l, left, disparity_map_right=disp_r).astype(np.float32) / 16.0
  • 说人话：

WLS 就是拿左图的纹理信息当"向导"------颜色相似的区域使劲平滑去噪，碰到颜色突变的地方（物体边缘）收手保持锐利。这样既去掉了噪点，又不会把边缘糊成一片。

3-5 完整代码
  • 深度估计脚本 depth_from_stereo.py 完整实现如下：
python 复制代码 
#!/usr/bin/env python3
"""
PixelXYZ 3D 双目深度图
用法:
  python3 depth_from_stereo.py              # 未标定，默认参数
  python3 depth_from_stereo.py --calib      # 加载标定，矫正+真实深度
"""

import cv2
import numpy as np
import os
import argparse

CALIB_FILE = os.path.join(os.path.dirname(os.path.abspath(__file__)),
                          "stereo_calib.npz")


def put_text(img, text, y, color=(0, 255, 255)):
    cv2.putText(img, text, (10, y), cv2.FONT_HERSHEY_SIMPLEX, 0.6, color, 1)


class StereoDepthCamera:
    def __init__(self, calib_path=None):
        self.cap = cv2.VideoCapture(2)
        self.cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*"YUYV"))
        self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, 2560)
        self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720)

        if not self.cap.isOpened():
            raise RuntimeError("Cannot open camera 2 (PixelXYZ 3D) --- 请检查 USB 连接")

        # 预热：丢弃前几帧
        for _ in range(5):
            self.cap.read()

        self.calib = None
        self.fx = 700.0
        self.baseline = 0.06

        if calib_path and os.path.exists(calib_path):
            self.calib = np.load(calib_path)
            mtx_l = self.calib["mtx_l"]
            dist_l = self.calib["dist_l"]
            mtx_r = self.calib["mtx_r"]
            dist_r = self.calib["dist_r"]
            R = self.calib["R"]
            T = self.calib["T"]
            h, w = self.calib["img_size"]  # 标定文件存的是 (height, width)

            R1, R2, P1, P2, Q, _, _ = cv2.stereoRectify(
                mtx_l, dist_l, mtx_r, dist_r, (w, h), R, T, alpha=0)
            self.Q = Q
            self.fx = float(P1[0, 0])
            self.baseline = float(abs(T[0, 0] if T.ndim > 1 else T[0]))

            self.map_l1, self.map_l2 = cv2.initUndistortRectifyMap(
                mtx_l, dist_l, R1, P1, (w, h), cv2.CV_32FC1)
            self.map_r1, self.map_r2 = cv2.initUndistortRectifyMap(
                mtx_r, dist_r, R2, P2, (w, h), cv2.CV_32FC1)

            print(f"标定: baseline={self.baseline*1000:.1f}mm fx={self.fx:.0f}")

        self.stereo = cv2.StereoSGBM_create(
            minDisparity=0, numDisparities=128, blockSize=7,
            P1=8 * 3 * 7 ** 2, P2=32 * 3 * 7 ** 2,
            disp12MaxDiff=1, uniquenessRatio=10,
            speckleWindowSize=100, speckleRange=32,
            preFilterCap=63, mode=cv2.STEREO_SGBM_MODE_SGBM_3WAY)

        # WLS 滤波
        self.wls = None
        self.right_matcher = None

    def read_rectified(self):
        """返回 (left, right, raw_frame)，失败返回 (None, None, None)"""
        ret, frame = self.cap.read()
        if not ret or frame is None or frame.size == 0:
            return None, None, None

        left = frame[:, :1280]
        right = frame[:, 1280:]
        gray_l = cv2.cvtColor(left, cv2.COLOR_BGR2GRAY)
        gray_r = cv2.cvtColor(right, cv2.COLOR_BGR2GRAY)

        if self.calib:
            rect_l = cv2.remap(gray_l, self.map_l1, self.map_l2, cv2.INTER_LINEAR)
            rect_r = cv2.remap(gray_r, self.map_r1, self.map_r2, cv2.INTER_LINEAR)
            return rect_l, rect_r, frame
        return gray_l, gray_r, frame

    def enable_wls(self):
        """启用 WLS 双边滤波，去噪保边"""
        self.wls = cv2.ximgproc.createDisparityWLSFilter(self.stereo)
        self.right_matcher = cv2.ximgproc.createRightMatcher(self.stereo)
        self.wls.setLambda(8000)     # 平滑强度（越大越平滑）
        self.wls.setSigmaColor(1.5)  # 颜色敏感度（保边缘）

    def compute_disparity(self, left, right):
        """计算视差：基础 SGBM 或 SGBM+WLS"""
        if self.wls is None:
            return self.stereo.compute(left, right).astype(np.float32) / 16.0

        disp_l = self.stereo.compute(left, right)
        disp_r = self.right_matcher.compute(right, left)
        return self.wls.filter(disp_l, left, disparity_map_right=disp_r).astype(np.float32) / 16.0

    def depth(self, disparity):
        d = np.where(disparity <= 0, 1e-6, disparity)
        return self.fx * self.baseline / d

    def release(self):
        self.cap.release()


def main():
    parser = argparse.ArgumentParser()
    parser.add_argument("--calib", action="store_true")
    parser.add_argument("--baseline", type=float, default=0.06)
    parser.add_argument("--focal", type=float, default=700)
    parser.add_argument("--wls", action="store_true", help="启用WLS滤波")
    args = parser.parse_args()

    calib = CALIB_FILE if args.calib else None
    cam = StereoDepthCamera(calib_path=calib)

    if args.wls:
        cam.enable_wls()
        print("WLS 滤波已启用")

    if not cam.calib:
        cam.baseline = args.baseline
        cam.fx = args.focal
        print(f"未标定: baseline={args.baseline}m fx={args.focal}")

    print("'q'=退出  's'=保存")

    while True:
        left, right, raw = cam.read_rectified()
        if left is None:
            continue

        disparity = cam.compute_disparity(left, right)
        depth = cam.depth(disparity)

        left_color = cv2.cvtColor(left, cv2.COLOR_GRAY2BGR)
        right_color = cv2.cvtColor(right, cv2.COLOR_GRAY2BGR)
        put_text(left_color, "Left (rectified)", 25)
        put_text(right_color, "Right (rectified)", 25)

        disp_vis = cv2.applyColorMap(
            cv2.normalize(disparity, None, 0, 255, cv2.NORM_MINMAX).astype(np.uint8),
            cv2.COLORMAP_JET)
        put_text(disp_vis, "Disparity", 25)

        depth_clip = np.clip(depth, 0, 10)
        depth_vis = cv2.applyColorMap(
            cv2.normalize(depth_clip, None, 0, 255, cv2.NORM_MINMAX).astype(np.uint8),
            cv2.COLORMAP_TURBO)
        depth_mid = depth_clip[depth_clip.shape[0] // 2, depth_clip.shape[1] // 2]
        put_text(depth_vis, f"Depth 0-10m (center: {depth_mid:.2f}m)", 25)

        # 2x2 拼成一个窗口: 左上=Left, 右上=Right, 左下=Disparity, 右下=Depth
        top_row = np.hstack([left_color, right_color])
        bot_row = np.hstack([disp_vis, depth_vis])
        combined = np.vstack([top_row, bot_row])
        cv2.imshow("Stereo Depth | L R / Disp Depth", combined)

        key = cv2.waitKey(1) & 0xFF
        if key == ord('q'):
            break
        elif key == ord('s'):
            cv2.imwrite("/tmp/depth.png", depth_vis)
            cv2.imwrite("/tmp/disparity.png", disp_vis)
            np.save("/tmp/depth_raw.npy", depth)
            print("已保存")

    cam.release()
    cv2.destroyAllWindows()


if __name__ == "__main__":
    main()
  • 运行方式：
bash 复制代码 
# 基础模式（未标定，默认 baseline=0.06m, fx=700）
python3 depth_from_stereo.py

# 加载标定 + WLS 滤波（推荐）
python3 depth_from_stereo.py --calib --wls

# 标定 + WLS + 手动指定参数
python3 depth_from_stereo.py --calib --wls --baseline 0.06 --focal 930
  • 运行后显示 2x2 拼合画面：
    • 左上：左目（矫正后）
    • 右上：右目（矫正后）
    • 左下：视差图（JET 色）
    • 右下：深度图 0-10m（TURBO 色），中心像素深度实时显示
  • 按 s 保存当前帧到 /tmp/，按 q 退出
3-6 对比
  • 为什么滤波前噪点这么多？原因有三：
    • 弱纹理区域匹配失败 ：白墙、桌面、地板这些几乎没有纹理的区域，SGBM 的匹配代价函数找不到可靠的对应点------左右目看到的都是同一片白色，算法无法判断"这片白墙的哪个像素对应右图哪个像素"，于是随机匹配，产生大范围噪点
    • 遮挡区域：物体边缘处，左图能看到但右图被遮挡（或反过来），这些区域根本不存在正确的匹配，算法仍然会强行给出一个"最优"但实际上是错误的结果
    • 光照/传感器噪声 ：廉价 UVC 摄像头的 sensor 信噪比有限，微弱的光照变化或 sensor 热噪声会导致像素值波动，SGBM 对这种逐像素的随机波动很敏感
  • 说人话：

SGBM 在白墙上就像近视眼没戴眼镜------只能看见一片白，根本分不清哪是哪，只好瞎猜。WLS 滤波器相当于给你配了一副"结构感知"眼镜，利用画面中的颜色边界告诉你：这里该平滑，这里是边缘给我停。

  • 滤波前

  • 滤波后

4 辅助内容

4-1 文中配图生成代码
  • 本文中的三角测量原理图和畸变类型图均用 matplotlib 绘制，代码如下：
三角测量原理图
python 复制代码 
import matplotlib.pyplot as plt
import numpy as np

fig, ax = plt.subplots(figsize=(14, 7.5))

b = 4        # baseline
f = 2.0      # focal length
Z = 6        # depth
X = b/2      # P between cameras

Ol = np.array([0, 0])
Or = np.array([b, 0])
P = np.array([X, Z])

xl_abs = Ol[0] + (X - Ol[0]) * f / Z   # = X*f/Z
xr_abs = Or[0] + (X - Or[0]) * f / Z   # = b + (X-b)*f/Z

xl_local = xl_abs
xr_local = xr_abs - b
d = xl_local - xr_local                # = f*b/Z

# Image plane at z = f
ax.axhline(y=f, color='gray', linestyle='--', linewidth=1.2, alpha=0.5)

# Cameras
ax.scatter(*Ol, c='blue', s=100, zorder=5)
ax.scatter(*Or, c='red', s=100, zorder=5)

# Point P
ax.scatter(*P, c='green', s=130, zorder=5)

# Rays
ax.plot([Ol[0], P[0]], [0, P[1]], 'blue', linewidth=2, alpha=0.8)
ax.plot([Or[0], P[0]], [0, P[1]], 'red', linewidth=2, alpha=0.8)

# Image plane intersections
Pl = np.array([xl_abs, f])
Pr = np.array([xr_abs, f])
ax.scatter(*Pl, c='darkblue', s=90, zorder=5)
ax.scatter(*Pr, c='darkred', s=90, zorder=5)

# Principal points
ax.scatter([0], [f], c='blue', s=50, marker='|', linewidths=2)
ax.scatter([b], [f], c='red', s=50, marker='|', linewidths=2)

# Annotations: baseline, focal length, depth, disparity
ax.annotate('', xy=(0, -0.55), xytext=(b, -0.55),
            arrowprops=dict(arrowstyle='<->', color='black', lw=2))
ax.text(b/2, -0.85, '$b$ (基线)', fontsize=13, ha='center')

ax.annotate('', xy=(-0.8, 0), xytext=(-0.8, f),
            arrowprops=dict(arrowstyle='<->', color='blue', lw=1.5))
ax.text(-1.2, f/2, '$f$', fontsize=13, color='blue', ha='center')

ax.annotate('', xy=(X+1.2, 0), xytext=(X+1.2, Z),
            arrowprops=dict(arrowstyle='<->', color='green', lw=2))
ax.text(X+1.6, Z/2, '$Z$', fontsize=13, color='green', ha='center')

ax.annotate('', xy=(xr_abs, f-1.25), xytext=(xl_abs, f-1.25),
            arrowprops=dict(arrowstyle='<->', color='purple', lw=3))
mid_x = (xl_abs + xr_abs) / 2
ax.text(mid_x, f-1.55, "$d = x_l' - x_r'$\n$= x_l' + |x_r'|$\n$= \\frac{f \\cdot b}{Z}$",
        fontsize=11, color='purple', ha='center')

ax.set_xlabel('X')
ax.set_ylabel('Z (深度方向)')
ax.set_title('双目三角测量原理', fontsize=15, fontweight='bold')
ax.set_aspect('equal')
ax.grid(True, alpha=0.15)

plt.tight_layout()
plt.savefig('triangulation_diagram.png', dpi=150)
畸变类型图
python 复制代码 
import matplotlib.pyplot as plt
import numpy as np

def distort_grid(k1, k2, k3, p1, p2, scale=1.0):
    x = np.linspace(-1, 1, 9) * scale
    y = np.linspace(-1, 1, 9) * scale
    X, Y = np.meshgrid(x, y)
    r2 = X**2 + Y**2
    r4 = r2**2
    r6 = r2**3

    radial = 1 + k1 * r2 + k2 * r4 + k3 * r6
    X_radial = X * radial
    Y_radial = Y * radial

    X_tan = 2 * p1 * X * Y + p2 * (r2 + 2 * X**2)
    Y_tan = p1 * (r2 + 2 * Y**2) + 2 * p2 * X * Y

    X_dist = X_radial + X_tan
    Y_dist = Y_radial + Y_tan
    return X, Y, X_dist, Y_dist

fig, axes = plt.subplots(1, 4, figsize=(18, 5))

# 1. Ideal
ax = axes[0]
X, Y, Xd, Yd = distort_grid(0, 0, 0, 0, 0)
for i in range(X.shape[0]):
    ax.plot(X[i, :], Y[i, :], 'gray', linewidth=0.8)
    ax.plot(X[:, i], Y[:, i], 'gray', linewidth=0.8)
ax.set_title('无畸变 (理想)', fontsize=13, fontweight='bold')
ax.set_aspect('equal'); ax.axis('off')

# 2. Barrel (k1 > 0)
ax = axes[1]
X, Y, Xd, Yd = distort_grid(0.5, 0.1, 0, 0, 0)
for i in range(X.shape[0]):
    ax.plot(Xd[i, :], Yd[i, :], 'blue', linewidth=0.8)
    ax.plot(Xd[:, i], Yd[:, i], 'blue', linewidth=0.8)
ax.set_title(r'桶形畸变 ($k_1>0$)', fontsize=13, fontweight='bold')
ax.set_aspect('equal'); ax.axis('off')

# 3. Pincushion (k1 < 0)
ax = axes[2]
X, Y, Xd, Yd = distort_grid(-0.5, 0, 0, 0, 0)
for i in range(X.shape[0]):
    ax.plot(Xd[i, :], Yd[i, :], 'red', linewidth=0.8)
    ax.plot(Xd[:, i], Yd[:, i], 'red', linewidth=0.8)
ax.set_title(r'枕形畸变 ($k_1<0$)', fontsize=13, fontweight='bold')
ax.set_aspect('equal'); ax.axis('off')

# 4. Tangential
ax = axes[3]
X, Y, Xd, Yd = distort_grid(0, 0, 0, 0.15, 0.1)
for i in range(X.shape[0]):
    ax.plot(Xd[i, :], Yd[i, :], 'green', linewidth=0.8)
    ax.plot(Xd[:, i], Yd[:, i], 'green', linewidth=0.8)
ax.set_title(r'切向畸变 ($p_1,p_2\neq 0$)', fontsize=13, fontweight='bold')
ax.set_aspect('equal'); ax.axis('off')

fig.suptitle('镜头畸变类型示意', fontsize=16, fontweight='bold')
plt.tight_layout()
plt.savefig('distortion_types.png', dpi=150)
极线矫正对比图
python 复制代码 
import matplotlib.pyplot as plt
import matplotlib.patches as mpatches
import numpy as np

fig, axes = plt.subplots(2, 2, figsize=(14, 9))

def draw_image_frame(ax, x, y, w, h, color='black'):
    rect = mpatches.FancyBboxPatch((x, y), w, h,
                                     boxstyle="round,pad=0.02",
                                     facecolor='#f5f5f5', edgecolor=color,
                                     linewidth=2)
    ax.add_patch(rect)

def draw_distorted_grid(ax, cx, cy, w, h, k1=0.3, color='gray'):
    nx, ny = 7, 5
    x = np.linspace(cx - w/2*0.9, cx + w/2*0.9, nx)
    y = np.linspace(cy - h/2*0.9, cy + h/2*0.9, ny)
    X, Y = np.meshgrid(x, y)
    r2 = ((X - cx)/(w/2))**2 + ((Y - cy)/(h/2))**2
    Xd = cx + (X - cx) * (1 + k1 * r2)
    Yd = cy + (Y - cy) * (1 + k1 * r2)
    for i in range(nx):
        ax.plot(Xd[:, i], Yd[:, i], color, linewidth=0.8, alpha=0.7)
    for i in range(ny):
        ax.plot(Xd[i, :], Yd[i, :], color, linewidth=0.8, alpha=0.7)

def draw_rectified_grid(ax, cx, cy, w, h, color='gray'):
    nx, ny = 7, 5
    x = np.linspace(cx - w/2*0.9, cx + w/2*0.9, nx)
    y = np.linspace(cy - h/2*0.9, cy + h/2*0.9, ny)
    for i in range(nx):
        ax.plot([x[i], x[i]], [y[0], y[-1]], color, linewidth=0.8, alpha=0.7)
    for i in range(ny):
        ax.plot([x[0], x[-1]], [y[i], y[i]], color, linewidth=0.8, alpha=0.7)

# Top-left: 左目矫正前
ax = axes[0, 0]
ax.set_xlim(0, 5); ax.set_ylim(0, 5); ax.axis('off')
ax.set_title('左目 --- 矫正前', fontsize=13, fontweight='bold')
draw_image_frame(ax, 0.3, 0.5, 4.4, 4.0, '')
draw_distorted_grid(ax, 2.5, 2.5, 4.4, 4.0, k1=0.25, color='royalblue')
xp = np.array([0.6, 4.4])
yp_bl = 3.2 + 0.15 * (xp - 2.5)
ax.plot(xp, yp_bl, 'orange', linewidth=2, linestyle='--')
ax.scatter([1.8], [3.1], c='red', s=80, zorder=5)
ax.text(1.8, 3.35, '$p_l$', fontsize=11, color='red', ha='center')

# Top-right: 右目矫正前
ax = axes[0, 1]
ax.set_xlim(0, 5); ax.set_ylim(0, 5); ax.axis('off')
ax.set_title('右目 --- 矫正前', fontsize=13, fontweight='bold')
draw_image_frame(ax, 0.3, 0.5, 4.4, 4.0, '')
draw_distorted_grid(ax, 2.5, 2.5, 4.4, 4.0, k1=0.2, color='firebrick')
yp_br = 2.5 + 0.12 * (xp - 2.5)
ax.plot(xp, yp_br, 'orange', linewidth=2, linestyle='--')
ax.scatter([3.2], [2.65], c='red', s=80, zorder=5)
ax.text(3.2, 2.9, '$p_r$', fontsize=11, color='red', ha='center')
# Y mismatch indicator
ax.annotate('', xy=(4.5, yp_bl[1]), xytext=(4.5, yp_br[1]),
            arrowprops=dict(arrowstyle='<->', color='red', lw=2))
ax.text(4.7, (yp_bl[1] + yp_br[1])/2, '不在\n同一行!',
        fontsize=9, color='red', ha='left', va='center')

# Bottom-left: 左目矫正后
ax = axes[1, 0]
ax.set_xlim(0, 5); ax.set_ylim(0, 5); ax.axis('off')
ax.set_title('左目 --- 矫正后', fontsize=13, fontweight='bold')
draw_image_frame(ax, 0.3, 0.5, 4.4, 4.0, '')
draw_rectified_grid(ax, 2.5, 2.5, 4.4, 4.0, color='royalblue')
ax.axhline(y=3.1, xmin=0.06, xmax=0.94, color='green', linewidth=2.5, linestyle='--')
ax.scatter([1.8], [3.1], c='red', s=80, zorder=5)

# Bottom-right: 右目矫正后
ax = axes[1, 1]
ax.set_xlim(0, 5); ax.set_ylim(0, 5); ax.axis('off')
ax.set_title('右目 --- 矫正后', fontsize=13, fontweight='bold')
draw_image_frame(ax, 0.3, 0.5, 4.4, 4.0, '')
draw_rectified_grid(ax, 2.5, 2.5, 4.4, 4.0, color='firebrick')
ax.axhline(y=3.1, xmin=0.06, xmax=0.94, color='green', linewidth=2.5, linestyle='--')
ax.scatter([3.2], [3.1], c='red', s=80, zorder=5)
ax.text(4.5, 3.1, '同行!', fontsize=11, color='green', ha='center', fontweight='bold',
        bbox=dict(boxstyle='round', facecolor='lightgreen', alpha=0.7))

fig.text(0.5, 0.52, r'$\Downarrow$ 极线矫正 (Bouguet)', fontsize=14,
         ha='center', fontweight='bold', color='#2980b9')

fig.suptitle('极线矫正前后对比', fontsize=16, fontweight='bold', y=0.98)
plt.tight_layout(rect=[0, 0, 1, 0.94])
plt.savefig('rectification_diagram.png', dpi=150)
WLS 滤波原理图
python 复制代码 
import matplotlib.pyplot as plt
import numpy as np

fig, axes = plt.subplots(3, 1, figsize=(12, 8))
x = np.linspace(0, 10, 300)

# True disparity: wall at d=3, object at d=6, edge at x=5
true_disparity = np.where(x < 5, 3.0, 6.0)

np.random.seed(42)
noise = np.random.normal(0, 0.8, len(x))
noise[45:55] *= 3  # extra noise near edge
raw_disparity = true_disparity + noise

# Guide image: bright/dark with sharp edge at x=5
guide = np.where(x < 5, 200.0, 50.0)

# WLS-like: adaptive window based on distance to edge
wls_output = np.copy(raw_disparity)
for i in range(len(x)):
    dist = abs(x[i] - 5)
    half = 12 if dist > 0.5 else (3 if dist > 0.15 else 1)
    lo, hi = max(0, i-half), min(len(x), i+half+1)
    wls_output[i] = np.mean(raw_disparity[lo:hi])

# Plot 1: SGBM Raw
ax = axes[0]
ax.plot(x, true_disparity, 'gray', linewidth=1.5, linestyle='--', alpha=0.6, label='True')
ax.plot(x, raw_disparity, 'blue', linewidth=0.6, alpha=0.8, label='SGBM Raw')
ax.axvline(x=5, color='red', linewidth=1.5, linestyle=':', alpha=0.5)
ax.set_ylabel('Disparity', fontsize=12)
ax.set_title('SGBM Raw --- Noisy, especially near edges', fontsize=13, fontweight='bold')
ax.legend(loc='upper right', fontsize=9)
ax.set_ylim(0, 9); ax.grid(True, alpha=0.2)

# Plot 2: Guide
ax = axes[1]
ax.plot(x, guide, 'green', linewidth=2.5, label='Guide (left image color)')
ax.axvline(x=5, color='red', linewidth=1.5, linestyle=':', alpha=0.5)
ax.set_ylabel('Intensity', fontsize=12)
ax.set_title('Guide Image --- Color jump = object edge', fontsize=13, fontweight='bold')
ax.legend(loc='upper right', fontsize=9)
ax.set_ylim(0, 260); ax.grid(True, alpha=0.2)

# Plot 3: WLS Output
ax = axes[2]
ax.plot(x, true_disparity, 'gray', linewidth=1.5, linestyle='--', alpha=0.6, label='True')
ax.plot(x, wls_output, 'purple', linewidth=2, alpha=0.9, label='WLS Filtered')
ax.axvline(x=5, color='red', linewidth=1.5, linestyle=':', alpha=0.5)
ax.set_xlabel('Pixel position (horizontal scanline)', fontsize=12)
ax.set_ylabel('Disparity', fontsize=12)
ax.set_title('WLS Output --- Smooth where colors match, sharp at edge', fontsize=13, fontweight='bold')
ax.legend(loc='upper right', fontsize=9)
ax.set_ylim(0, 9); ax.grid(True, alpha=0.2)

fig.suptitle('WLS Filter Principle (1D cross-section)', fontsize=15, fontweight='bold', y=1.01)
plt.tight_layout()
plt.savefig('wls_principle.png', dpi=150)
4-2 WLS 滤波前后对比脚本
  • 运行 python3 wls_compare.py --calib，左右并排对比 SGBM 原始 vs WLS 滤波的视差图和深度图，按 s 截图
python 复制代码 
#!/usr/bin/env python3
"""
WLS 滤波前后对比 --- 用于截屏
左侧: SGBM 原始  |  右侧: SGBM + WLS
"""

import cv2
import numpy as np
import os
import argparse

CALIB_FILE = os.path.join(os.path.dirname(os.path.abspath(__file__)),
                          "stereo_calib.npz")


def put_text(img, text, y, color=(0, 255, 255)):
    cv2.putText(img, text, (10, y), cv2.FONT_HERSHEY_SIMPLEX, 0.6, color, 1)


class StereoDepthCamera:
    def __init__(self, calib_path=None):
        self.cap = cv2.VideoCapture(2)
        self.cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*"YUYV"))
        self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, 2560)
        self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720)
        if not self.cap.isOpened():
            raise RuntimeError("Cannot open camera 2 (PixelXYZ 3D)")
        for _ in range(5):
            self.cap.read()

        self.calib = None
        self.fx = 700.0
        self.baseline = 0.06

        if calib_path and os.path.exists(calib_path):
            self.calib = np.load(calib_path)
            mtx_l = self.calib["mtx_l"]
            dist_l = self.calib["dist_l"]
            mtx_r = self.calib["mtx_r"]
            dist_r = self.calib["dist_r"]
            R = self.calib["R"]
            T = self.calib["T"]
            h, w = self.calib["img_size"]
            R1, R2, P1, P2, Q, _, _ = cv2.stereoRectify(
                mtx_l, dist_l, mtx_r, dist_r, (w, h), R, T, alpha=0)
            self.Q = Q
            self.fx = float(P1[0, 0])
            self.baseline = float(abs(T[0, 0] if T.ndim > 1 else T[0]))
            self.map_l1, self.map_l2 = cv2.initUndistortRectifyMap(
                mtx_l, dist_l, R1, P1, (w, h), cv2.CV_32FC1)
            self.map_r1, self.map_r2 = cv2.initUndistortRectifyMap(
                mtx_r, dist_r, R2, P2, (w, h), cv2.CV_32FC1)
            print(f"标定: baseline={self.baseline*1000:.1f}mm fx={self.fx:.0f}")

        # 两个独立 matcher：raw 永远不被 WLS 污染
        self.stereo_raw = cv2.StereoSGBM_create(
            minDisparity=0, numDisparities=128, blockSize=7,
            P1=8 * 3 * 7 ** 2, P2=32 * 3 * 7 ** 2,
            disp12MaxDiff=1, uniquenessRatio=10,
            speckleWindowSize=100, speckleRange=32,
            preFilterCap=63, mode=cv2.STEREO_SGBM_MODE_SGBM_3WAY)

        self.stereo = cv2.StereoSGBM_create(
            minDisparity=0, numDisparities=128, blockSize=7,
            P1=8 * 3 * 7 ** 2, P2=32 * 3 * 7 ** 2,
            disp12MaxDiff=1, uniquenessRatio=10,
            speckleWindowSize=100, speckleRange=32,
            preFilterCap=63, mode=cv2.STEREO_SGBM_MODE_SGBM_3WAY)

        self.wls = None
        self.right_matcher = None

    def read_rectified(self):
        ret, frame = self.cap.read()
        if not ret or frame is None or frame.size == 0:
            return None, None, None
        left = frame[:, :1280]
        right = frame[:, 1280:]
        gray_l = cv2.cvtColor(left, cv2.COLOR_BGR2GRAY)
        gray_r = cv2.cvtColor(right, cv2.COLOR_BGR2GRAY)
        if self.calib:
            rect_l = cv2.remap(gray_l, self.map_l1, self.map_l2, cv2.INTER_LINEAR)
            rect_r = cv2.remap(gray_r, self.map_r1, self.map_r2, cv2.INTER_LINEAR)
            return rect_l, rect_r, frame
        return gray_l, gray_r, frame

    def enable_wls(self):
        self.wls = cv2.ximgproc.createDisparityWLSFilter(self.stereo)
        self.right_matcher = cv2.ximgproc.createRightMatcher(self.stereo)
        self.wls.setLambda(8000)
        self.wls.setSigmaColor(1.5)

    def compute_disparity_raw(self, left, right):
        """纯 SGBM，使用独立 matcher，不受 WLS 影响"""
        return self.stereo_raw.compute(left, right).astype(np.float32) / 16.0

    def compute_disparity_wls(self, left, right):
        """SGBM + WLS"""
        disp_l = self.stereo.compute(left, right)
        disp_r = self.right_matcher.compute(right, left)
        return self.wls.filter(disp_l, left, disparity_map_right=disp_r).astype(np.float32) / 16.0

    def depth(self, disparity):
        d = np.where(disparity <= 0, 1e-6, disparity)
        return self.fx * self.baseline / d

    def release(self):
        self.cap.release()


def main():
    parser = argparse.ArgumentParser()
    parser.add_argument("--calib", action="store_true")
    args = parser.parse_args()

    calib = CALIB_FILE if args.calib else None
    cam = StereoDepthCamera(calib_path=calib)
    cam.enable_wls()
    print("WLS 已启用，同时显示原始 SGBM 和 WLS 滤波结果")
    print("'q'=退出  's'=保存")

    while True:
        left, right, raw = cam.read_rectified()
        if left is None:
            continue

        # 原始 SGBM
        disp_raw = cam.compute_disparity_raw(left, right)
        depth_raw = cam.depth(disp_raw)

        # SGBM + WLS
        disp_wls = cam.compute_disparity_wls(left, right)
        depth_wls = cam.depth(disp_wls)

        # 可视化
        disp_raw_vis = cv2.applyColorMap(
            cv2.normalize(disp_raw, None, 0, 255, cv2.NORM_MINMAX).astype(np.uint8),
            cv2.COLORMAP_JET)
        put_text(disp_raw_vis, "Disparity (SGBM only)", 25)

        disp_wls_vis = cv2.applyColorMap(
            cv2.normalize(disp_wls, None, 0, 255, cv2.NORM_MINMAX).astype(np.uint8),
            cv2.COLORMAP_JET)
        put_text(disp_wls_vis, "Disparity (SGBM + WLS)", 25, (0, 255, 0))

        depth_raw_vis = cv2.applyColorMap(
            cv2.normalize(np.clip(depth_raw, 0, 10), None, 0, 255, cv2.NORM_MINMAX).astype(np.uint8),
            cv2.COLORMAP_TURBO)
        mid_raw = np.clip(depth_raw, 0, 10)[depth_raw.shape[0] // 2, depth_raw.shape[1] // 2]
        put_text(depth_raw_vis, f"Depth SGBM (center: {mid_raw:.2f}m)", 25)

        depth_wls_vis = cv2.applyColorMap(
            cv2.normalize(np.clip(depth_wls, 0, 10), None, 0, 255, cv2.NORM_MINMAX).astype(np.uint8),
            cv2.COLORMAP_TURBO)
        mid_wls = np.clip(depth_wls, 0, 10)[depth_wls.shape[0] // 2, depth_wls.shape[1] // 2]
        put_text(depth_wls_vis, f"Depth WLS (center: {mid_wls:.2f}m)", 25, (0, 255, 0))

        # 2x2: 上=视差, 下=深度; 左=Raw, 右=WLS
        combined = np.vstack([
            np.hstack([disp_raw_vis, disp_wls_vis]),
            np.hstack([depth_raw_vis, depth_wls_vis])])
        cv2.imshow("WLS Before/After | Top=Disparity Bottom=Depth", combined)

        key = cv2.waitKey(1) & 0xFF
        if key == ord('q'):
            break
        elif key == ord('s'):
            cv2.imwrite("/tmp/wls_before_disp.png", disp_raw_vis)
            cv2.imwrite("/tmp/wls_after_disp.png", disp_wls_vis)
            cv2.imwrite("/tmp/wls_before_depth.png", depth_raw_vis)
            cv2.imwrite("/tmp/wls_after_depth.png", depth_wls_vis)
            cv2.imwrite("/tmp/wls_compare.png", combined)
            print("已保存到 /tmp/wls_*.png")

    cam.release()
    cv2.destroyAllWindows()


if __name__ == "__main__":
    main()

总结

  • 本文从硬件层面到软件层面，完整覆盖了双目深度估计的全流程
  • 核心要点回顾：
    • 双目相机硬件 ：理解拼接帧格式和 UVC 驱动特性，注意用整数索引打开而非字符串路径
    • 双目标定 ：张正友法 → 单目 + 双目联合标定 → 极线矫正，img_size 注意 ( w , h ) (w, h) (w,h) 顺序
    • 深度估计 ：SGBM 半全局匹配 → WLS 保边滤波 → 三角测量 Z = f b / d Z = fb/d Z=fb/d，alpha=0 避免黑边
    • 工程细节 ：棋盘格四维覆盖度采集、npz 文件使用、2x2 画面实时显示
  • 所有代码均在 stereo_ws/src/ 下，可直接运行
  • 如有错误，欢迎指出！
  • 感谢观看！
相关推荐
卷无止境1 小时前
Python 相对导入与绝对导入的坑：从原理到工程实践
后端·python
FriendshipT1 小时前
Ultralytics：简要解读YOLOv8 → YOLO11 → YOLO26网络架构
人工智能·pytorch·python·深度学习·yolo·目标检测
BaoshengTT2 小时前
成都理想贴膜能否分期及汽车贴膜分期行业规则 保圣威固 7V 不凡门店
python·汽车
卷无止境2 小时前
Python调试那点事儿：从pdb到日志系统的实战指南
后端·python
正和视觉教育2 小时前
深圳CCD视觉培训：快速掌握机器视觉检测核心技术
人工智能·python·计算机视觉·视觉检测
axinawang2 小时前
第16课：编码、解码、转义字符、转义序列、输出格式化
python
不会计算机的g_c__b2 小时前
Python + OpenCV 实现文档扫描与OCR识别系统
python·opencv·ocr
梦想不只是梦与想2 小时前
Python 中的 reduce 函数：累积计算
python·reduce
唐山泽际科技2 小时前
2026年本地生活服务（如餐饮、美业）怎样借助LBS（地理位置）精准获客？
python·智能手机·生活
热门推荐
01GitHub 镜像站点022026年7月AI圈大地震：GPT-5.6被政府限制、Claude入驻Slack、Anthropic自研芯片03如何新建文件夹？ 电脑新建文件夹的4种方法04国内可直接用、免费额度/永久免费的大模型API清单（含 SiliconFlow、火山、阿里、智谱、百度、Kimi、DeepSeek、DMXAPI 等）05AI科技热点日报 | 2026年07月01日06AI 编程 IDE 全景解析 2026：Agent 全面接管开发链路072026 国产 AI 大模型横评：DeepSeek、通义千问、Kimi、文心一言、星火、豆包谁更能打？08微信历史版本含下载地址（ Windows PC | 安卓 | MAC ）及设置微信不更新09幻兽帕鲁 - 服务器管理员权限与 GM 命令完全指南102026 年 AI 大模型 & AI 编程工具实战全总结