简介：这份源码包面向计算机视觉学习者与需要快速验证双目测距方案的开发者，将YOLOv5目标检测与双目视觉测距整合为可直接运行的工程，可用于自动驾驶感知、机器人导航等场景的距离估计实验。包内共106个文件，以25个py源码、52个pyc缓存、18个yaml模型与数据配置为主，并配有3个sh运行脚本、Dockerfile、预训练权重、示例图片及演示视频，压缩包约20.88MB，便于按训练、检测、后处理模块理解流程。已有2849人学习下载，说明其在目标检测与立体视觉结合方向具有一定参考价值。读者可从中获得端到端工程结构、左右图像匹配与视差计算思路、模型配置与依赖环境线索，以及从检测结果到距离输出的完整实现参考，适合具备PyTorch与OpenCV基础的中级学习者研读和二次开发。

1. 从两张图到一个米级读数：这套 YOLOv5 双目测距源码到底解决了什么

白天在车间拍一对左右图，代码跑完直接给出一串带类别的框和到相机的直线距离，误差能压到厘米级——这是我第一次把这套 YOLOv5 双目测距源码完整跑起来时的状态。它把两件原本各干各的事拼在了一起：YOLOv5 负责回答「这是什么、在哪」，双目几何负责回答「离我多远」，深度学习负责检测，几何负责量距，中间用一张视差图当桥。适合谁？做抓取粗定位、AGV 避障前向测距、货架体积粗估这类项目的人，手里已经有两张同步图、又不想上激光雷达。不适合要求毫米级绝对精度的场景，那是另一套工具的事。

2. 把几何关系先钉死：Z=fB/d 里三个参数哪个最容易错

真正动手之前，把公式和单位关系固定下来，后面 debug 会少走很多弯路。这一章不讲铺开的光学，只讲这套源码里你必须认识的四个量，以及它们各自从哪来、错一步会怎么翻车。

2.1 视差的定义：为什么是 x_left 减 x_right

理想双目是水平放置、光轴平行的两个相机，同一个物点在左右图上成像位置不同，横坐标差值就是视差 d = x_L - x_R。物点越近，视差越大；无穷远处视差趋近 0。由相似三角形可得：

Z = f * B / d

• Z：物点到相机光心的深度，单位跟 B 一致
• f：焦距，单位像素（注意，不是毫米）
• B：基线，两个相机光心之间的距离，单位毫米
• d：视差，单位像素

这套源码里，f 来自标定得到的cameraMatrix的 fx，B 来自双目标定输出的平移向量 T 的模长，d 来自 SGBM 生成的视差图。三个量里任何一个量纲不对，结果直接差一个数量级，而且看起来"很像对的"，因为画面还在动、框还在跟着目标走，只是读数离谱。

提示：OpenCV 的stereoCalibrate返回的 T 是右相机相对左相机的平移向量，它的单位和你棋盘格方格边长的单位完全一致。方格填 25（毫米），T 就是毫米；方格填 0.025（米），T 就是米。中间混一次，结果就是 1000 倍的错。

2.2 焦距必须用像素，且必须和当前分辨率匹配

标定出来的 fx 带的是像素单位，它跟图像分辨率绑定。常见的翻车是这样：标定用 1280×720 跑出一套内参，实际部署时为了帧率换成了 640×360，fx 还是老的，测距结果整体偏大一倍。要么重新标定，要么按比例缩放：

import numpy as np K = np.array([[700.0, 0.0, 640.0], [0.0, 700.0, 360.0], [0.0, 0.0, 1.0]]) scale = 0.5 # 从 1280x720 降到 640x360 # 只缩 fx/fy 和主点，畸变系数与分辨率无关，不动 K_scaled = K.copy() K_scaled[0, 0] *= scale K_scaled[1, 1] *= scale K_scaled[0, 2] *= scale K_scaled[1, 2] *= scale

这段的意义是：内参矩阵里 fx、fy 和主点 cx、cy 是像素量，跟分辨率线性相关；畸变系数 k1、k2、p1、p2 是无量纲的归一化坐标系数，不需要动。缩放完建议拿一个已知距离的物体回归验证一遍，别直接信算术。

2.3 深度分辨率随距离平方衰减：先算清楚你的量程

很多人问"这套东西能测多远"，正确问法是"在多远的地方还能保持可接受的精度"。对 Z = fB/d 求导，深度误差与视差误差的关系是：

ΔZ = Z² / (f * B) * Δd

以 f=700 px、B=120 mm、视差误差 1 px 为例：

这张表是选型的核心依据。同样一套硬件，1 米处误差一厘米出头，5 米处误差三十厘米。如果你的项目要在 5 米外判断"有没有人"，那够用；要在 5 米外量货箱尺寸，趁早换方案或者加大基线。基线 B 加大能提升精度，代价是近处盲区变大、两个相机视场重叠率下降，常见做法是 B 取工作距离的 1/10 到 1/5 之间做平衡。

2.4 视差搜索范围要按最近工作距离算

SGBM 的numDisparities决定它能找到的最大视差，也就是最近能测多远。算法只会在这个范围内搜索，超出的部分变成空洞。计算方式：

f_px = 700.0 # 标定得到的 fx baseline_mm = 120.0 z_min_mm = 500.0 # 你希望最近能测到 500mm d_max = f_px * baseline_mm / z_min_mm # 168.0 px num_disp = int(np.ceil(d_max / 16.0) * 16) # 必须是 16 的整数倍 print(num_disp) # 176

numDisparities必须被 16 整除，这是 OpenCV 对 SGBM 的硬约束，不是风格问题。设小了，近处物体整片视差缺失，深度算出来是 0 或者乱值；设大了，计算量上去、远处精度还未必提升。按最近工作距离算出来再往上取整到 16 的倍数，是最省事的做法。

3. 标定与校正的完整链路：棋盘格、stereoCalibrate 到 remap

这一章是整套源码能不能用的分水岭。检测和匹配的代码网上到处都是，真正决定测距准不准的是这一步。我见过太多人跳过校正直接拿原始图做 SGBM，然后抱怨"视差图糊成一团"，其实极线根本没对齐，匹配器在错误的行上找对应点。

3.1 采集：15 对起步，覆盖四角，两个方向都要倾斜

标定板用常见的 9×6 内角点棋盘格，方格边长先用卡尺量实际值——打印时选"适应页面"会把 25 mm 缩成 24.3 mm，这个误差会等比传到基线上，属于血泪经验。采集要求：

• 左右相机同时抓图，硬件同步最好，软同步至少保证标定板静止
• 不少于 15 对有效图，20 对更稳
• 棋盘格要出现在画面四角和中区，别只在正中间拍
• 至少两个倾斜方向（绕 X 轴、绕 Y 轴各来几组），纯正对的图对解畸变帮助很小
• 左右图的棋盘格必须都被完整检出，缺一个就丢弃这一对

采集完先做一轮筛选，把角点检测失败的、模糊的、过曝的剔掉。宁要 15 对干净的，不要 30 对带噪的。

3.2 单目标定先过关，再做双目标定

顺序上，先分别标左右相机拿到各自的内参和畸变系数，检查重投影误差；再把内参用CALIB_FIX_INTRINSIC固定住，只解 R 和 T。这样两个阶段的问题不会互相污染。

import cv2 import numpy as np import glob PATTERN = (9, 6) # 内角点列数、行数 SQUARE_MM = 25.0 # 卡尺实测的方格边长，单位 mm objp = np.zeros((PATTERN[0] * PATTERN[1], 3), np.float32) objp[:, :2] = np.mgrid[0:PATTERN[0], 0:PATTERN[1]].T.reshape(-1, 2) * SQUARE_MM criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 1e-3) def collect(paths): objpoints, imgpoints = [], [] shape = None for p in paths: img = cv2.imread(p) gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) shape = gray.shape[::-1] # (w, h) ok, corners = cv2.findChessboardCorners(gray, PATTERN, None) if not ok: continue # 检不出就丢，别硬塞 corners = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria) objpoints.append(objp) imgpoints.append(corners) return objpoints, imgpoints, shape left_paths = sorted(glob.glob("data/left/*.png")) right_paths = sorted(glob.glob("data/right/*.png")) objL, imgL, shape = collect(left_paths) objR, imgR, _ = collect(right_paths) assert len(objL) == len(objR) >= 15, "左右有效样本数必须一致且不少于 15" # 单目：先各自标，看 RMS 是否可接受 rmsL, KL, dL, _, _ = cv2.calibrateCamera(objL, imgL, shape, None, None) rmsR, KR, dR, _, _ = cv2.calibrateCamera(objR, imgR, shape, None, None) print("left rms", rmsL, "right rms", rmsR) # 经验阈值：< 0.5 px # 双目：固定内参，只解 R / T flags = cv2.CALIB_FIX_INTRINSIC rms, KL, dL, KR, dR, R, T, E, F = cv2.stereoCalibrate( objL, imgL, imgR, KL, dL, KR, dR, shape, criteria=(cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 1e-6), flags=flags) baseline = float(np.linalg.norm(T)) print("stereo rms", rms, "baseline_mm", baseline)

逻辑说明：collect负责把每张图上检出的角点做亚像素精化，亚像素这一步不做，重投影误差会虚高。calibrateCamera每个相机单独调用，返回的 RMS 是重投影误差，单位像素，超过 1 px 基本说明采集质量有问题，先别往下走。stereoCalibrate里固定内参是关键，让优化只在 R、T 六个自由度上进行，收敛更稳。最后一行np.linalg.norm(T)就是基线，单位跟随SQUARE_MM，这里是毫米。

参数说明：cornerSubPix的窗口 (11,11) 是搜索半窗，棋盘格在图上比较小的时候可以降到 (5,5)。stereoCalibrate的迭代终止条件给到 100 次和 1e-6，比单目标定更严一点，因为这里的误差会直接变成基线误差。

3.3 校正三段式：stereoRectify、initUndistortRectifyMap、remap

标定完只是拿到参数，真正让左右图"行对齐"的是校正。三步固定顺序：

# 1) 求校正变换：R1/R2 让两图共面，P1/P2 是新投影矩阵，Q 是重投影矩阵 R1, R2, P1, P2, Q, roi1, roi2 = cv2.stereoRectify( KL, dL, KR, dR, shape, R, T, flags=cv2.CALIB_ZERO_DISPARITY, alpha=0) # 2) 预计算映射表，只做一次，别在循环里反复算 map1x, map1y = cv2.initUndistortRectifyMap(KL, dL, R1, P1, shape, cv2.CV_32FC1) map2x, map2y = cv2.initUndistortRectifyMap(KR, dR, R2, P2, shape, cv2.CV_32FC1) # 3) 每帧 remap def rectify(left, right): l = cv2.remap(left, map1x, map1y, cv2.INTER_LINEAR) r = cv2.remap(right, map2x, map2y, cv2.INTER_LINEAR) return l, r

alpha=0表示校正后只保留有效像素，画面会被裁掉一圈黑边，但不会引入无效区域；alpha=1保留全部像素但边缘会有黑边，做 SGBM 时黑边会产生假视差，一般选 0。CALIB_ZERO_DISPARITY保证主点在校正后仍然对齐，这样视差搜索从 0 开始就够，省一半计算量。映射表一定要在初始化阶段算好，initUndistortRectifyMap内部是逐像素查表运算，放进每帧循环里会白白吃掉几十毫秒。

3.4 验收：画极线，肉眼确认行对齐

校正做没做对，不用等测距结果，画一组极线就能看出来。在校正后的左图上取若干个点，在右图的同一行画横线，如果对应点都落在同一行上，说明极线对齐了。

def check_epipolar(left_rect, right_rect, rows=(100, 200, 300, 400)): canvas = right_rect.copy() for y in rows: cv2.line(canvas, (0, y), (canvas.shape[1], y), (0, 0, 255), 1) # 上下拼接，人工对比同一行的纹理是否对应 return np.vstack([left_rect, canvas]) cv2.imwrite("debug/epipolar_check.png", check_epipolar(*rectify(l_raw, r_raw)))

验收标准很朴素：同一行上的纹理在左右图里应该能对上，偏差控制在 1 到 2 像素以内。如果明显斜着错位，回头查两件事——左右图有没有传反，以及stereoCalibrate的输入顺序是否和stereoRectify一致。这两处顺序错一次，画面能跑，读数全废。

4. 把 YOLOv5 接上去：检测框怎么喂给视差图才不飘

检测和测距的耦合点只有一处：框。框给得太松，框里混进背景，视差中值被拉偏；框给得太紧，有效视差像素太少，读数抖。这一章讲清楚取样策略和 SGBM 参数的配合。

4.1 检测在左图做，坐标直接复用

YOLOv5 的推理输出是[x1, y1, x2, y2, conf, cls]，全部在输入图的坐标系里。因为校正后的左图和原图分辨率相同（remap不改变尺寸），检测框可以直接用在校正后的左图上取 ROI，不需要额外做坐标变换。这是整套流程里最省事的一环，前提是检测和测距用的是同一张左图、同一个分辨率。

import torch model = torch.hub.load('ultralytics/yolov5', 'yolov5s', pretrained=True) model.conf = 0.35 # 置信度阈值，按场景调 model.iou = 0.45 def detect(img_bgr): results = model(img_bgr) # xyxy 是归一化前的像素坐标，直接可用 return results.xyxy[0].cpu().numpy() # N x 6 def pick_roi(box, ratio=0.5): x1, y1, x2, y2 = box[:4].astype(int) w, h = x2 - x1, y2 - y1 # 只取框中心 ratio 比例的区域，避开框边缘的背景污染 cx, cy = x1 + w / 2, y1 + h / 2 rw, rh = int(w * ratio / 2), int(h * ratio / 2) return x1 + int(w / 2) - rw, y1 + int(h / 2) - rh, x1 + int(w / 2) + rw, y1 + int(h / 2) + rh

pick_roi里那个 0.5 是很实际的经验值。目标检测框通常比物体本身大一圈，尤其是有遮挡或者物体形状不规则的时候，框的四角大概率是背景。背景的视差和前景差得远，混进中值计算里会把结果往远处拉。取中心一半，牺牲一点像素数量换稳定性，通常划算；小目标可以放宽到 0.7，否则有效像素太少。

4.2 视差取样用中位数，别用均值

拿到 ROI 之后，在视差图上取同一块区域。SGBM 输出里无效像素通常是负数（minDisparity为 0 时无效值常见为 -16 或 0，取决于 OpenCV 版本和模式），必须先过滤，否则一算就崩。

def roi_disparity(disp, roi): x1, y1, x2, y2 = roi patch = disp[y1:y2, x1:x2].astype(np.float32) patch = patch[patch > 0] # 过滤无效视差 if patch.size < 30: # 有效点太少，判为不可信 return None # 中位数抗离群点，均值会被少量错误匹配拉偏 return float(np.median(patch)) / 16.0 # SGBM 输出放大 16 倍存储 def disparity_to_depth(d, f_px, baseline_mm): if d is None or d <= 0: return None return f_px * baseline_mm / d # 单位 mm

两个细节值得说清楚。第一，SGBM 的视差图用CV_16S存储，数值是真实视差的 16 倍，所以要除以 16，忘除的话深度会小 16 倍，而且结果看起来"挺像那么回事"，最难发现。第二，中位数比均值稳得多，ROI 里哪怕有 20% 的噪点，中位数基本不动，均值会明显偏移。

4.3 SGBM 参数按你的基线和分辨率配

参数表贴出来，每一项都对应一个具体后果：

def build_matcher(num_disp=176, block=5): cn = 1 # 灰度图 return cv2.StereoSGBM_create( minDisparity=0, numDisparities=num_disp, blockSize=block, P1=8 * cn * block * block, P2=32 * cn * block * block, disp12MaxDiff=1, uniquenessRatio=10, speckleWindowSize=100, speckleRange=2, mode=cv2.STEREO_SGBM_MODE_SGBM_3WAY) matcher = build_matcher() disp = matcher.compute(left_gray_rect, right_gray_rect)

SGBM_3WAY模式比默认模式慢一些但视差更连续，静态场景值得换。做匹配前建议对左右图各做一次 CLAHE，缓解两个相机曝光不一致导致的匹配失败，尤其是自动曝光没锁死的时候，这一步能救回不少空洞。

4.4 把距离画回画面，同时给一个置信度

最后一步是把深度值叠回图上。别只画数字，把有效视差像素数也标出来，方便现场判断这一帧的读数靠不靠谱。

def draw(frame, box, depth_mm, valid_cnt): x1, y1, x2, y2 = box[:4].astype(int) label = f"{int(box[5])}" if depth_mm: label += f" {depth_mm/1000:.2f}m" color = (0, 255, 0) if valid_cnt > 200 else (0, 200, 255) else: label += " N/A" color = (0, 0, 255) cv2.rectangle(frame, (x1, y1), (x2, y2), color, 2) cv2.putText(frame, label, (x1, y1 - 6), cv2.FONT_HERSHEY_SIMPLEX, 0.6, color, 2) return frame

用颜色区分置信度比单纯打印数字有用得多。有效像素数低于阈值时标黄，让现场的人一眼看出"这帧别信"，比事后查日志高效。多目标场景下每个框独立算深度，互不干扰，这是把检测和测距拆开做的好处。

5. 避坑排查：五个把测距搞成玄学的细节

这一章是我踩过的坑里挑出来最费时间的五个。共同点是：代码不报错、画面正常、结果就是不对，没有日志告诉你哪里错了，只能靠推理。

5.1 标定和硬件埋的雷

现象一：所有目标系统性偏大 3% 到 5%，换个距离比例基本不变。

原因几乎一定是棋盘格的实际尺寸和代码里的不一致。打印机"适应页面"缩放、纸张受潮、测量时量的外边缘而不是内角点间距，都会引入比例误差。这个误差会等比传到 T 向量的模长上，也就是基线，于是所有深度同比例偏。

解决：用卡尺量三个以上方格取平均，把真实值填进SQUARE_MM。验证方法很简单，把棋盘格放在一个已知距离（卷尺量，比如 1500 mm），跑一遍测距，看读数偏差是否在可接受范围。偏差是比例性的就改方格尺寸，是固定偏移的就查主点。

现象二：单目标定 RMS 只有 0.2 px，双目标定结果却很差，极线怎么都调不齐。

原因通常是采集时左右图不同步。标定板在移动过程中被左右相机在不同时刻拍到，角点位置对不上，stereoCalibrate解出来的 R、T 就是错的。软同步在静态拍摄时没问题，一旦手持移动就会翻车。

解决：标定时把板子放稳再触发，或者上硬件同步线。另外确认左右图的文件名严格一一对应，用sorted(glob)之后还要做一次长度和内容校验，中途丢一张图就会错位。

现象三：视差图整片是黑的，或者只有零星几个点。

先排除左右图接反。校正后左右图传反了，视差会全为负，被patch > 0全部过滤掉。检查方法：把两张图并排看，左边图的物体应该比右边图偏右一点点（对近处物体而言）。还有一种情况是画面纹理太少，白墙、纯色纸箱、金属反光面，SGBM 在无纹理区域本来就没辙。

解决：接反了就换回来，并同步检查stereoCalibrate和stereoRectify的输入顺序。纹理不足就加辅助，投一个散斑投影器是最直接的办法，成本很低；或者退而求其次，用检测框边缘的纹理（通常是有的）来估，别指望框中心一定是好纹理。

5.2 匹配和融合阶段的坑

现象四：目标边缘深度跳变剧烈，同一个箱子前面读 1.2 m、侧面读 2.5 m。

这是典型的"前后景粘连"。SGBM 的匹配窗口横跨了深度不连续处，窗口内一半是前景一半是背景，匹配代价被平均，结果落在中间。blockSize越大越明显，P2越大越平滑也越容易糊边。

解决：把blockSize降到 5 甚至 3，代价是视差图噪点变多；同时把取样区域从整框缩到中心，避开物体轮廓。如果目标是规则物体（箱子、托盘），还可以只取框下半部分，因为上半部分常常是背景。

现象五：距离读数一直在小幅跳动，同一目标静止不动，读数在 ±15 cm 之间来回摆。

三个可能叠在一起：检测框本身在抖，逐帧之间框的位置差几个像素；取样用了均值被离群点拉；以及视差图本身的量化噪声。单帧读数本来就不该被直接使用。

解决：检测框做时间维平滑（简单的指数移动平均就够），深度读数同样做 EMA：

class DepthSmoother: def __init__(self, alpha=0.3, max_jump_mm=800): self.alpha = alpha self.prev = {} self.max_jump = max_jump_mm def update(self, track_id, depth_mm): if depth_mm is None: return self.prev.get(track_id) p = self.prev.get(track_id) if p is None or abs(depth_mm - p) > self.max_jump: self.prev[track_id] = depth_mm # 首帧或突变，直接重置 else: self.prev[track_id] = self.alpha * depth_mm + (1 - self.alpha) * p return self.prev[track_id]

alpha取 0.3 意味着新观测占三成，响应不算慢，同时能压掉大部分抖动。max_jump是突变保护，防止目标真的快速靠近时被平滑拖住。目标跟踪 ID 可以用简单的 IOU 匹配串起来，不用上复杂的跟踪器。

6. 精度验证与工程化收尾：用卷尺和一段录像把误差压下去

代码跑通只是起点，能不能交付看的是误差曲线。我一般会做一张实测表：把标定板或者一个平面目标放在 0.5 m、1 m、2 m、3 m、4 m 五个位置，每个位置用卷尺量真实距离，代码连测 30 帧取中位数，算绝对误差和相对误差。

表格填完会有两个发现。第一，误差基本遵循第 2 章的平方关系，如果某一段突然跳变，说明那个距离的视差正好落在numDisparities边界附近，回去把范围放宽一档。第二，相对误差在近处往往比理论值大，因为近距离时物体占的像素多、框内混进的背景比例也高，取样比例要相应收紧。

验证方法上，除了静态点位，我还会录一段手持缓慢靠近目标的视频跑离线批处理，把每帧的读数打出来看曲线是否单调。如果曲线出现台阶状跳变，多半是视差图里主峰发生了切换，这时候uniquenessRatio往上调 2 到 3 通常能缓解。反过来，如果曲线过于平滑、跟不上真实运动，说明 EMA 的alpha给小了，或者P2给大了把边缘糊掉了。

工程化收尾还有两件事值得提前做。一是把标定参数存成npz，加载时校验shape和当前分辨率是否一致，不一致直接报错退出，别让它悄悄跑出错误结果。二是给 SGBM 加一个 ROI 版本：先用检测框把左右图裁出一个小区域再做匹配，计算量能降一个数量级，帧率从 15 fps 提到 40 fps 以上，代价是只对框内目标有视差，整幅场景的视差图就没有了——如果下游只用框内距离，这笔买卖很划算。

# 参数持久化 + 分辨率一致性校验 np.savez("calib/stereo.npz", K1=KL, D1=dL, K2=KR, D2=dR, R=R, T=T, shape=shape) def load_calib(path, cur_shape): z = np.load(path, allow_pickle=True) saved = tuple(z["shape"]) if saved != tuple(cur_shape): raise ValueError(f"标定分辨率 {saved} 与当前 {cur_shape} 不一致，请重标或缩放内参") return z["K1"], z["D1"], z["K2"], z["D2"], z["R"], z["T"]

这个校验看着啰嗦，但它替我挡过至少两次事故——一次是换相机忘了重标，一次是推理脚本里 resize 写错了参数。那次之后我每次接入新硬件，第一件事就是跑一遍分辨率校验加一组卷尺点位，不管多赶时间都不跳过。测距这种东西，错的时候不报错，只会安静地给你一个看起来很合理的数字。希望帮到你。

本文还有配套的精品资源，点击获取