diff --git a/.idea/misc.xml b/.idea/misc.xml new file mode 100644 index 0000000..dc5f227 --- /dev/null +++ b/.idea/misc.xml @@ -0,0 +1,7 @@ + + + + + + \ No newline at end of file diff --git a/.idea/vcs.xml b/.idea/vcs.xml new file mode 100644 index 0000000..94a25f7 --- /dev/null +++ b/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/docs/双目测距方案.md b/docs/双目测距方案.md new file mode 100644 index 0000000..5ace1fb --- /dev/null +++ b/docs/双目测距方案.md @@ -0,0 +1,432 @@ +# 双目目标测距技术规格（基于对偶检测） + +> **范围**：平台无关、语言无关的技术规格。通过立体相机对与目标检测器估计被检测目标的距离（及可选三维位置），不依赖稠密立体匹配或端到端深度网络。 +> +> **版本**：1.0（由项目专用路线图抽取并泛化而成）。 + +--- + +## 1. 目的与定位 + +### 1.1 动机 + +单目目标检测只能给出像素位置，无法提供度量距离。立体相机对提供几何约束，可将对应检测结果转换为距离。该方法适用于： + +- 只需关心少量离散目标（而非稠密环境地图）； +- 稠密立体匹配代价过高或不必要； +- 已有互补测距传感器（如激光雷达），双目估计可作为次要或近场来源。 + +### 1.2 一句话概述 + +左右图像各自独立运行轻量目标检测器；从对应检测框的边缘（或中心）提取视差；通过经典立体公式 \(Z = F \cdot B / D\) 将视差转换为度量距离。不生成稠密对应场或深度图。 + +### 1.3 能力边界 + +- 仅对被检测器报告的目标（如特定类别）测量距离； +- 有效工作距离由基线、焦距与可接受误差决定（见第 5 章）； +- 不输出稠密深度、实例掩码或目标以外的场景几何； +- 不替代主远程测距传感器，定位为近中距离的互补来源。 + +--- + +## 2. 硬件与输入假设 + +所有数值参数均为配置项，禁止硬编码。 + +| 项目 | 典型/示例值 | 说明 | +|------|-------------|------| +| 基线 \(B\) | 0.06 m（示例） | 可配置；误差与 \(1/B\) 成正比 | +| 图像分辨率 | 如 1920×1080 | 检测流水线支持的任意分辨率 | +| 帧率 | 如 30 fps | 流水线须满足所选帧率的周期 | +| 焦距 / 视场 / 传感器尺寸 | — | 须由标定或厂商数据获得 | +| 内参与外参 | — | 立体校正所必需（见第 3 章） | +| 同步方式 | 优先硬件同步 | 软件同步可行，但须量化残余误差 | +| 检测器输入尺寸 | 如 640×640 | 左右目的 letter-box / 缩放变换必须完全一致 | + +--- + +## 3. 前置条件：标定、立体校正与帧同步 + +### 3.1 标定与立体校正 + +本方法假设图像对已完成立体校正，使得： + +- 对应极线水平且对齐（同一物理点在两图中 \(y\) 坐标相同）； +- 视差纯为水平分量：\(D = x_L - x_R\)。 + +所需数据： + +1. 左右相机各自的内参（\(f_x, f_y, c_x, c_y\)）与畸变系数； +2. 两相机之间的相对旋转与平移； +3. 应用于每一帧的校正映射（或等效处理）。 + +**验证方法**：校正后收集匹配检测框，统计垂直残差 \(|\Delta y|\)。实用接受准则为 P95 \(\le 1\) px（实测后确定）。若残差过大，须进行离线自标定（如棋盘格）并在线重映射。 + +### 3.2 帧同步 + +若两路曝光时刻不一致，运动目标会产生虚假视差。 + +| 采集形态 | 同步质量 | 建议处理方式 | +|----------|----------|--------------| +| 同帧拼接单流（side-by-side） | 硬件保证同曝光 | 解码一次，裁切为左右图 | +| 双独立流 | 取决于硬件触发或软件时间戳 | 按序号 + 接收时间窗配对；监测残余时序抖动 | + +无硬件同步时，须在预期目标速度下量化距离误差，必要时限制仅用于低速场景。 + +### 3.3 时间戳约定 + +优先使用处理平台上的单调接收时间戳。不得伪造传输层未提供的采集时间戳。 + +--- + +## 4. 数学模型 + +### 4.1 符号约定 + +| 符号 | 含义 | 单位 | +|------|------|------| +| \(B\) | 基线（左右光心间距） | m | +| \(F\) | 像素焦距（校正后 \(f_x = f_y\)） | px | +| \(c_x, c_y\) | 主点 | px | +| \(x_L, y_L\) | 特征点在**左目（参考目）**图像中的像素坐标 | px | +| \(x_R\) | 同一特征点在右目图像中的像素坐标 | px | +| \(D\) | 视差 \(= x_L - x_R\) | px | +| \(Z\) | 前向距离（沿光轴分量） | m | +| \(X, Y\) | 左目坐标系下的横向 / 纵向偏移 | m | +| \(R\) | 斜距 \(\sqrt{X^2+Y^2+Z^2}\) | m | + +约定：校正后以左目为参考系；目标位于相机前方时视差为正。 + +### 4.2 核心公式（校正后坐标系） + +\[ +\begin{align*} +D &= x_L - x_R \\ +Z &= \frac{F \cdot B}{D} \\ +X &= \frac{(x_L - c_x) \cdot Z}{F} \\ +Y &= \frac{(y_L - c_y) \cdot Z}{F} \\ +R &= \sqrt{X^2 + Y^2 + Z^2} +\end{align*} +\] + +- 当主测距传感器同样报告前向距离时，优先使用 \(Z\)（前向分量）；仅在需要真实欧氏距离时使用 \(R\)。 +- 小角度下 \(Z \approx R\)；大偏航角时二者差异增大。 + +### 4.3 检测器预处理后的坐标映射 + +检测器通常接收固定尺寸的 letter-box 或缩放图像。必须遵守两条约束： + +1. **左右图像使用完全相同的几何变换**（同缩放、同填充）。否则垂直对齐性质被破坏。 +2. **视差在原始图像坐标系下计算**。将检测框映射回原生分辨率后再计算 \(D\)，可保留传感器的全部像素精度。 + +内参 \(F, c_x, c_y\) 同样须对应原生分辨率。 + +### 4.4 有损压缩的影响 + +帧内压缩（如 MJPEG）会在框边缘引入定位噪声。其对视差不确定度 \(\delta D\) 的贡献应计入第 5.2 节误差预算。若实测影响显著，可提高码流质量档位，或在带宽允许时改用无压缩 / 轻压缩格式。 + +--- + +## 5. 视差取点与误差分析 + +### 5.1 对应点选择 + +同一目标在两目中的检测框宽度很少严格相等（检测抖动、外观差异）。使用框中心会使宽度差的一半混入视差。 + +| 方法 | 主要误差来源 | 建议 | +|------|--------------|------| +| 框中心 | 宽度差直接进入视差 | 对照 / 回退方案 | +| **同侧边缘**（如两框左边缘） | 仅边缘定位噪声 | **主方案** | +| 多边缘 + 亚像素精化 | 实现成本更高 | 可选后续精化 | + +实用取法： + +\[ +D = x_L^{\text{左边缘}} - x_R^{\text{左边缘}} +\] + +（若检测器直接给出左上角坐标，该值可直接使用。）当边缘测量被判定不可靠时，回退到中心视差并输出诊断警告（需节流）。 + +### 5.2 误差传播 + +由 \(D = F B / Z\) 得： + +\[ +\begin{align*} +\delta Z &\approx \frac{Z^2 \cdot \delta D}{F B} && \text{（绝对误差）} \\ +\frac{\delta Z}{Z} &\approx \frac{Z \cdot \delta D}{F B} && \text{（相对误差）} +\end{align*} +\] + +\(\delta D\) 的构成： + +- 检测框边缘定位噪声； +- 校正后残余垂直失配； +- 运动目标的时序不同步； +- 压缩伪影。 + +基线与焦距的标定误差线性传递，完成出厂或自标定后通常可忽略。 + +### 5.3 示例误差表 + +下表仅为**示意**。假设 \(B = 0.06\,\text{m}\)、\(\delta D = 1\,\text{px}\)、原生宽度 1920 px。给出两种示例水平视场；真实焦距须由标定数据代入。 + +像素焦距近似：\(F = (W/2) / \tan(\text{HFOV}/2)\)。 + +| 距离 \(Z\) | \(D\) @ \(F\approx960\,\text{px}\)（HFOV \(\approx90^\circ\)） | \(D\) @ \(F\approx1663\,\text{px}\)（HFOV \(\approx60^\circ\)） | \(\delta Z\) @ 90° | \(\delta Z\) @ 60° | +|-----------:|----------------------------------------------------------:|------------------------------------------------------------:|-------------------:|-------------------:| +| 5 m | 11.5 px | 20.0 px | 0.43 m | 0.25 m | +| 10 m | 5.8 px | 10.0 px | 1.7 m | 1.0 m | +| 15 m | 3.8 px | 6.7 px | 3.9 m | 2.3 m | +| 20 m | 2.9 px | 5.0 px | 6.9 m | 4.0 m | +| 30 m | 1.9 px | 3.3 px | 15.6 m | 9.0 m | +| 50 m | 1.2 px | 2.0 px | 43 m | 25 m | +| 100 m| 0.6 px | 1.0 px | 174 m | 100 m | + +\(\delta D\) 加倍则表中所有误差加倍。取得真实焦距后必须重算本表。 + +### 5.4 有效工作距离 + +实用有效性判据（可调）： + +\[ +\text{valid} \;\Leftrightarrow\; D \ge D_{\min}\;\text{（如 3 px）} \;\land\; \delta Z(\delta D=1\,\text{px}) \le Z_{\text{tol}}\;\text{（如 2 m）}. +\] + +按上表示例，广角镜头有效域约 \(\le 15\,\text{m}\)，较窄视场约 \(\le 20\,\text{m}\)。域外估计标记为无效；方位信息仍可发布，距离由其他传感器承担。 + +敏感性：误差 \(\propto 1/B\)。基线加倍可使误差减半、有效域约扩大一倍——这是提升远距离能力最直接的杠杆。 + +### 5.5 远距离扩展（最小实现范围之外） + +- 亚像素边缘精化（灰度重心、抛物线拟合等）； +- 距离估计的时间滤波； +- 更大基线或更长焦距； +- 相对真值传感器拟合的经验修正项； +- 优雅降级为“仅方位 + 外部距离”。 + +--- + +## 6. 左右目关联 + +**输入**：属于同一逻辑帧的两路检测结果（见第 7 章）。 + +**硬约束**（任一不满足即拒绝配对）： + +- 类别标识一致； +- 垂直残差 \(|\Delta y| \le \tau_y\)（校正后，如 2 px）； +- 视差落在目标距离上下限对应的区间内； +- 相对高度相近：\(|h_L - h_R| / \max(h_L,h_R) \le \tau_h\)（如 0.25）。 + +**代价与贪心匹配**： + +\[ +\text{cost} = \frac{|\Delta y|}{\tau_y} + \frac{|\Delta h|}{\tau_h\cdot\max(h_L,h_R)}. +\] + +按代价升序排列候选对；仅当 \(\text{cost} < 1\) 时接受；强制一一配对。 + +多数跟踪场景目标数极少（一两个），\(O(n^2)\) 贪心算法足够。匹配器应通过接口注入，以便后续可替换为匈牙利算法、网络流等而不改动调用方。 + +**失败处理**：未匹配的检测本帧不产生距离，方位仍可发布；流水线其余部分继续运行。 + +**可选连续性线索**：若存在轨迹标识，优先匹配相同 track id 的框。 + +--- + +## 7. 单周期数据流水线 + +目标周期为所选帧率的倒数（如 30 fps 时 \(\le 33\,\text{ms}\)）。 + +``` +采集（优先硬件同步的立体对） + → 解码 / 去马赛克 / 色彩转换 + → 几何预处理（校正映射、左右完全一致的 letter-box / 缩放） + → 左右图像并行推理 + → 检测框映射回原生分辨率 + → 关联（第 6 章） + → 视差 → (Z, X, Y, R) + → 有效性判定（第 5.4 节） + → 可选时间滤波 + → 发布结果 +``` + +- **配对键**：共用帧序号（或序号不可用时采用紧接收时间窗）。 +- **时间滤波（轻量）**：对 \(Z\) 做一阶低通，配合跳变重置门限。跨帧关联可用类别 + 最近邻连续性；完整多目标跟踪为可选项。 +- **失败路径**：任一阶段失败（解码、推理、关联）导致丢帧或发布 `valid = false` 消息。失败日志须节流。 + +--- + +## 8. 图像通路考虑 + +具体编解码与加速硬件因平台而异，此处仅陈述逻辑要求。 + +- 优先从相机缓冲到校正、letter-box 直至检测器输入张量的零拷贝通路。 +- 校正与 letter-box 对左右两目必须位级一致。 +- 若采集格式为有损，须量化其对边缘噪声的贡献（见第 4.4 节）。 + +--- + +## 9. 性能预算 + +具体数值取决于所选检测器、加速器与图像尺寸。关键架构要求是： + +> 左右推理**必须并发执行**。串行执行通常使时延翻倍，可能无法满足帧周期预算。 + +代表性预算骨架（用实测值填充）： + +| 阶段 | 预算（示意） | +|------|--------------| +| 采集周期 | 1 / 帧率 | +| 解码 + 预处理 | 待实测 | +| 左目推理 | 单设备 / 单核上的检测器时延 | +| 右目推理 | 与左目并发 | +| 关联 + 测距 | 少量目标时 < 1 ms | +| 端到端时延 | 实测；须为控制或融合消费方留有余量 | + +--- + +## 10. 模块分解（语言无关） + +以下逻辑模块将算法、I/O 与配置清晰分离。 + +| 模块 | 职责 | +|------|------| +| StereoRanger | 纯算法：关联、视差提取、距离计算、有效性、可选滤波。接收检测结果 + 相机参数，输出距离消息。可完全单元测试。 | +| 检测前端 | 两个独立检测实例（或同一实例调用两次），输出原生坐标下的框。 | +| 图像采集与预处理 | 采集、解码、校正、左右完全一致的几何变换。 | +| 配置 | 基线、内参、视差上下限、关联阈值、滤波系数——全部外置。 | + +建议的结果记录（字段名仅为示意）： + +``` +header // 时间戳、序号、健康状态 +frame_sequence +class_id +track_id // 未知时为 0 +forward_distance_m // Z +slant_range_m // R（可选） +disparity_px +confidence // [0,1] +valid +centre_pixel_x_mid // 虚拟双目中心（见第 11.2 节） +bbox_* // 参考目框，供诊断 +``` + +--- + +## 11. 传感器融合与控制接口 + +### 11.1 距离源优先级（示例策略） + +``` +主传感器有效 → 使用主传感器 +主传感器无效 / 超龄， + 双目有效 → 使用双目 +双目超出有效距离域 → 标记无效 +两者均不可用 → “无距离”降级模式 +``` + +### 11.2 交叉校验 + +两者同时有效且 \(|Z_{\text{stereo}} - d_{\text{primary}}|\) 超过相对或绝对阈值时，记录节流警告并以主传感器为准。注意 \(Z\) 为前向分量；大偏航角时与纯径向测量存在差异。 + +### 11.3 方位：虚拟双目中心 + +每枚物理相机相对模组中线偏移 \(\pm B/2\)。直接使用单目像素偏移会引入恒定角度偏差 + +\[ +\beta_{\text{bias}} \approx \arctan\bigl((B/2)/Z\bigr) +\] + +近距离时不可忽略（如 \(B=60\,\text{mm}\) 时 3 m 处约 0.6°）。 + +校正后，位于基线中点的**虚拟相机**对同一目标的投影恰为两目像素的平均值： + +\[ +\begin{align*} +x_{\text{mid}} &= \frac{x_L + x_R}{2}, \\ +y_{\text{mid}} &= y_L = y_R. +\end{align*} +\] + +因此： + +- 存在立体匹配时，发布 \(x_{\text{mid}}\)（及 \(y_{\text{mid}}\)）作为方位源； +- 仅单目检出时，回退到该目原始坐标（接受偏差），或在存在最近有效距离时施加单侧修正 \(\Delta x = F\cdot(B/2)/Z\)。 + +模组中线与机体中轴之间的固定安装偏移属于外参标定问题，与双目几何无关。 + +### 11.4 互补工作域 + +双目测距通常在近中距离最准确，而窄波束主传感器在此区间可能丢靶；主传感器在远距离（双目视差坍缩处）保持优势。简单的优先级或融合规则即可实现全程覆盖。 + +--- + +## 12. 建议实施阶段 + +| 阶段 | 内容 | 出口条件 | +|------|------|----------| +| **v1.0** | 同步对采集、一致预处理、双路检测、同侧边缘视差、贪心关联、\(Z=F B/D\)、有效性标志、结果发布、可选与主测距传感器融合 | 标定数据可用；实测误差与第 5 章模型一致；基本时延预算满足 | +| **v1.1** | 出厂校正不足时的自标定 / 重映射、亚像素边缘精化、时间滤波、track-id 关联、相对真值的经验偏差修正 | 具备 v1.0 外场数据 | +| **v2.0**（可选） | 用实例分割掩码替代检测框、评估学习式立体或单目深度网络、更大基线或更长焦距硬件、框内局部稠密精化 | 单独可行性研究 | + +--- + +## 13. 验证清单 + +| 项目 | 方法 | 通过标准（示意） | +|------|------|------------------| +| 校正质量 | 静态场景，匹配框 \(|\Delta y|\) 直方图 | P95 \(\le 1\) px（实测后调整） | +| 测距精度 | 多个已知距离的静态标靶 + 真值传感器 | 误差趋势与第 5.3 节一致；有效域内 \(\delta Z\) 在容限内 | +| 同步性 | 目标快速横向运动 | 无系统性虚假视差 | +| 时延与吞吐 | 并发负载下的端到端计时 | 满足所选帧率预算 | +| 融合 / 降级 | 主传感器丢失、超龄、双目超距的回放或仿真 | 所有降级路径行为符合规格 | +| 日志纪律 | 注入失败 | 仅输出节流后的诊断信息 | + +--- + +## 14. 设计取舍记录 + +| 决策 | 理由 | +|------|------| +| 不做稠密立体匹配 | 稀疏目标无需；全分辨率对应计算代价高 | +| 几何公式而非学习深度 | 可解释、无需额外训练数据，并为后续网络评估提供干净真值基线 | +| 贪心关联 | 目标基数极小；接口保留后续升级空间 | +| 前向分量 \(Z\) 作为主距离输出 | 与多数单点测距传感器语义对齐 | +| 虚拟双目中心作为方位源 | 消除物理相机偏移引起的恒定近场角度偏差 | +| 同侧边缘视差 | 抵消两目框宽差 | +| 双目作为次要 / 互补源 | 绝对误差随 \(Z^2\) 增长；方法在主传感器最弱处价值最大 | + +--- + +## 15. 开放问题 / 配置检查清单 + +具体部署前须用真实数值填补以下项： + +1. 焦距（或完整内参）与畸变模型。 +2. 出厂立体标定是否可用及其格式；否则规划自标定。 +3. 采集形态（同帧拼接单流 vs 双流）与同步方式。 +4. 有损压缩对边缘噪声的实测贡献。 +5. 目标计算设备上所选检测器的实测时延（并确认左右推理可并发）。 +6. 可接受的绝对 / 相对距离误差及由此确定的有效工作包络。 +7. 未来硬件是否考虑更大基线或不同光学配置。 + +--- + +## 附录 A – 公式速查 + +\[ +\begin{align*} +D &= x_L - x_R \\ +Z &= \frac{F B}{D} \\ +\delta Z &\approx \frac{Z^2\,\delta D}{F B} \\ +x_{\text{mid}} &= \frac{x_L + x_R}{2} +\end{align*} +\] + +有效性（示例）：\(D \ge 3\,\text{px}\) 且 \(\delta Z(\delta D=1\,\text{px}) \le 2\,\text{m}\)。 + +--- + +*文档结束。* diff --git a/utils/demo_stereo_ranger.py b/utils/demo_stereo_ranger.py new file mode 100644 index 0000000..a7741e6 --- /dev/null +++ b/utils/demo_stereo_ranger.py @@ -0,0 +1,96 @@ +"""stereo_ranger demo：合成多目标场景演示单帧 update() 全流程。 + +本 demo 不使用任何检测器：按已知 (X, Y, Z) 目标解析投影生成左右目 +检测框（模拟"外部检测器输出，框已在校正后原生坐标系"），并叠加 ±1 px +亚像素抖动，随后用 StereoRanger 测距并对照真值。 + +运行（pytorch 环境）： + python utils/demo_stereo_ranger.py [--config path/to/stereo.json] + +依赖仅为标准库 + stereo_ranger 包（将 utils/ 加入 PYTHONPATH 或于 utils/ 下运行）。 +""" + +import argparse +import random +import sys +from pathlib import Path + +sys.path.insert(0, str(Path(__file__).resolve().parent)) + +from stereo_ranger import Detection, StereoConfig, StereoRanger, focal_from_hfov, load_from_json, range_error_abs + +# 模拟相机：1920x1080，HFOV 60°（文档 §5.3 示例之一） +IMAGE_W = 1920 +IMAGE_H = 1080 +HFOV_DEG = 60.0 +BASELINE_M = 0.06 + + +def project_to_boxes(x_m, y_m, z_m, fx, fy, cx, cy, b, rng, w_m=0.5, h_m=0.3): + """目标物理宽高 -> 左右目框；叠加亚像素抖动模拟检测噪声（种子固定）。""" + w_px = fx * w_m / z_m + h_px = fy * h_m / z_m + u_l = fx * x_m / z_m + cx + v_l = fy * y_m / z_m + cy + u_r = u_l - fx * b / z_m + jitter = lambda: rng.uniform(-1.0, 1.0) # ±1 px 框边缘抖动 + left = Detection(u_l - w_px / 2 + jitter(), v_l - h_px / 2 + jitter(), + u_l + w_px / 2 + jitter(), v_l + h_px / 2 + jitter(), + conf=0.95, cls_id=0, track_id=1) + right = Detection(u_r - w_px / 2 + jitter(), v_l - h_px / 2 + jitter(), + u_r + w_px / 2 + jitter(), v_l + h_px / 2 + jitter(), + conf=0.9, cls_id=0, track_id=1) + return left, right + + +def main() -> None: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--config", default=None, help="StereoConfig JSON 文件（可选）") + args = parser.parse_args() + + cfg = load_from_json(args.config) if args.config else StereoConfig( + fx_px=focal_from_hfov(IMAGE_W, HFOV_DEG), + fy_px=focal_from_hfov(IMAGE_W, HFOV_DEG), + cx_px=IMAGE_W / 2.0, cy_px=IMAGE_H / 2.0, + baseline_m=BASELINE_M, + range_tol_m=2.0, + ) + ranger = StereoRanger(cfg) + rng = random.Random(20260909) + + targets = [(0.0, 0.0, 5.0), (2.0, 0.5, 8.0), (-1.5, -0.4, 12.0)] + left_dets, right_dets = [], [] + for x_m, y_m, z_m in targets: + l, r = project_to_boxes(x_m, y_m, z_m, cfg.fx_px, cfg.fy_px, cfg.cx_px, cfg.cy_px, + cfg.baseline_m, rng) + left_dets.append(l) + right_dets.append(r) + # 一个只在左目检出的干扰框 -> 应输出 matched=False 的方位记录 + left_dets.append(Detection(100.0, 900.0, 160.0, 950.0, conf=0.6, cls_id=9)) + + reports = ranger.update(left_dets, right_dets, frame_sequence=1, timestamp_s=0.0) + + print(f"内参: F={cfg.fx_px:.1f}px cx={cfg.cx_px:.0f} B={cfg.baseline_m:.3f}m " + f"Z 窗口 [{cfg.z_min_m}, {cfg.z_max_m}]m") + print(f"{'真值 Z':>8} {'估计 Z':>8} {'真值 X':>7} {'估计 X':>7} {'视差':>6} {'δZ估计':>7} " + f"{'有效':>5} {'x_mid':>7} 备注") + for i, r in enumerate(reports): + if not r.matched: + print(f"{'-':>8} {'-':>8} {'-':>7} {'-':>7} {'-':>6} {'-':>7} {'-':>5} " + f"{r.centre_pixel_x_mid:7.1f} 左目未配对（方位直发）") + continue + truth = targets[i] + print(f"{truth[2]:8.2f} {r.forward_distance_m:8.2f} {truth[0]:7.2f} {r.lateral_m:7.2f} " + f"{r.disparity_px:6.1f} {r.error_est_m:7.2f} {str(r.valid):>5} " + f"{r.centre_pixel_x_mid:7.1f} src={r.disparity_source}") + print(f"\n共 {len(reports)} 条记录（{sum(1 for r in reports if r.valid)} 条有效）") + + print("\n误差模型示意（规格 §5.3，δD = 1 px）：") + print(f"{'Z (m)':>7} {'δZ (m)':>8}") + for z in [5, 10, 15, 20, 30]: + err = range_error_abs(z, cfg.fx_px, cfg.baseline_m, 1.0) + print(f"{z:7d} {err:8.2f}") + + +if __name__ == "__main__": + main() diff --git a/utils/stereo_ranger/README.md b/utils/stereo_ranger/README.md new file mode 100644 index 0000000..9cd2c55 --- /dev/null +++ b/utils/stereo_ranger/README.md @@ -0,0 +1,65 @@ +# stereo_ranger（Python） + +双目稀疏目标测距（规格：`docs/双目测距方案.md`）。输入左右目各自、**校正后原生坐标系**下的检测框，输出目标的 \(Z, X, Y, R\) 与方位。不做目标识别（YOLO）、不做图像采集与立体校正——对应规格 §10 中可独立替换的检测前端与采集预处理模块。 + +仅依赖标准库（`math`/`logging`），不 import 本仓库任何其他模块。 + +## 接口清单 + +| 名称 | 说明 | +| --- | --- | +| `Detection(x1, y1, x2, y2, conf=1.0, cls_id=0, track_id=0)` | 输入检测框（px，左上/右下角） | +| `StereoConfig(...)` | 配置（默认值 = 规格示例值，见下） | +| `load_from_json(path)` | 从 JSON 构建配置（缺字段/未知字段/非法值抛 `ValueError`） | +| `StereoRanger(config=None)` | 测距器主类 | +| `.update(left_dets, right_dets, frame_sequence=0, timestamp_s=None) -> list[RangeReport]` | 单帧主流程（返回记录数与左目检测数一致、顺序一致） | +| `.reset()` / `.config` | 清空滤波状态 / 当前配置 | +| `RangeReport` | 输出记录（字段见 `types.py`） | +| `focal_from_hfov(w_px, hfov_deg)` / `range_error_abs(z, fx, B, δD=1)` / `range_error_rel(...)` | 工具函数（§5.2/§5.3） | + +## 与检测器的约定（规格 §4.3） + +1. 左右图像必须使用**完全一致**的预处理几何（同 letter-box、同缩放），由检测前端保证； +2. 传入本模块的框必须已映射回**原生分辨率**（视差在原生坐标计算），内参 `fx/fy/cx/cy` 亦对应原生分辨率； +3. 图像对须已完成立体校正（同一目标在两目 y 坐标一致）；校正残留由 `vertical_tau_px` 容错； +4. YOLO 推理及左右目并行执行由调用方负责，本模块只收检测结果。 + +## 单帧流程（对照规格） + +硬约束过滤（类别一致、`|Δy| ≤ τy`、高度比 `≤ τh`、视差落在 `FB/z_max ~ FB/z_min` 区间）→ 代价 `|Δy|/τy + |Δh|/(τh·max(hL,hR))` 升序贪心一一配对（严格 `cost < 1`；同 `track_id` 优先）→ 主取**同侧左边缘**视差，越出关联窗口时按配置回退中心视差（节流警告）→ `Z=FB/D`、`X=(xL−cx)Z/F`、`Y=(yL−cy)Z/F`、`R=√(X²+Y²+Z²)` → 有效性 `D ≥ D_min 且 δZ ≤ Z_tol` → 可选一阶低通滤波（按 (cls, track) 分键，跳变超门限重置；`track_id=0` 不过滤）。 + +## 输出语义 + +- `matched=True, valid=True`：有效测距； +- `matched=True, valid=False`：已配对但超出有效工作域（§5.4），距离/坐标仍发布供诊断，方位可用； +- `matched=False, valid=False`：左目检测本帧未配对（§6 失败处理），仅单目方位回退。 + +方位源 `centre_pixel_x_mid` 为基线中点虚拟相机投影（§11.3）；注意 `X` 属**左目坐标系**（§4.2），二者相差 `B/2`，勿混用。 + +## StereoConfig 关键项 + +| 字段 | 默认 | 说明 | +| --- | --- | --- | +| `fx_px/fy_px/cx_px/cy_px` | 960/960/960/540 | 校正后内参（原生分辨率） | +| `baseline_m` | 0.06 | 基线 B | +| `z_min_m / z_max_m` | 1.0 / 50.0 | 距离窗口（派生视差关联区间） | +| `vertical_tau_px` / `height_ratio_tau` | 2.0 / 0.25 | 关联阈值 τy / τh | +| `disparity_min_px` / `range_tol_m` | 3.0 / 2.0 | 有效性判据 D_min / Z_tol | +| `disp_noise_px` | 1.0 | 误差模型 δD（用于 δZ 估计与有效性） | +| `use_center_fallback` | true | 边缘视差越窗时回退中心 | +| `filter_enabled` / `filter_alpha` / `filter_jump_ratio` | false / 0.3 / 0.2 | 时间滤波 | +| `warn_interval_s` | 5.0 | 诊断日志节流间隔 | + +## 运行 + +```bash +cd utils +"D:/program file/conda/envs/pytorch/python.exe" -m pytest -q # 全部测试（149 条） +"D:/program file/conda/envs/pytorch/python.exe" demo_stereo_ranger.py # 合成场景 demo +``` + +## 未覆盖边界（属其他模块/后续版本） + +- 图像采集、立体校正 remap、帧同步、side-by-side 裁切（规格 §3）； +- YOLO/检测器及其预处理（由调用方按上文约定接入）； +- 亚像素边缘精化、跨帧跟踪（`track_id` 由外部追踪器提供后，时间滤波即可用）、与主测距传感器融合（§5.5/§11）。 diff --git a/utils/stereo_ranger/__init__.py b/utils/stereo_ranger/__init__.py new file mode 100644 index 0000000..8a11b35 --- /dev/null +++ b/utils/stereo_ranger/__init__.py @@ -0,0 +1,30 @@ +"""stereo_ranger —— 双目稀疏目标测距（规格：docs/双目测距方案.md）。 + +纯算法包：输入校正后原生坐标系的左右目检测框，输出目标的 (Z, X, Y, R) +与方位。不做目标识别、图像采集与立体校正；仅依赖标准库。 +""" + +from .config import StereoConfig, load_from_json +from .stereo_ranger import ( + DISP_SOURCE_CENTER, + DISP_SOURCE_EDGE, + StereoRanger, + focal_from_hfov, + range_error_abs, + range_error_rel, +) +from .types import BBoxXYXY, Detection, RangeReport + +__all__ = [ + "BBoxXYXY", + "DISP_SOURCE_CENTER", + "DISP_SOURCE_EDGE", + "Detection", + "RangeReport", + "StereoConfig", + "StereoRanger", + "focal_from_hfov", + "load_from_json", + "range_error_abs", + "range_error_rel", +] diff --git a/utils/stereo_ranger/config.py b/utils/stereo_ranger/config.py new file mode 100644 index 0000000..ba8ef07 --- /dev/null +++ b/utils/stereo_ranger/config.py @@ -0,0 +1,82 @@ +"""StereoConfig 与 JSON 配置加载。 + +规格 docs/双目测距方案.md：全部数值参数均为配置项（§2），默认值取文档 +示例值。配置校验在构造期一次性完成，热路径不读 JSON。 +""" + +import json +from dataclasses import dataclass, fields +from pathlib import Path +from typing import Any, Dict, Tuple + + +@dataclass +class StereoConfig: + # ---- 相机参数（校正后、对应原生分辨率，§3.1 / §4.3）---- + fx_px: float = 960.0 + fy_px: float = 960.0 + cx_px: float = 960.0 + cy_px: float = 540.0 + baseline_m: float = 0.06 # 基线 B + + # ---- 目标距离窗口：关联硬约束（§6），派生视差区间 [FB/z_max, FB/z_min] ---- + z_min_m: float = 1.0 + z_max_m: float = 50.0 + + # ---- 关联阈值（§6）---- + vertical_tau_px: float = 2.0 # 垂直残差 tau_y + height_ratio_tau: float = 0.25 # 相对高度差 tau_h + + # ---- 视差取点与有效性（§5）---- + disparity_min_px: float = 3.0 # D_min：D 低于此值直接无效 + range_tol_m: float = 2.0 # Z_tol：δZ <= Z_tol 才算有效 + disp_noise_px: float = 1.0 # δD（误差模型中的边缘定位噪声） + use_center_fallback: bool = True # 左边缘视差越窗时回退中心（§5.1） + + # ---- 可选时间滤波（§7，一阶低通 + 跳变重置）---- + filter_enabled: bool = False + filter_alpha: float = 0.3 + filter_jump_ratio: float = 0.2 # 相对跳变超此门限则重置滤波器 + + # ---- 诊断日志节流 ---- + warn_interval_s: float = 5.0 + + def disparity_bounds_px(self) -> Tuple[float, float]: + """关联用的视差区间 (lo, hi)：由 z_min/z_max 派生（§6）。""" + fb = self.fx_px * self.baseline_m + return fb / self.z_max_m, fb / self.z_min_m + + def __post_init__(self) -> None: + _require(self.fx_px > 0.0, "fx_px", "必须为正") + _require(self.fy_px > 0.0, "fy_px", "必须为正") + _require(self.cx_px >= 0.0, "cx_px", "必须非负") + _require(self.cy_px >= 0.0, "cy_px", "必须非负") + _require(self.baseline_m > 0.0, "baseline_m", "必须为正") + _require(0.0 < self.z_min_m < self.z_max_m, "z_min_m/z_max_m", "须满足 0 < z_min_m < z_max_m") + _require(self.vertical_tau_px > 0.0, "vertical_tau_px", "必须为正") + _require(self.height_ratio_tau > 0.0, "height_ratio_tau", "必须为正") + _require(self.disparity_min_px > 0.0, "disparity_min_px", "必须为正") + _require(self.range_tol_m > 0.0, "range_tol_m", "必须为正") + _require(self.disp_noise_px > 0.0, "disp_noise_px", "必须为正") + _require(0.0 < self.filter_alpha <= 1.0, "filter_alpha", "须落在 (0, 1]") + _require(self.filter_jump_ratio >= 0.0, "filter_jump_ratio", "必须非负") + _require(self.warn_interval_s >= 0.0, "warn_interval_s", "必须非负") + + +def load_from_json(path: str) -> StereoConfig: + """从 JSON 文件构建配置。字段缺失或非法取值抛 ValueError（携带字段名）。""" + with open(path, encoding="utf-8") as f: + data: Dict[str, Any] = json.load(f) + known = {field.name for field in fields(StereoConfig)} + unknown = set(data) - known + if unknown: + raise ValueError(f"未知配置字段: {sorted(unknown)}") + missing = known - set(data) + if missing: + raise ValueError(f"缺少配置字段: {sorted(missing)}") + return StereoConfig(**data) + + +def _require(ok: bool, field_name: str, message: str) -> None: + if not ok: + raise ValueError(f"配置非法: {field_name} {message}") diff --git a/utils/stereo_ranger/stereo_ranger.py b/utils/stereo_ranger/stereo_ranger.py new file mode 100644 index 0000000..12c6b0f --- /dev/null +++ b/utils/stereo_ranger/stereo_ranger.py @@ -0,0 +1,215 @@ +"""StereoRanger —— 双目稀疏目标测距（纯算法，规格 docs/双目测距方案.md）。 + +输入为左右目各自、校正后原生坐标系下的检测框列表；不做目标识别，不做 +图像采集/立体校正。单帧主流程：关联（§6）→ 视差取点（§5.1）→ +Z=F·B/D 与 (X, Y, R)（§4.2）→ 有效性（§5.4）→ 可选时间滤波（§7）。 +依赖仅为标准库。 +""" + +import logging +import math +import time +from typing import Dict, List, Optional, Sequence, Tuple + +from .config import StereoConfig +from .types import BBoxXYXY, Detection, RangeReport + +logger = logging.getLogger("stereo_ranger") + +DISP_SOURCE_EDGE = "left_edge" +DISP_SOURCE_CENTER = "center" + + +def focal_from_hfov(image_width_px: float, hfov_deg: float) -> float: + """由水平视场与图像宽近似像素焦距：F = (W/2) / tan(HFOV/2)（§5.3）。""" + return (image_width_px / 2.0) / math.tan(math.radians(hfov_deg) / 2.0) + + +def range_error_abs(z_m: float, fx_px: float, baseline_m: float, delta_d_px: float = 1.0) -> float: + """绝对距离误差估计 δZ ≈ Z^2·δD/(F·B)（§5.2）。""" + return z_m * z_m * delta_d_px / (fx_px * baseline_m) + + +def range_error_rel(z_m: float, fx_px: float, baseline_m: float, delta_d_px: float = 1.0) -> float: + """相对距离误差 δZ/Z ≈ Z·δD/(F·B)（§5.2）。""" + return z_m * delta_d_px / (fx_px * baseline_m) + + +class StereoRanger: + """双目稀疏目标测距器。 + + 用法：每帧调用一次 update()，传入同一逻辑帧的左右目检测结果。 + """ + + def __init__(self, config: Optional[StereoConfig] = None): + self._cfg = config if config is not None else StereoConfig() + self._z_state: Dict[Tuple[int, int], float] = {} # (cls_id, track_id) -> 滤波 Z + self._warn_at: Dict[str, float] = {} + + @property + def config(self) -> StereoConfig: + return self._cfg + + def reset(self) -> None: + self._z_state.clear() + self._warn_at.clear() + + def update( + self, + left: Sequence[Detection], + right: Sequence[Detection], + frame_sequence: int = 0, + timestamp_s: Optional[float] = None, + ) -> List[RangeReport]: + """单帧主流程：返回报告列表（每个左目检测一条，保持左目输入顺序）。 + + 配对成功且测距有效的报告 valid=True；配对成功但超出有效工作域的 + valid=False（距离仍给出，§5.4）；未配对的左目检测 matched=False， + 仅发布单目方位（§6 失败处理）。 + """ + pairs = self._match(left, right) + matched_r: Dict[int, int] = {l_idx: r_idx for l_idx, r_idx, _ in pairs} + reports: List[RangeReport] = [] + for l_idx, det_l in enumerate(left): + r_idx = matched_r.get(l_idx) + if r_idx is None: + reports.append(self._unmatched_report(det_l, frame_sequence, timestamp_s)) + continue + report = self._build_report(det_l, right[r_idx], frame_sequence, timestamp_s) + reports.append(report) + return reports + + # ---- 内部实现 ---- + + def _match(self, left: Sequence[Detection], right: Sequence[Detection]) -> List[Tuple[int, int, Detection]]: + """硬约束过滤 + 代价升序贪心一一配对（§6）。返回 (左下标, 右下标, 左框)。""" + cfg = self._cfg + d_lo, d_hi = cfg.disparity_bounds_px() + candidates: List[Tuple[int, int, Detection, float]] = [] + for i, det_l in enumerate(left): + for j, det_r in enumerate(right): + if det_l.cls_id != det_r.cls_id: + continue + dy = abs(det_l.cy - det_r.cy) + if dy > cfg.vertical_tau_px: + continue + h_max = max(det_l.height, det_r.height) + if h_max <= 0.0: + continue + dh_ratio = abs(det_l.height - det_r.height) / h_max + if dh_ratio > cfg.height_ratio_tau: + continue + cost = dy / cfg.vertical_tau_px + dh_ratio / cfg.height_ratio_tau + if cost >= 1.0: + continue + d_edge = det_l.x1 - det_r.x1 + if not (d_lo <= d_edge <= d_hi): + d_center = det_l.cx - det_r.cx + if cfg.use_center_fallback and (d_lo <= d_center <= d_hi): + self._warn("center_fallback", "左边缘视差越出窗口，回退中心视差（宽度差可能混入）") + else: + continue + same_track = det_l.track_id > 0 and det_l.track_id == det_r.track_id + candidates.append((0 if same_track else 1, cost, i, j, det_l)) + candidates.sort(key=lambda c: (c[0], c[1])) + matched_l: set = set() + used_r: set = set() + pairs: List[Tuple[int, int, Detection]] = [] + for _, _, i, j, det_l in candidates: + if i in matched_l or j in used_r: + continue + pairs.append((i, j, det_l)) + matched_l.add(i) + used_r.add(j) + return pairs + + def _build_report(self, det_l: Detection, det_r: Detection, seq: int, ts: Optional[float]) -> RangeReport: + cfg = self._cfg + d_edge = det_l.x1 - det_r.x1 + d_lo, d_hi = cfg.disparity_bounds_px() + if d_lo <= d_edge <= d_hi: + d = d_edge + source = DISP_SOURCE_EDGE + else: + d = det_l.cx - det_r.cx + source = DISP_SOURCE_CENTER + z = cfg.fx_px * cfg.baseline_m / d + report = RangeReport( + frame_sequence=seq, + timestamp_s=ts, + class_id=det_l.cls_id, + track_id=det_l.track_id if det_l.track_id != 0 else det_r.track_id, + matched=True, + valid=False, # §5.4 判定见下 + forward_distance_m=z, + disparity_px=d, + disparity_source=source, + confidence=min(det_l.conf, det_r.conf), + centre_pixel_x_mid=(det_l.cx + det_r.cx) / 2.0, + centre_pixel_y_mid=det_l.cy, + ref_bbox=(det_l.x1, det_l.y1, det_l.x2, det_l.y2), + ) + err = range_error_abs(z, cfg.fx_px, cfg.baseline_m, cfg.disp_noise_px) + if d >= cfg.disparity_min_px and err <= cfg.range_tol_m: + report.valid = True + z_out = self._filter_z(report.class_id, report.track_id, z) + report.forward_distance_m = z_out + report.error_est_m = range_error_abs(z_out, cfg.fx_px, cfg.baseline_m, cfg.disp_noise_px) + else: + report.error_est_m = err + self._fill_xyz(report, det_l.cx) + return report + + def _fill_xyz(self, report: RangeReport, x_l_px: float) -> None: + """由 Z 与左目（参考目）框中心像素回算左目系 X, Y, R（§4.2）。 + + 注意：方位源 centre_pixel_x_mid 属于基线中点虚拟相机（§11.3）， + 而 X 定义在左目坐标系，二者相差 B/2，不得混用。 + """ + cfg = self._cfg + z = report.forward_distance_m + x = (x_l_px - cfg.cx_px) * z / cfg.fx_px + y = (report.centre_pixel_y_mid - cfg.cy_px) * z / cfg.fy_px + report.lateral_m = x + report.vertical_m = y + report.slant_range_m = math.sqrt(x * x + y * y + z * z) + + def _filter_z(self, cls_id: int, track_id: int, z: float) -> float: + """可选一阶低通 + 跳变重置（§7）。track_id 为 0（无轨迹）时不过滤。""" + cfg = self._cfg + if not cfg.filter_enabled or track_id == 0: + return z + key = (cls_id, track_id) + prev = self._z_state.get(key) + if prev is None: + self._z_state[key] = z + return z + if abs(z - prev) / prev > cfg.filter_jump_ratio: + self._z_state[key] = z # 跳变：重置 + return z + z_out = (1.0 - cfg.filter_alpha) * prev + cfg.filter_alpha * z + self._z_state[key] = z_out + return z_out + + def _unmatched_report(self, det: Detection, seq: int, ts: Optional[float]) -> RangeReport: + return RangeReport( + frame_sequence=seq, + timestamp_s=ts, + class_id=det.cls_id, + track_id=det.track_id, + matched=False, + valid=False, + confidence=det.conf, + centre_pixel_x_mid=det.cx, + centre_pixel_y_mid=det.cy, + ref_bbox=(det.x1, det.y1, det.x2, det.y2), + ) + + def _warn(self, category: str, message: str) -> None: + """节流诊断（§7：失败日志须节流）。""" + now = time.monotonic() + last = self._warn_at.get(category) + if last is not None and now - last < self._cfg.warn_interval_s: + return + self._warn_at[category] = now + logger.warning("[%s] %s", category, message) diff --git a/utils/stereo_ranger/types.py b/utils/stereo_ranger/types.py new file mode 100644 index 0000000..fc964b2 --- /dev/null +++ b/utils/stereo_ranger/types.py @@ -0,0 +1,71 @@ +"""stereo_ranger 输入输出数据结构：Detection / RangeReport。 + +规格：docs/双目测距方案.md §10 结果记录。 +""" + +from dataclasses import dataclass +from typing import Optional, Tuple + +BBoxXYXY = Tuple[float, float, float, float] + + +@dataclass +class Detection: + """校正后原生坐标系下的一个检测框（单位 px）。 + + 调用方负责：左右目预处理几何完全一致，且框坐标已映射回原生分辨率 + （规格 §4.3）。x1/y1 为左上角，x2/y2 为右下角（x1 即"左边缘"）。 + """ + + x1: float + y1: float + x2: float + y2: float + conf: float = 1.0 + cls_id: int = 0 + track_id: int = 0 # 未知为 0 + + @property + def width(self) -> float: + return self.x2 - self.x1 + + @property + def height(self) -> float: + return self.y2 - self.y1 + + @property + def cx(self) -> float: + return (self.x1 + self.x2) / 2.0 + + @property + def cy(self) -> float: + return (self.y1 + self.y2) / 2.0 + + +@dataclass +class RangeReport: + """单目标单帧测距结果（一个左目检测对应一条记录）。 + + matched=False 表示该左目检测本帧未配对：valid 恒为 False，距离字段为 + None，centre_pixel_x/y_mid 为单目自身中心（方位回退，规格 §11.3）。 + matched=True 且 valid=False 表示已配对但距离超出有效工作域 + （规格 §5.4）：距离字段仍给出估计值供诊断，方位照常发布。 + """ + + frame_sequence: int + class_id: int + track_id: int + matched: bool + valid: bool + forward_distance_m: Optional[float] = None # Z，前向分量（主输出） + lateral_m: Optional[float] = None # X，左目系横向偏移 + vertical_m: Optional[float] = None # Y，左目系纵向偏移 + slant_range_m: Optional[float] = None # R = sqrt(X^2 + Y^2 + Z^2) + disparity_px: Optional[float] = None # D = xL - xR（>0 为前方） + disparity_source: Optional[str] = None # "left_edge" / "center" + confidence: float = 0.0 + error_est_m: Optional[float] = None # δZ 估计（δD = 配置 disp_noise_px） + centre_pixel_x_mid: float = 0.0 # 虚拟双目中心 x（方位源，§11.3） + centre_pixel_y_mid: float = 0.0 # y_mid（校正后 yL == yR） + ref_bbox: Optional[BBoxXYXY] = None # 左目（参考目）框，供诊断 + timestamp_s: Optional[float] = None diff --git a/utils/tests/stereo_ranger/config_test.py b/utils/tests/stereo_ranger/config_test.py new file mode 100644 index 0000000..612dd4a --- /dev/null +++ b/utils/tests/stereo_ranger/config_test.py @@ -0,0 +1,76 @@ +"""StereoConfig 默认值、派生区间与 JSON 加载测试（规格 §2：参数全部外置）。""" + +import json + +import pytest + +from stereo_ranger.config import StereoConfig, load_from_json + +VALID = { + "fx_px": 1663.0, "fy_px": 1663.0, "cx_px": 960.0, "cy_px": 540.0, + "baseline_m": 0.12, "z_min_m": 2.0, "z_max_m": 40.0, + "vertical_tau_px": 3.0, "height_ratio_tau": 0.3, + "disparity_min_px": 4.0, "range_tol_m": 3.0, "disp_noise_px": 1.5, + "use_center_fallback": False, "filter_enabled": True, + "filter_alpha": 0.5, "filter_jump_ratio": 0.1, "warn_interval_s": 1.0, +} + + +def test_defaults_and_disparity_window(): + cfg = StereoConfig() + assert cfg.baseline_m == 0.06 and cfg.fx_px == 960.0 + # FB = 57.6，z 窗口 [1, 50] m -> D 窗口 [57.6/50, 57.6/1] + lo, hi = cfg.disparity_bounds_px() + assert lo == pytest.approx(1.152) + assert hi == pytest.approx(57.6) + + +@pytest.mark.parametrize("kwargs", [ + {"baseline_m": 0.0}, + {"fx_px": -1.0}, + {"fy_px": 0.0}, + {"cx_px": -1.0}, + {"z_min_m": 0.0}, + {"z_max_m": 10.0, "z_min_m": 20.0}, + {"vertical_tau_px": 0.0}, + {"height_ratio_tau": 0.0}, + {"disparity_min_px": 0.0}, + {"range_tol_m": 0.0}, + {"disp_noise_px": 0.0}, + {"filter_alpha": 0.0}, + {"filter_alpha": 1.5}, + {"filter_jump_ratio": -0.1}, + {"warn_interval_s": -1.0}, +]) +def test_invalid_config_raises(kwargs): + with pytest.raises(ValueError): + StereoConfig(**kwargs) + + +def test_load_from_json_roundtrip(tmp_path): + p = tmp_path / "stereo.json" + p.write_text(json.dumps(VALID), encoding="utf-8") + cfg = load_from_json(str(p)) + for field, value in VALID.items(): + assert getattr(cfg, field) == value + lo, hi = cfg.disparity_bounds_px() + assert hi == pytest.approx(1663.0 * 0.12 / 2.0) # FB / z_min + assert lo == pytest.approx(1663.0 * 0.12 / 40.0) # FB / z_max + + +def test_load_from_json_missing_field_reports_name(tmp_path): + p = tmp_path / "stereo.json" + data = dict(VALID) + del data["baseline_m"] + p.write_text(json.dumps(data), encoding="utf-8") + with pytest.raises(ValueError, match="baseline_m"): + load_from_json(str(p)) + + +def test_load_from_json_unknown_field_rejected(tmp_path): + p = tmp_path / "stereo.json" + data = dict(VALID) + data["magic_extra"] = 1 + p.write_text(json.dumps(data), encoding="utf-8") + with pytest.raises(ValueError, match="magic_extra"): + load_from_json(str(p)) diff --git a/utils/tests/stereo_ranger/stereo_ranger_test.py b/utils/tests/stereo_ranger/stereo_ranger_test.py new file mode 100644 index 0000000..6d69a97 --- /dev/null +++ b/utils/tests/stereo_ranger/stereo_ranger_test.py @@ -0,0 +1,302 @@ +"""stereo_ranger 测距核心单元测试。 + +规格：docs/双目测距方案.md §4（几何）、§5（视差/有效性/误差）、§6（关联）、 +§7（滤波）。场景全部由"目标在左目系 (X, Y, Z)"解析投影生成左右框， +再反推验证测距/方位输出。 +""" + +import math + +import pytest + +from stereo_ranger.config import StereoConfig +from stereo_ranger.stereo_ranger import ( + DISP_SOURCE_CENTER, + DISP_SOURCE_EDGE, + StereoRanger, + focal_from_hfov, + range_error_abs, + range_error_rel, +) +from stereo_ranger.types import Detection + +CFG = StereoConfig() +FB = CFG.fx_px * CFG.baseline_m # 57.6 px·m + + +def box(cx, cy, w=100.0, h=60.0, cls_id=0, conf=1.0, track_id=0): + return Detection(cx - w / 2, cy - h / 2, cx + w / 2, cy + h / 2, + conf, cls_id, track_id) + + +def target_pair(x_m, y_m, z_m, w=100.0, h=60.0, cls_id=0, conf=1.0, track_id=0): + """目标中心在左目系 (x_m, y_m, z_m)，解析投影为左右目同尺寸框。""" + u_l = CFG.fx_px * x_m / z_m + CFG.cx_px + v_l = CFG.fy_px * y_m / z_m + CFG.cy_px + u_r = u_l - FB / z_m + return (box(u_l, v_l, w, h, cls_id, conf, track_id), + box(u_r, v_l, w, h, cls_id, conf, track_id)) + + +def one_report(ranger, left, right, **kw): + reports = ranger.update(left, right, **kw) + assert len(reports) == len(left) + return reports + + +# ---- §4.2 / §5.3 几何与误差公式 ---- + +def test_focal_from_hfov_matches_doc_examples(): + assert focal_from_hfov(1920.0, 90.0) == pytest.approx(960.0) + assert focal_from_hfov(1920.0, 60.0) == pytest.approx(1662.769) + + +def test_range_error_matches_doc_table(): + # 文档 §5.3 示意表：B=0.06, F=960, δD=1 px + expected = {5: 0.4340, 10: 1.7361, 15: 3.9063, 20: 6.9444, + 30: 15.625, 50: 43.4028, 100: 173.611} + for z, err in expected.items(): + assert range_error_abs(z, 960.0, 0.06, 1.0) == pytest.approx(err, rel=1e-3) + assert range_error_rel(10.0, 960.0, 0.06, 1.0) == pytest.approx(1.7361 / 10.0, rel=1e-3) + + +def test_on_axis_target_z_and_bearing(): + left, right = target_pair(0.0, 0.0, 10.0) + r = one_report(StereoRanger(), [left], [right], frame_sequence=7, timestamp_s=1.5)[0] + assert r.matched and r.valid + assert r.forward_distance_m == pytest.approx(10.0, rel=1e-9) + assert r.disparity_px == pytest.approx(FB / 10.0) + assert r.disparity_source == DISP_SOURCE_EDGE + assert r.lateral_m == pytest.approx(0.0, abs=1e-9) + assert r.vertical_m == pytest.approx(0.0, abs=1e-9) + assert r.slant_range_m == pytest.approx(10.0, rel=1e-9) + assert r.frame_sequence == 7 and r.timestamp_s == 1.5 + # 虚拟双目中心：两目中心平均，y 取左目（§11.3） + assert r.centre_pixel_x_mid == pytest.approx((left.cx + right.cx) / 2.0) + assert r.centre_pixel_y_mid == left.cy + + +def test_off_axis_target_xyz_and_slant_range(): + left, right = target_pair(1.2, 0.3, 10.0) + r = one_report(StereoRanger(), [left], [right])[0] + assert r.valid + assert r.lateral_m == pytest.approx(1.2, rel=1e-9) + assert r.vertical_m == pytest.approx(0.3, rel=1e-9) + assert r.slant_range_m == pytest.approx(math.sqrt(1.2**2 + 0.3**2 + 10.0**2), rel=1e-9) + assert r.error_est_m == pytest.approx(range_error_abs(10.0, CFG.fx_px, CFG.baseline_m, CFG.disp_noise_px)) + + +def test_x_uses_left_camera_frame_not_virtual_center(): + # X 定义在左目系（§4.2），用左目中心像素；方位 x_mid 是虚拟相机量（§11.3） + left, right = target_pair(0.0, 0.0, 10.0) + r = one_report(StereoRanger(), [left], [right])[0] + assert r.lateral_m == pytest.approx(0.0, abs=1e-9) # 目标在左光轴上 + assert r.centre_pixel_x_mid < left.cx # 但虚拟中心相对左光轴偏左 B/2 + + +def test_width_diff_on_right_edge_only_does_not_taint_left_edge(): + # §5.1 主方案：同侧（左）边缘对视差免疫"宽度差"。右框仅向 x2 侧扩宽 + # 时，左边缘不变、中心被拉偏——边缘取点仍得精确 Z。 + left, right = target_pair(0.0, 0.0, 10.0) + right_extended = Detection(right.x1, right.y1, right.x2 + 40.0, right.y2) + r = one_report(StereoRanger(), [left], [right_extended])[0] + assert r.matched and r.valid + assert r.forward_distance_m == pytest.approx(10.0, rel=1e-9) + assert r.disparity_source == DISP_SOURCE_EDGE + assert right_extended.width != left.width # 确有宽度差 + + +def test_center_fallback_when_edge_out_of_window(caplog): + # 左框过宽使左边缘视差越出窗口：回退中心并节流警告（§5.1），同实例内不刷屏 + left, right = target_pair(0.0, 0.0, 10.0) + left_wider = box(left.cx, left.cy, w=300.0, h=60.0) + ranger = StereoRanger() + r = one_report(ranger, [left_wider], [right])[0] + r2 = one_report(ranger, [left_wider], [right])[0] + assert r.matched and r.valid + assert r.forward_distance_m == pytest.approx(10.0, rel=1e-9) + assert r.disparity_source == DISP_SOURCE_CENTER + assert r2.disparity_source == DISP_SOURCE_CENTER + assert len(caplog.records) == 1 + + +def test_center_fallback_disabled_leaves_unmatched(): + left, right = target_pair(0.0, 0.0, 10.0) + left_wider = box(left.cx, left.cy, w=300.0, h=60.0) + cfg = StereoConfig(use_center_fallback=False) + r = one_report(StereoRanger(cfg), [left_wider], [right])[0] + assert not r.matched and not r.valid + assert r.forward_distance_m is None + + +# ---- §6 关联硬约束与贪心一一配对 ---- + +def test_class_mismatch_rejected(): + left, _ = target_pair(0.0, 0.0, 10.0) + _, right = target_pair(0.0, 0.0, 10.0, cls_id=1) + r = one_report(StereoRanger(), [left], [right])[0] + assert not r.matched + + +def test_vertical_residual_beyond_tau_rejected(): + left, _ = target_pair(0.0, 0.0, 10.0) + right = box(CFG.cx_px - FB / 10.0, left.cy + 3.0) # |Δy| = 3 > τy = 2 + r = one_report(StereoRanger(), [left], [right])[0] + assert not r.matched + + +def test_height_ratio_beyond_tau_rejected(): + left, _ = target_pair(0.0, 0.0, 10.0) + _, right = target_pair(0.0, 0.0, 10.0, h=40.0) # |60-40|/60 > 0.25 + r = one_report(StereoRanger(), [left], [right])[0] + assert not r.matched + + +def test_cost_equal_one_rejected(): + # 文档 §6：仅当 cost < 1 时接受（严格小于） + # dy = 1 px -> 0.5，dh 比 = 0.125 -> 0.5，cost 精确 = 1.0 + left, _ = target_pair(0.0, 0.0, 10.0) + x_off = FB / 10.0 + right = Detection(left.x1 - x_off, left.y1 + 4.75, + left.x1 - x_off + 52.5, left.y1 + 4.75 + 52.5) + # 右框中心 = 左框中心 + 1 px（dy=1），高度 52.5（dh 比 0.125） + assert right.cy == pytest.approx(left.cy + 1.0) + r = one_report(StereoRanger(), [left], [right])[0] + assert not r.matched + + +def test_disparity_out_of_z_window_rejected(): + # 目标过近（D > FB/z_min）与过远（D < FB/z_max）均不可配对（§6） + near_l, near_r = target_pair(0.0, 0.0, 0.5) + far_l, far_r = target_pair(0.0, 0.0, 60.0) + r = one_report(StereoRanger(), [near_l], [near_r])[0] + assert not r.matched + r = one_report(StereoRanger(), [far_l], [far_r])[0] + assert not r.matched + + +def test_greedy_one_to_one_crossing_indices(): + # 目标 A(光轴上, Z=10) 与 B(右偏, Z=6)；右目框以下标交叉顺序传入。 + # 贪心须按代价配对：L0->R_A、L1->R_B，即使 R_B 在下标 0 处。 + l_a, r_a = target_pair(0.0, 0.0, 10.0) + l_b, r_b = target_pair(0.3, 0.0, 6.0) + reports = one_report(StereoRanger(), [l_a, l_b], [r_b, r_a]) + assert all(r.matched for r in reports) + assert reports[0].forward_distance_m == pytest.approx(10.0, rel=1e-9) + assert reports[1].forward_distance_m == pytest.approx(6.0, rel=1e-9) + assert reports[0].ref_bbox == (l_a.x1, l_a.y1, l_a.x2, l_a.y2) + + +def test_track_id_continuity_preference(): + # §6：存在轨迹标识时优先匹配相同 track id + l_a, r_a = target_pair(0.0, 0.0, 10.0, track_id=7) + l_b, r_b = target_pair(0.3, 0.0, 8.0, track_id=8) + reports = one_report(StereoRanger(), [l_a, l_b], [r_b, r_a]) + by_track = {r.track_id: r.forward_distance_m for r in reports} + assert by_track[7] == pytest.approx(10.0, rel=1e-9) + assert by_track[8] == pytest.approx(8.0, rel=1e-9) + + +def test_unmatched_left_reports_bearing_only(): + left, right = target_pair(0.0, 0.0, 10.0) + stray = box(500.0, 300.0, cls_id=3, conf=0.8) + reports = one_report(StereoRanger(), [left, stray], [right], frame_sequence=42, timestamp_s=1.5) + assert reports[0].matched and reports[0].valid + u = reports[1] + assert not u.matched and not u.valid + assert u.forward_distance_m is None + assert u.centre_pixel_x_mid == stray.cx and u.centre_pixel_y_mid == stray.cy + assert u.confidence == 0.8 and u.class_id == 3 + assert u.frame_sequence == 42 and u.timestamp_s == 1.5 + + +def test_empty_sides(): + ranger = StereoRanger() + assert ranger.update([], []) == [] + r = one_report(ranger, [box(100.0, 100.0)], [])[0] + assert not r.matched + + +def test_report_order_follows_left_detections(): + ranger = StereoRanger() + l1, r1 = target_pair(0.0, 0.0, 10.0, cls_id=0) + l2, r2 = target_pair(0.0, 0.0, 8.0, cls_id=2) + reports = ranger.update([l2, l1], [r1, r2]) # 输出顺序与输入左目一致 + assert [r.class_id for r in reports] == [2, 0] + assert reports[0].confidence == min(l2.conf, r2.conf) + + +# ---- §5.4 有效性判定 ---- + +def test_beyond_range_tolerance_invalid_but_distance_kept(): + left, right = target_pair(0.0, 0.0, 15.0) # δZ ≈ 3.9 m > 2 m + r = one_report(StereoRanger(), [left], [right])[0] + assert r.matched + assert not r.valid + assert r.forward_distance_m == pytest.approx(15.0, rel=1e-9) + assert r.lateral_m is not None # 方位/坐标仍发布（§5.4 域外标记无效） + assert r.error_est_m == pytest.approx(range_error_abs(15.0, CFG.fx_px, CFG.baseline_m, CFG.disp_noise_px)) + + +def test_within_range_tolerance_valid(): + left, right = target_pair(0.0, 0.0, 10.0) # δZ ≈ 1.74 m <= 2 m + r = one_report(StereoRanger(), [left], [right])[0] + assert r.valid + + +def test_invalid_when_disparity_below_dmin(): + # 放宽 range_tol 以单独暴露 D_min 判据：D = 2.3 px < 3 px -> 无效 + cfg = StereoConfig(range_tol_m=50.0) + left, right = target_pair(0.0, 0.0, 25.0) + r = one_report(StereoRanger(cfg), [left], [right])[0] + assert r.matched and not r.valid + + +# ---- §7 时间滤波（可选） ---- + +def moving_pair(z_m, track_id=1, cls_id=0): + """返回 (左框列表, 右框列表)：目标沿光轴移动，track_id 可配。""" + left = box(CFG.cx_px, CFG.cy_px, cls_id=cls_id, track_id=track_id) + right = box(CFG.cx_px - FB / z_m, CFG.cy_px, cls_id=cls_id, track_id=track_id) + return [left], [right] + + +def test_filter_smooths_and_resets_on_jump(): + cfg = StereoConfig(filter_enabled=True, filter_alpha=0.3, filter_jump_ratio=0.2, + range_tol_m=50.0) # 放宽有效域，让 10~13 m 均有效以观察滤波 + ranger = StereoRanger(cfg) + z1 = one_report(ranger, *moving_pair(10.0), frame_sequence=1)[0] + assert z1.forward_distance_m == pytest.approx(10.0, rel=1e-9) # 首帧置位 + z2 = one_report(ranger, *moving_pair(11.0), frame_sequence=2)[0] # 跳变 10% <= 20%：平滑 + assert z2.forward_distance_m == pytest.approx(0.7 * 10.0 + 0.3 * 11.0, rel=1e-9) + z3 = one_report(ranger, *moving_pair(13.0), frame_sequence=3)[0] # 跳变 26% > 20%：重置 + assert z3.forward_distance_m == pytest.approx(13.0, rel=1e-9) + + +def test_filter_skipped_without_track_id(): + cfg = StereoConfig(filter_enabled=True, filter_alpha=0.3) + ranger = StereoRanger(cfg) + one_report(ranger, *moving_pair(10.0, track_id=0), frame_sequence=1) + z2 = one_report(ranger, *moving_pair(11.0, track_id=0), frame_sequence=2)[0] + assert z2.forward_distance_m == pytest.approx(11.0, rel=1e-9) + + +def test_reset_clears_filter_state(): + cfg = StereoConfig(filter_enabled=True, filter_alpha=0.3, range_tol_m=50.0) + ranger = StereoRanger(cfg) + one_report(ranger, *moving_pair(10.0), frame_sequence=1) # 状态 = 10.0 + ranger.reset() + # 若不重置，10 -> 11 会平滑为 10.3；重置后应直接输出原始 11.0 + z = one_report(ranger, *moving_pair(11.0), frame_sequence=2)[0] + assert z.forward_distance_m == pytest.approx(11.0, rel=1e-9) + + +def test_invalid_measurements_do_not_pollute_filter_state(): + cfg = StereoConfig(filter_enabled=True, filter_alpha=0.3, range_tol_m=50.0) + ranger = StereoRanger(cfg) + one_report(ranger, *moving_pair(10.0), frame_sequence=1) + z2 = one_report(ranger, *moving_pair(25.0), frame_sequence=2)[0] # D < D_min：无效 + assert not z2.valid + z3 = one_report(ranger, *moving_pair(10.5), frame_sequence=3)[0] + # 无效帧未污染状态：仍按与首帧 10.0 平滑 + assert z3.forward_distance_m == pytest.approx(0.7 * 10.0 + 0.3 * 10.5, rel=1e-9)