新增双目测距模块v1.0（未测试）
This commit is contained in:
keita
Generated
+7
@@ -0,0 +1,7 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<project version="4">
|
||||
<component name="Black">
|
||||
<option name="sdkName" value="D:\program file\conda\envs\pytorch" />
|
||||
</component>
|
||||
<component name="ProjectRootManager" version="2" project-jdk-name="D:\program file\conda\envs\pytorch" project-jdk-type="Python SDK" />
|
||||
</project>
|
||||
Generated
+6
@@ -0,0 +1,6 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<project version="4">
|
||||
<component name="VcsDirectoryMappings">
|
||||
<mapping directory="$PROJECT_DIR$" vcs="Git" />
|
||||
</component>
|
||||
</project>
|
||||
+432
@@ -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}\)。
|
||||
|
||||
---
|
||||
|
||||
*文档结束。*
|
||||
@@ -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()
|
||||
@@ -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）。
|
||||
@@ -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",
|
||||
]
|
||||
@@ -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}")
|
||||
@@ -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)
|
||||
@@ -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
|
||||
@@ -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))
|
||||
@@ -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)
|
||||
Reference in New Issue
Block a user