双目测距方案重构与其他小修改
This commit is contained in:
keita
+27
-9
@@ -1,9 +1,12 @@
|
||||
# utils —— reference-c++ 两个子项目的 Python 移植
|
||||
# utils —— reference-c++ 的 Python 移植 + 双目测距新实现
|
||||
|
||||
将 `reference-c++/` 下的 `target_tracker`（多目标追踪过滤器）与
|
||||
`visual_track_control`（视觉跟踪控制律）移植为 Python，**两包互不依赖**
|
||||
（唯一的数据转换点在胶水层 demo）。
|
||||
|
||||
另有 `stereo_ranger`（双目稀疏目标测距）：**按规格新实现**（非 C++ 移植），
|
||||
规格 `docs/双目测距技术路线.md`；同样与既有两包互不依赖。
|
||||
|
||||
## 目录结构
|
||||
|
||||
```
|
||||
@@ -12,19 +15,24 @@ utils/
|
||||
│ └── README.md # 接口清单 + C++ 差异
|
||||
├── visual_track_control/ # 包 2：三通道控制律 + MAVSDK offboard 网关
|
||||
│ └── README.md # 接口清单 + C++ 差异
|
||||
├── stereo_ranger/ # 包 3：双目稀疏目标测距（纯标准库，无第三方依赖）
|
||||
│ └── README.md # 接口清单 + 与规格的偏差
|
||||
├── demo_track_control.py # 胶水层集成演示（模拟检测 → 追踪 → 控制律 → 发送）
|
||||
├── demo_stereo_ranger.py # 双目测距演示（合成框对，六场景，无硬件）
|
||||
├── config/track_control.json # 控制律配置（拷贝自 reference-c++）
|
||||
├── tests/ # pytest 测试（对应 C++ 全部 13 个测试文件）
|
||||
│ ├── target_tracker/ # 27 条
|
||||
│ └── visual_track_control/ # 78 条
|
||||
├── conftest.py # 保证可从任意工作目录导入两包
|
||||
├── config/stereo_ranger.json # 双目测距配置（占位值，待标定回填）
|
||||
├── tests/ # pytest 测试
|
||||
│ ├── target_tracker/ # 28 条（对应 C++ 测试文件）
|
||||
│ ├── visual_track_control/ # 81 条（对应 C++ 测试文件）
|
||||
│ └── stereo_ranger/ # 111 条（规格 §11.4 构件级 + 全流程）
|
||||
├── conftest.py # 保证可从任意工作目录导入三包
|
||||
└── pytest.ini # 收集 *_test.py 命名（与 C++ 测试文件名对齐）
|
||||
```
|
||||
|
||||
## 环境
|
||||
|
||||
conda env `pytorch`（`D:\program file\conda\envs\pytorch`），Python 3.12，
|
||||
依赖：numpy、mavsdk、pytest。
|
||||
依赖：numpy、mavsdk、pytest（stereo_ranger 仅标准库）。
|
||||
|
||||
## 运行测试
|
||||
|
||||
@@ -33,8 +41,9 @@ cd utils
|
||||
"D:/program file/conda/envs/pytorch/python.exe" -m pytest -q
|
||||
```
|
||||
|
||||
105 条测试全部通过（含 C++ 验收用例的逐条移植：§9.4 六条硬验收、A1/A2/A4
|
||||
回归、PID anti-windup C1、超龄 C4、首拍限幅 S6）。
|
||||
220 条测试全部通过：109 条 C++ 移植用例（含 §9.4 六条硬验收、A1/A2/A4
|
||||
回归、PID anti-windup C1、超龄 C4、首拍限幅 S6）+ 111 条 stereo_ranger
|
||||
（合成框对驱动，无需真实图像/硬件）。
|
||||
|
||||
## 运行演示
|
||||
|
||||
@@ -44,11 +53,17 @@ python utils/demo_track_control.py --speedup 10
|
||||
|
||||
# MAVSDK 实发（需飞控/SITL 在 udp://:14540）
|
||||
python utils/demo_track_control.py --transport mavsdk
|
||||
|
||||
# 双目测距（合成框对，离线，无相机/飞控）
|
||||
python utils/demo_stereo_ranger.py
|
||||
```
|
||||
|
||||
时间线 13 s（20 Hz）：正常跟踪 6 s（vx 从零渐变 → 逼近限速 3 m/s）→
|
||||
控制链路时间线 13 s（20 Hz）：正常跟踪 6 s（vx 从零渐变 → 逼近限速 3 m/s）→
|
||||
目标消失 4 s（Coast 预测，60 帧后轨道判亡）→ 判零速刹车 3 s。
|
||||
|
||||
双目演示六场景：逼近（三道有效性门 + 滤波）→ 远场域外（方位仍发布）→
|
||||
单目退化 → 非同帧拒收 → 一致性降级/判无效 → 多目标贪心配对。
|
||||
|
||||
## 移植约定
|
||||
|
||||
- MAVLink 部分改为 **MAVSDK 语义映射**（offboard API，非字节透传），三分支
|
||||
@@ -56,3 +71,6 @@ python utils/demo_track_control.py --transport mavsdk
|
||||
- 测试逐条移植 C++ gtest 断言（`EXPECT_DOUBLE_EQ` → 精确 `==`，
|
||||
`EXPECT_NEAR` → `pytest.approx`）；MAVSDK 部分因无 SITL/实机，仅 mock 验证。
|
||||
- 行为修复项（C1/C2/C4、A1/A2/A4、S6）在测试文件头注释中标注对应章节。
|
||||
- `stereo_ranger` 不是移植：按规格五步流程新实现，与规格的偏差（越界=有效性门、
|
||||
视差域权威、补充 consistency_jump_px/isolation_gate_px 等）见其包 README；
|
||||
⚠ 全部数值为占位值，标定完成前不得作为飞行控制决策依据。
|
||||
|
||||
@@ -4,7 +4,7 @@
|
||||
"control": {
|
||||
"frequency_hz": 20.0,
|
||||
"visual_stale_ms": 200,
|
||||
"radar_stale_ms": 500,
|
||||
"distance_stale_ms": 500,
|
||||
"attitude_stale_ms": 300
|
||||
},
|
||||
|
||||
@@ -36,8 +36,9 @@
|
||||
"derivative_filter_coef": 0.2,
|
||||
"approach_velocity_limit_mps": 3.0,
|
||||
"retreat_velocity_limit_mps": 1.0,
|
||||
"no_distance_action": "hold",
|
||||
"no_distance_approach_limit_mps": 0.5
|
||||
"no_distance_action": "slow_approach",
|
||||
"no_distance_approach_limit_mps": 0.5,
|
||||
"no_distance_exit_grace_ms": 3000
|
||||
},
|
||||
|
||||
"accel_limit": {
|
||||
|
||||
@@ -1,96 +0,0 @@
|
||||
"""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()
|
||||
@@ -10,7 +10,7 @@
|
||||
→ CommandSender（mock 记录 / MAVSDK offboard 实发）
|
||||
|
||||
场景时间线（20 Hz，--speedup 可加速）：
|
||||
0~ 6s 正常跟踪：目标匀速横移 + 雷达距离 30→10 m 前向逼近
|
||||
0~ 6s 正常跟踪：目标匀速横移 + 双目距离 30→10 m 前向逼近
|
||||
6~10s 目标消失：追踪器 Coast 预测（60 帧 = 3 s 后轨道判丢，
|
||||
控制律自动切换零速刹车）
|
||||
10~13s 持续判丢：维持零速刹车
|
||||
@@ -73,8 +73,8 @@ def simulate_detections(tick: int, normal_ticks: int, period_s: float):
|
||||
return [Detection(cx - w / 2.0, cy - h / 2.0, cx + w / 2.0, cy + h / 2.0, 0.9)]
|
||||
|
||||
|
||||
def simulate_radar(tick: int, normal_ticks: int, period_s: float):
|
||||
"""场景雷达源：正常阶段关联成立、距离 30→10 m；之后未关联。"""
|
||||
def simulate_distance(tick: int, normal_ticks: int, period_s: float):
|
||||
"""场景双目距离源：正常阶段有效、距离 30→10 m；之后无效。"""
|
||||
if tick >= normal_ticks:
|
||||
return 0.0, False
|
||||
distance = max(10.0, 30.0 - tick * period_s * 3.0)
|
||||
@@ -155,15 +155,15 @@ async def run_demo(args) -> None:
|
||||
for tick in range(total_ticks):
|
||||
now_ms = 1000 + tick * period_ms
|
||||
detections = simulate_detections(tick, normal_ticks, period_s)
|
||||
radar_m, associated = simulate_radar(tick, normal_ticks, period_s)
|
||||
distance_m, distance_valid = simulate_distance(tick, normal_ticks, period_s)
|
||||
|
||||
result = tracker.update(detections)
|
||||
snap = ControlSnapshot(
|
||||
track=track_result_to_state(result),
|
||||
track_time_ms=now_ms, # 每拍取样即打戳（视觉始终新鲜）
|
||||
radar_distance_m=radar_m,
|
||||
associated_to_target=associated,
|
||||
radar_time_ms=now_ms if associated else 0,
|
||||
target_distance_m=distance_m,
|
||||
distance_valid=distance_valid,
|
||||
distance_time_ms=now_ms,
|
||||
attitude_present=True,
|
||||
attitude_time_ms=now_ms, # 模拟姿态源持续可用
|
||||
yaw_rad=0.0, # 机头朝北（NED 旋转恒等）
|
||||
|
||||
@@ -1,65 +1,112 @@
|
||||
# stereo_ranger（Python）
|
||||
# stereo_ranger（Python 版）
|
||||
|
||||
双目稀疏目标测距（规格：`docs/双目测距方案.md`）。输入左右目各自、**校正后原生坐标系**下的检测框，输出目标的 \(Z, X, Y, R\) 与方位。不做目标识别（YOLO）、不做图像采集与立体校正——对应规格 §10 中可独立替换的检测前端与采集预处理模块。
|
||||
双目稀疏目标测距：对**同一时刻**左右两目已立体校正图像上的目标检测框做立体
|
||||
几何解算，输出少量目标的**前向距离 Z + 斜距 R（参考）+ 虚拟双目中心方位
|
||||
c_mid + 有效性/置信度**。不做目标识别、图像采集/立体校正、稠密深度。
|
||||
|
||||
仅依赖标准库（`math`/`logging`），不 import 本仓库任何其他模块。
|
||||
规格：`docs/双目测距技术路线.md`（算法与语义权威）。
|
||||
|
||||
⚠ **全部参数为占位值**（焦距/标定待厂商数据）：标定与地面真值验证完成前，
|
||||
本模块输出**仅限静态标靶/回放/台架验证，不得作为飞行控制决策依据**（§12.3）。
|
||||
|
||||
## 接口清单
|
||||
|
||||
| 名称 | 说明 |
|
||||
| --- | --- |
|
||||
| `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） |
|
||||
| `StereoRanger(config=None, matcher=None)` | 主入口；`matcher` 可注入自定义匹配器 |
|
||||
| `.update(left, right, now_ms=0) -> List[RangeRecord]` | 单周期处理（§5 五步），纯计算、无 IO |
|
||||
| `.reset()` | 清空全部运行期状态（滤波/一致性/隔离/节流计数） |
|
||||
| `.config` / `.matcher` | 只读访问（配置与匹配器） |
|
||||
| `StereoConfig` / `load_from_json(path)` | 17 项配置；缺项/未知字段/类型错误/非法取值一律 `ValueError`（fail-fast） |
|
||||
| `.focal_baseline_px_m` / `.z_min_m` / `.z_max_m` / `.z_error_budget_max_m` | 派生只读量（F·B 常量、视差域互推的距离域、误差预算远界） |
|
||||
| `Detection(x1,y1,x2,y2, confidence, class_id, track_id)` | 单框（校正后坐标系；含 `cx/cy/width/height/bbox` 属性） |
|
||||
| `DetectionFrame(frame_id, detections)` | 单目单帧 |
|
||||
| `RangeRecord` | 输出记录：`frame_id / class_id / degradation / valid / track_id / forward_distance_m(Z) / slant_range_m(R) / disparity_px(D) / center_mid_px(c_mid) / reference_box / reference_eye / confidence / timestamp_ms` |
|
||||
| `Degradation` / `Eye` / `DisparitySource` | 枚举：完整/仅方位；左/右目；同侧边缘/中心回退 |
|
||||
| `sample_point` / `SamplePoint` / `select_disparity` / `Disparity` | 取点器（§6.1，纯函数） |
|
||||
| `Matcher`（Protocol）/ `GreedyMatcher` / `MatchPair` | 匹配器（§6.2，接口注入，可换匈牙利等） |
|
||||
| `solve_range` / `RangeSolution` / `virtual_center_px` / `forward_distance_m` | 测距与方位源（§3.2/§6.6，纯函数） |
|
||||
| `evaluate_validity` / `ValidityDecision` / `range_error_abs_m` / `range_error_rel` / `confidence_from_cost` / `DELTA_D_PX` | 有效性三道门与误差模型（§6.4/§7.1，纯函数） |
|
||||
| `ConsistencyMonitor` / `ConsistencyStatus` | 一致性监控（§6.7，每目标状态） |
|
||||
| `DistanceFilter` | 距离低通 + 跳变重置（§6.5，每目标状态） |
|
||||
| `TargetStore` | 目标隔离与老化清理（§4，track_id 优先 + 最近邻兜底） |
|
||||
| `WarnThrottle` | 告警节流（第 1 次 + 每满 100 次，带累计计数） |
|
||||
|
||||
## 与检测器的约定（规格 §4.3）
|
||||
`logger` 名称为 `stereo_ranger`，走标准 `logging`。
|
||||
|
||||
1. 左右图像必须使用**完全一致**的预处理几何（同 letter-box、同缩放），由检测前端保证；
|
||||
2. 传入本模块的框必须已映射回**原生分辨率**（视差在原生坐标计算），内参 `fx/fy/cx/cy` 亦对应原生分辨率；
|
||||
3. 图像对须已完成立体校正（同一目标在两目 y 坐标一致）；校正残留由 `vertical_tau_px` 容错；
|
||||
4. YOLO 推理及左右目并行执行由调用方负责，本模块只收检测结果。
|
||||
## 单帧流程（`update` 五步）
|
||||
|
||||
## 单帧流程（对照规格）
|
||||
1. **同帧契约**：`left.frame_id != right.frame_id` → 拒收整帧、返回空列表、WARN 节流（§2.1）；
|
||||
2. **取点**：逐框取同侧（左）边缘 x 与框中心；坐标非有限值的框丢弃并 WARN（不参与解算也不发布）；
|
||||
3. **匹配**：硬约束（类别一致、`|Δy| ≤ tau_y_px`、框高相对差 ≤ `tau_h_ratio`）+ 代价升序贪心一一配对，`cost < 1` 才接受；
|
||||
4. **配对目标**：`D → Z = F·B/D`（`D ≤ 0` 判违例）→ `R` 参考 → `c_mid = (cL + cR)/2` → 门① 域内 / 门② 误差预算 / 门③ 一致性 → 有效帧过 `DistanceFilter`，记 `Z_out`；
|
||||
5. **未配对框**：发布仅方位记录（`BEARING_ONLY`，`valid = false`，右目未匹配同样发布）；帧末对未出现目标记失效、连续 `filter_reset_frames` 帧未出现整体清理。
|
||||
|
||||
硬约束过滤（类别一致、`|Δ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` 不过滤）。
|
||||
记录顺序：先按左目框顺序（配对→完整 / 未配对→仅方位），再给未配对右目框的仅方位记录。
|
||||
|
||||
## 输出语义
|
||||
## 实现决策（v1.0 冻结，已回填规格）
|
||||
|
||||
- `matched=True, valid=True`：有效测距；
|
||||
- `matched=True, valid=False`：已配对但超出有效工作域（§5.4），距离/坐标仍发布供诊断，方位可用；
|
||||
- `matched=False, valid=False`：左目检测本帧未配对（§6 失败处理），仅单目方位回退。
|
||||
规格 `docs/双目测距技术路线.md` 已按下列决策修订（§4/§6.1/§6.2/§6.4/§6.7/
|
||||
§9/§11 均已更新），本节保留决策动机与实现落点，供重写/移植核对。
|
||||
|
||||
方位源 `centre_pixel_x_mid` 为基线中点虚拟相机投影（§11.3）；注意 `X` 属**左目坐标系**（§4.2），二者相差 `B/2`，勿混用。
|
||||
1. **越界 = 有效性门，不是配对硬约束**：规格 §6.2 原把"视差在域内"列入
|
||||
硬约束，与 §5/§6.3/§6.4/§2.3 的表述冲突；按 §5 主流程实现并回填——
|
||||
越界目标仍配对、仍发布 `D` 与 `c_mid`（远场方位闭环的输入基础），仅
|
||||
`valid = false`。
|
||||
2. **视差域权威**：`disparity_min_px / disparity_max_px` 为真源，`z_min_m / z_max_m`
|
||||
为派生只读属性；原 §9.2 占位值不自洽（`D_min = 3 px ⇔ 19.2 m ≠ 20 m`），
|
||||
距离域配置项已从规格删除（§6.4/§9.2）。
|
||||
3. **不实现 §6.1 的"左右边缘各算一次取中值"精化**：两支的 median 恒等于均值，
|
||||
即框中心视差，与"同侧边缘"的理由矛盾，规格已删除该项（早期笔误），留
|
||||
v1.1 亚像素精化。
|
||||
4. **补充配置项**（原文 §9 未列、算法需要，规格 §9.3/§9.5 已回填）：
|
||||
`consistency_jump_px`（§6.7 视差跳变门限）、`isolation_gate_px`（§4 最近邻
|
||||
兜底的门限）。
|
||||
5. **δD 固定 1 px**：`DELTA_D_PX = 1.0`（§7.1 ⚠占位）；标定回填后可参数化。
|
||||
6. **R 与已发布 Z 同基准**：`R = R_raw · z_out / z_raw`（X/Y/Z 均 ∝ Z，精确换基，
|
||||
保证 `R ≥ Z`）；无效帧无 `Z_out` 时 `R` 取本帧原始值（诊断参考）。规格
|
||||
§2.2/§6.5 已注明。
|
||||
7. **置信度映射**（§6.4 ⚠占位）：`clamp(1 − cost, 0, 1)`；连续跳变 ≥ N 帧乘 0.5
|
||||
（`_CONFIDENCE_DEGRADE_FACTOR`）；判无效置 0。
|
||||
8. **一致性只在配对成功帧推进**：仅方位帧没有 D 观测量，不推进 D 序列；但目标
|
||||
未出现的帧会记一次滤波失效（§6.5 失效清理），两条状态机职责不混。
|
||||
9. **取点回退规则**：边缘视差越窗且中心视差在域内 → 回退中心
|
||||
（`CENTER_FALLBACK` + WARN）；两者均越窗 → 保留边缘值供诊断与违例告警。
|
||||
10. **track_id 优先路径提前启用**（v1.1 语义，规格 §4 已按此冻结）：左右
|
||||
`track_id` 一致且 > 0 时用 `(track, 类别, id)` 作隔离键；最近邻只在无
|
||||
track_id 的状态间匹配，一一归属。
|
||||
11. **§3.2 勘误**：原文 `X_C = X + B/2` 与 §6.6 的符号相反（应为 `X_C = X − B/2`），
|
||||
规格已修正；代码不输出 `X_C`，以 §6.6 恒等式为准。
|
||||
|
||||
## StereoConfig 关键项
|
||||
## 运行期状态清单（§11.1，漏一个就变行为）
|
||||
|
||||
| 字段 | 默认 | 说明 |
|
||||
| --- | --- | --- |
|
||||
| `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 | 诊断日志节流间隔 |
|
||||
| 状态 | 所属 | 初值 | 改变时机 |
|
||||
| --- | --- | --- | --- |
|
||||
| 连续异常计数 ×目标 | `ConsistencyMonitor` | 0 | 每帧异常 +1 / 正常帧清零 |
|
||||
| 上次滤波值、初始化标志、连续失效计数 ×目标 | `DistanceFilter` | 无 / false / 0 | 有效帧写入 / 首有效帧置位 / 无效帧 +1 |
|
||||
| 最近位置、最近出现帧 ×目标 | `TargetStore` | 首帧写入 | 每次 `resolve` |
|
||||
| 帧序号 | `StereoRanger` | 0 | 每帧 +1（拒收帧不加） |
|
||||
| 告警计数 ×异常类 | `WarnThrottle` | 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
|
||||
"D:/program file/conda/envs/pytorch/python.exe" -m pytest tests/stereo_ranger -q # 111 条
|
||||
"D:/program file/conda/envs/pytorch/python.exe" -m pytest -q # 全量 220 条
|
||||
python utils/demo_stereo_ranger.py # 六场景合成框对演示
|
||||
```
|
||||
|
||||
## 未覆盖边界（属其他模块/后续版本）
|
||||
构件级测试逐条对应 §11.4（取点器/匹配器/测距/有效性/滤波器/一致性/全流程），
|
||||
**全部用合成框对驱动，无需真实图像**（几何是解析可断言的）：
|
||||
|
||||
- 图像采集、立体校正 remap、帧同步、side-by-side 裁切（规格 §3）；
|
||||
- YOLO/检测器及其预处理（由调用方按上文约定接入）；
|
||||
- 亚像素边缘精化、跨帧跟踪（`track_id` 由外部追踪器提供后，时间滤波即可用）、与主测距传感器融合（§5.5/§11）。
|
||||
- 取点：同侧边缘取值、框宽差不进入视差、中心对照、越窗回退、边界含等号、负视差；
|
||||
- 匹配：硬约束各一例、`cost ≥ 1` 拒绝、贪心排序、一一配对；
|
||||
- 测距：`Z = F·B/D` 回收、`D ≤ 0` 违例、`c_mid` 等权平均恒等式、误差平方增长；
|
||||
- 有效性：三道门与边界归类、预算门通过/拦截两侧；
|
||||
- 滤波/一致性：低通收敛、跳变重置不拖尾、失效清理、N 帧降级 / M 帧判无效 / 正常帧清零；
|
||||
- 全流程：单目标、滤波、远场越界仍配对、单目退化、记录顺序、非同帧拒收与节流、
|
||||
空输入、非有限框、track_id 跨大位移保持状态、最近邻越门限新建状态、匹配器注入、`reset`。
|
||||
|
||||
demo 覆盖：① 逼近（三道门依次生效 + 滤波）② 远场域外（方位照常发布）
|
||||
③ 单目退化 ④ 非同帧拒收与节流 ⑤ 一致性降级/判无效 ⑥ 多目标贪心配对。
|
||||
|
||||
@@ -1,30 +1,72 @@
|
||||
"""stereo_ranger —— 双目稀疏目标测距（规格：docs/双目测距方案.md）。
|
||||
"""stereo_ranger —— 双目稀疏目标测距（规格：docs/双目测距技术路线.md）。
|
||||
|
||||
纯算法包：输入校正后原生坐标系的左右目检测框，输出目标的 (Z, X, Y, R)
|
||||
与方位。不做目标识别、图像采集与立体校正；仅依赖标准库。
|
||||
纯算法包：输入校正后坐标系的同帧左右目检测框，输出目标距离记录（Z/R/D、
|
||||
虚拟双目中心方位、valid/confidence）。不做目标识别、图像采集与立体校正；
|
||||
仅依赖标准库。⚠ 全部数值参数为占位值，标定与地面验证完成前不得作为飞行
|
||||
控制决策依据（§12.3 红线）。
|
||||
"""
|
||||
|
||||
from .config import StereoConfig, load_from_json
|
||||
from .stereo_ranger import (
|
||||
DISP_SOURCE_CENTER,
|
||||
DISP_SOURCE_EDGE,
|
||||
StereoRanger,
|
||||
focal_from_hfov,
|
||||
range_error_abs,
|
||||
from .consistency import ConsistencyMonitor, ConsistencyStatus
|
||||
from .distance_filter import DistanceFilter
|
||||
from .geometry import (
|
||||
DELTA_D_PX,
|
||||
RangeSolution,
|
||||
ValidityDecision,
|
||||
confidence_from_cost,
|
||||
evaluate_validity,
|
||||
forward_distance_m,
|
||||
range_error_abs_m,
|
||||
range_error_rel,
|
||||
solve_range,
|
||||
virtual_center_px,
|
||||
)
|
||||
from .types import BBoxXYXY, Detection, RangeReport
|
||||
from .matching import GreedyMatcher, MatchPair, Matcher
|
||||
from .sampling import Disparity, SamplePoint, sample_point, select_disparity
|
||||
from .stereo_ranger import StereoRanger
|
||||
from .target_store import TargetStore
|
||||
from .types import (
|
||||
BBoxXYXY,
|
||||
Degradation,
|
||||
Detection,
|
||||
DetectionFrame,
|
||||
DisparitySource,
|
||||
Eye,
|
||||
RangeRecord,
|
||||
)
|
||||
from .warn_throttle import WarnThrottle
|
||||
|
||||
__all__ = [
|
||||
"BBoxXYXY",
|
||||
"DISP_SOURCE_CENTER",
|
||||
"DISP_SOURCE_EDGE",
|
||||
"DELTA_D_PX",
|
||||
"ConsistencyMonitor",
|
||||
"ConsistencyStatus",
|
||||
"Degradation",
|
||||
"Detection",
|
||||
"RangeReport",
|
||||
"DetectionFrame",
|
||||
"Disparity",
|
||||
"DisparitySource",
|
||||
"DistanceFilter",
|
||||
"Eye",
|
||||
"GreedyMatcher",
|
||||
"MatchPair",
|
||||
"Matcher",
|
||||
"RangeRecord",
|
||||
"RangeSolution",
|
||||
"SamplePoint",
|
||||
"StereoConfig",
|
||||
"StereoRanger",
|
||||
"focal_from_hfov",
|
||||
"TargetStore",
|
||||
"ValidityDecision",
|
||||
"WarnThrottle",
|
||||
"confidence_from_cost",
|
||||
"evaluate_validity",
|
||||
"forward_distance_m",
|
||||
"load_from_json",
|
||||
"range_error_abs",
|
||||
"range_error_abs_m",
|
||||
"range_error_rel",
|
||||
"sample_point",
|
||||
"select_disparity",
|
||||
"solve_range",
|
||||
"virtual_center_px",
|
||||
]
|
||||
|
||||
@@ -1,50 +1,51 @@
|
||||
"""StereoConfig 与 JSON 配置加载。
|
||||
"""StereoConfig 与 JSON 配置加载（§9 配置参数全量表）。
|
||||
|
||||
规格 docs/双目测距方案.md：全部数值参数均为配置项（§2），默认值取文档
|
||||
示例值。配置校验在构造期一次性完成，热路径不读 JSON。
|
||||
全部参数配置外置、构造期一次加载并校验：缺项、类型错误、非法取值一律拒绝
|
||||
进入运行期（fail-fast）。所有默认值均为 ⚠占位值，未整定前不得用于飞行决策
|
||||
（§12.3 红线 1/2）。
|
||||
|
||||
域权威（实现决策）：视差域 disparity_min_px / disparity_max_px 为权威，
|
||||
距离域 z_min_m / z_max_m 由 Z = F·B/D 派生为只读属性——域判定使用视差
|
||||
（§11.2 直接观测量），两套阈值不再冗余存储、避免互推舍入不一致（§6.4）。
|
||||
"""
|
||||
|
||||
import json
|
||||
import math
|
||||
from dataclasses import dataclass, fields
|
||||
from pathlib import Path
|
||||
from typing import Any, Dict, Tuple
|
||||
from typing import Any, Dict
|
||||
|
||||
# JSON 加载时的整数型字段（其余按数值校验）
|
||||
_INT_FIELDS = frozenset({"consistency_N_frames", "consistency_M_frames", "filter_reset_frames"})
|
||||
|
||||
|
||||
@dataclass
|
||||
class StereoConfig:
|
||||
# ---- 相机参数（校正后、对应原生分辨率，§3.1 / §4.3）----
|
||||
# ---- 几何组（来自标定 P1/P2，§9.1；⚠全部为占位值）----
|
||||
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
|
||||
baseline_m: float = 0.06
|
||||
|
||||
# ---- 目标距离窗口：关联硬约束（§6），派生视差区间 [FB/z_max, FB/z_min] ----
|
||||
z_min_m: float = 1.0
|
||||
z_max_m: float = 50.0
|
||||
# ---- 匹配组（§9.2）----
|
||||
tau_y_px: float = 2.0
|
||||
tau_h_ratio: float = 0.25
|
||||
disparity_min_px: float = 3.0
|
||||
disparity_max_px: float = 57.6 # ⚠占位：F=960/B=0.06 下 ⇔ 近界 1.0 m
|
||||
|
||||
# ---- 关联阈值（§6）----
|
||||
vertical_tau_px: float = 2.0 # 垂直残差 tau_y
|
||||
height_ratio_tau: float = 0.25 # 相对高度差 tau_h
|
||||
# ---- 有效性组（§9.3）----
|
||||
max_delta_z_m: float = 2.0 # 误差预算 δZ(δD = 1px) 上限
|
||||
consistency_N_frames: int = 3 # 连续跳变降置信度的帧数
|
||||
consistency_M_frames: int = 5 # 连续异常判 invalid 的帧数
|
||||
consistency_jump_px: float = 3.0 # ⚠补充项：§9 未列，§6.7 的视差跳变门限
|
||||
|
||||
# ---- 视差取点与有效性（§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）
|
||||
# ---- 滤波组（§9.4）----
|
||||
smooth_alpha: float = 0.3
|
||||
jump_reset_m: float = 5.0
|
||||
filter_reset_frames: int = 5
|
||||
|
||||
# ---- 可选时间滤波（§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
|
||||
# ---- 目标隔离（§4：类别 + 框位置最近邻；⚠补充项）----
|
||||
isolation_gate_px: float = 50.0
|
||||
|
||||
def __post_init__(self) -> None:
|
||||
_require(self.fx_px > 0.0, "fx_px", "必须为正")
|
||||
@@ -52,31 +53,71 @@ class StereoConfig:
|
||||
_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", "必须非负")
|
||||
_require(self.tau_y_px > 0.0, "tau_y_px", "必须为正")
|
||||
_require(self.tau_h_ratio > 0.0, "tau_h_ratio", "必须为正")
|
||||
_require(
|
||||
0.0 < self.disparity_min_px < self.disparity_max_px,
|
||||
"disparity_min_px/disparity_max_px",
|
||||
"须满足 0 < 下限 < 上限",
|
||||
)
|
||||
_require(self.max_delta_z_m > 0.0, "max_delta_z_m", "必须为正")
|
||||
_require(self.consistency_N_frames >= 1, "consistency_N_frames", "必须 ≥ 1")
|
||||
_require(
|
||||
self.consistency_M_frames >= self.consistency_N_frames,
|
||||
"consistency_M_frames",
|
||||
"必须 ≥ consistency_N_frames",
|
||||
)
|
||||
_require(self.consistency_jump_px > 0.0, "consistency_jump_px", "必须为正")
|
||||
_require(0.0 < self.smooth_alpha <= 1.0, "smooth_alpha", "须落在 (0, 1]")
|
||||
_require(self.jump_reset_m > 0.0, "jump_reset_m", "必须为正")
|
||||
_require(self.filter_reset_frames >= 1, "filter_reset_frames", "必须 ≥ 1")
|
||||
_require(self.isolation_gate_px > 0.0, "isolation_gate_px", "必须为正")
|
||||
|
||||
@property
|
||||
def focal_baseline_px_m(self) -> float:
|
||||
"""F·B：构造期一次算好的常量，热路径只做乘除（§11.1）。"""
|
||||
return self.fx_px * self.baseline_m
|
||||
|
||||
@property
|
||||
def z_min_m(self) -> float:
|
||||
"""近界（由 disparity_max_px 派生）。"""
|
||||
return self.focal_baseline_px_m / self.disparity_max_px
|
||||
|
||||
@property
|
||||
def z_max_m(self) -> float:
|
||||
"""远界（由 disparity_min_px 派生）。"""
|
||||
return self.focal_baseline_px_m / self.disparity_min_px
|
||||
|
||||
@property
|
||||
def z_error_budget_max_m(self) -> float:
|
||||
"""误差预算门限对应的最远距离 sqrt(δZ_max·F·B)（δD = 1px，§7.2）。"""
|
||||
return math.sqrt(self.max_delta_z_m * self.focal_baseline_px_m)
|
||||
|
||||
|
||||
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
|
||||
"""从 JSON 文件构建配置；缺项/未知字段/类型错误/非法取值一律抛 ValueError。"""
|
||||
with open(path, encoding="utf-8") as handle:
|
||||
data: Dict[str, Any] = json.load(handle)
|
||||
if not isinstance(data, dict):
|
||||
raise ValueError("配置非法: 顶层须为 JSON 对象")
|
||||
known = {item.name for item in fields(StereoConfig)}
|
||||
unknown = sorted(set(data) - known)
|
||||
if unknown:
|
||||
raise ValueError(f"未知配置字段: {sorted(unknown)}")
|
||||
missing = known - set(data)
|
||||
raise ValueError(f"未知配置字段: {unknown}")
|
||||
missing = sorted(known - set(data))
|
||||
if missing:
|
||||
raise ValueError(f"缺少配置字段: {sorted(missing)}")
|
||||
raise ValueError(f"缺少配置字段: {missing}")
|
||||
for name, value in data.items():
|
||||
if isinstance(value, bool):
|
||||
raise ValueError(f"配置类型非法: {name} 须为数值")
|
||||
if name in _INT_FIELDS:
|
||||
if not isinstance(value, int):
|
||||
raise ValueError(f"配置类型非法: {name} 须为整数")
|
||||
elif not isinstance(value, (int, float)):
|
||||
raise ValueError(f"配置类型非法: {name} 须为数值")
|
||||
return StereoConfig(**data)
|
||||
|
||||
|
||||
def _require(ok: bool, field_name: str, message: str) -> None:
|
||||
if not ok:
|
||||
def _require(condition: bool, field_name: str, message: str) -> None:
|
||||
if not condition:
|
||||
raise ValueError(f"配置非法: {field_name} {message}")
|
||||
|
||||
@@ -0,0 +1,74 @@
|
||||
"""一致性监控（§6.7）：时序层面质量自检，每目标独立状态。
|
||||
|
||||
机载激光移除后无外部距离基准，飞行中只能靠自身时序一致性监控数据质量：
|
||||
**只能降级、不能纠偏**（绝对精度靠地面静态验证）。匹配层面的质量（|Δy|
|
||||
残差、框高一致性、代价门限）由匹配硬约束承担，本节只管时序层面。
|
||||
"""
|
||||
|
||||
from dataclasses import dataclass
|
||||
from typing import Dict, Hashable, Optional
|
||||
|
||||
|
||||
@dataclass
|
||||
class ConsistencyStatus:
|
||||
"""单目标单帧一致性判定结果（§6.7）。"""
|
||||
|
||||
jump_px: Optional[float] # |D − 上次 D|；首帧无参考为 None
|
||||
abnormal: bool # 本帧是否跳变超门限
|
||||
streak: int # 连续异常帧数（含本帧，正常帧清零）
|
||||
confidence_degraded: bool # streak ≥ N：置信度降级
|
||||
invalid: bool # streak ≥ M：质量语义判无效
|
||||
|
||||
|
||||
class ConsistencyMonitor:
|
||||
"""每目标：上次视差 + 连续异常计数（§11.1 必须持久化的状态）。"""
|
||||
|
||||
def __init__(self, jump_px: float, degrade_frames: int, invalid_frames: int) -> None:
|
||||
if jump_px <= 0.0:
|
||||
raise ValueError("jump_px 必须为正")
|
||||
if degrade_frames < 1:
|
||||
raise ValueError("degrade_frames 必须 ≥ 1")
|
||||
if invalid_frames < degrade_frames:
|
||||
raise ValueError("invalid_frames 必须 ≥ degrade_frames")
|
||||
self._jump_px = jump_px
|
||||
self._degrade_frames = degrade_frames
|
||||
self._invalid_frames = invalid_frames
|
||||
self._last_d: Dict[Hashable, float] = {}
|
||||
self._streak: Dict[Hashable, int] = {}
|
||||
|
||||
def advance(self, key: Hashable, disparity_px: float) -> ConsistencyStatus:
|
||||
"""推进目标 key 的一致性判定（配对成功帧每帧一次）。"""
|
||||
last = self._last_d.get(key)
|
||||
if last is None:
|
||||
jump: Optional[float] = None
|
||||
abnormal = False
|
||||
streak = 0
|
||||
else:
|
||||
jump = abs(disparity_px - last)
|
||||
abnormal = jump > self._jump_px
|
||||
streak = self._streak.get(key, 0) + 1 if abnormal else 0
|
||||
self._last_d[key] = disparity_px
|
||||
if streak:
|
||||
self._streak[key] = streak
|
||||
else:
|
||||
self._streak.pop(key, None)
|
||||
return ConsistencyStatus(
|
||||
jump_px=jump,
|
||||
abnormal=abnormal,
|
||||
streak=streak,
|
||||
confidence_degraded=streak >= self._degrade_frames,
|
||||
invalid=streak >= self._invalid_frames,
|
||||
)
|
||||
|
||||
def streak(self, key: Hashable) -> int:
|
||||
"""目标 key 当前连续异常计数。"""
|
||||
return self._streak.get(key, 0)
|
||||
|
||||
def drop(self, key: Hashable) -> None:
|
||||
"""清理目标 key 的状态（目标老化清理时调用）。"""
|
||||
self._last_d.pop(key, None)
|
||||
self._streak.pop(key, None)
|
||||
|
||||
def clear(self) -> None:
|
||||
self._last_d.clear()
|
||||
self._streak.clear()
|
||||
@@ -0,0 +1,68 @@
|
||||
"""时间滤波器（§6.5）：一阶低通 + 单帧跳变重置，每目标独立状态。
|
||||
|
||||
- 单帧毛刺由跳变重置处理，持续性异常由一致性监控（§6.7）处理，两者职责
|
||||
不同不得合并；
|
||||
- 滤波只在**有效帧**之间进行，无效/缺席帧不参与平滑；α = 1 等效不滤波；
|
||||
- 连续失效（无效或缺席）达 filter_reset_frames 帧清空状态，下次有效帧重新
|
||||
初始化（防止用陈旧状态平滑新测量）。
|
||||
"""
|
||||
|
||||
from typing import Dict, Hashable, Optional
|
||||
|
||||
|
||||
class DistanceFilter:
|
||||
"""每目标：上次滤波值 + 连续失效计数（§11.1 必须持久化的状态）。"""
|
||||
|
||||
def __init__(self, alpha: float, jump_reset_m: float, reset_frames: int) -> None:
|
||||
if not 0.0 < alpha <= 1.0:
|
||||
raise ValueError("alpha 须落在 (0, 1]")
|
||||
if jump_reset_m <= 0.0:
|
||||
raise ValueError("jump_reset_m 必须为正")
|
||||
if reset_frames < 1:
|
||||
raise ValueError("reset_frames 必须 ≥ 1")
|
||||
self._alpha = alpha
|
||||
self._jump_reset_m = jump_reset_m
|
||||
self._reset_frames = reset_frames
|
||||
self._values: Dict[Hashable, float] = {}
|
||||
self._invalid_streak: Dict[Hashable, int] = {}
|
||||
|
||||
def advance(self, key: Hashable, z_m: float) -> float:
|
||||
"""有效帧推进：未初始化→直接置值；跳变超门限→重置；否则一阶低通。"""
|
||||
previous = self._values.get(key)
|
||||
self._invalid_streak.pop(key, None)
|
||||
if previous is None:
|
||||
self._values[key] = z_m
|
||||
return z_m
|
||||
if abs(z_m - previous) > self._jump_reset_m:
|
||||
self._values[key] = z_m # 直接重置而非滤波，防止拖尾
|
||||
return z_m
|
||||
smoothed = previous + self._alpha * (z_m - previous)
|
||||
self._values[key] = smoothed
|
||||
return smoothed
|
||||
|
||||
def invalidate(self, key: Hashable) -> None:
|
||||
"""无效/缺席帧：连续失效达门限后清空该目标滤波状态（§6.5 记失效）。"""
|
||||
if key not in self._values:
|
||||
return
|
||||
streak = self._invalid_streak.get(key, 0) + 1
|
||||
if streak >= self._reset_frames:
|
||||
self._values.pop(key, None)
|
||||
self._invalid_streak.pop(key, None)
|
||||
else:
|
||||
self._invalid_streak[key] = streak
|
||||
|
||||
def value(self, key: Hashable) -> Optional[float]:
|
||||
"""目标 key 当前滤波值；未初始化/已清空为 None。"""
|
||||
return self._values.get(key)
|
||||
|
||||
def invalid_streak(self, key: Hashable) -> int:
|
||||
"""目标 key 当前连续失效计数（内部状态，供测试/诊断）。"""
|
||||
return self._invalid_streak.get(key, 0)
|
||||
|
||||
def drop(self, key: Hashable) -> None:
|
||||
self._values.pop(key, None)
|
||||
self._invalid_streak.pop(key, None)
|
||||
|
||||
def clear(self) -> None:
|
||||
self._values.clear()
|
||||
self._invalid_streak.clear()
|
||||
@@ -0,0 +1,84 @@
|
||||
"""测距计算与有效性判定（§3.2/§6.4/§7.1）：纯函数、无状态。
|
||||
|
||||
Z = F·B/D（前向分量，距离通道唯一消费量，§10.5）；X/Y 以左目系表达；
|
||||
R = sqrt(X² + Y² + Z²) 仅参考输出；c_mid = 两目框中心等权平均（虚拟双目
|
||||
中心，§6.6，逐点精确、无近似）。
|
||||
"""
|
||||
|
||||
import math
|
||||
from dataclasses import dataclass
|
||||
from typing import Optional, Tuple
|
||||
|
||||
from .config import StereoConfig
|
||||
from .types import Detection
|
||||
|
||||
DELTA_D_PX = 1.0 # 误差预算的δD 基准（§7.2 定义 δZ(δD = 1px)）
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class RangeSolution:
|
||||
"""测距解算结果（Z 为前向分量，X/Y 属左目系，§3.2）。"""
|
||||
|
||||
z_m: float
|
||||
x_m: float
|
||||
y_m: float
|
||||
r_m: float # 斜距（参考值，不进控制回路）
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ValidityDecision:
|
||||
"""有效性判定结果（§6.4 三道门的前两道；门③一致性由调用方合流）。"""
|
||||
|
||||
in_domain: bool # ① 域内：D ∈ [D_min, D_max]
|
||||
budget_ok: bool # ② 误差预算：δZ(δD = 1px) ≤ δZ_max
|
||||
valid: bool # ① 且 ②
|
||||
|
||||
|
||||
def forward_distance_m(disparity_px: float, config: StereoConfig) -> float:
|
||||
"""Z = F·B/D；调用方须先判 D ≤ 0 物理违例（§6.3/§11.2 除零纪律）。"""
|
||||
if disparity_px <= 0.0:
|
||||
raise ValueError(f"视差须为正（先判违例再除）: D={disparity_px}")
|
||||
return config.focal_baseline_px_m / disparity_px
|
||||
|
||||
|
||||
def solve_range(left_box: Detection, right_box: Detection, disparity_px: float, config: StereoConfig) -> RangeSolution:
|
||||
"""由视差与参考目（左目）框中心解算 (Z, X, Y, R)（§3.2）。"""
|
||||
z_m = forward_distance_m(disparity_px, config)
|
||||
x_m = (left_box.cx - config.cx_px) * z_m / config.fx_px
|
||||
y_m = (left_box.cy - config.cy_px) * z_m / config.fy_px
|
||||
r_m = math.sqrt(x_m * x_m + y_m * y_m + z_m * z_m)
|
||||
return RangeSolution(z_m=z_m, x_m=x_m, y_m=y_m, r_m=r_m)
|
||||
|
||||
|
||||
def virtual_center_px(left_box: Detection, right_box: Detection) -> Tuple[float, float]:
|
||||
"""虚拟双目中心 c_mid = (cL + cR)/2（§6.6：等权平均 = 把相机搬到模组中线）。
|
||||
|
||||
与视差取点（同侧边缘）是两个不同的取点，不得混用（§6.6）。
|
||||
"""
|
||||
return ((left_box.cx + right_box.cx) / 2.0, (left_box.cy + right_box.cy) / 2.0)
|
||||
|
||||
|
||||
def range_error_abs_m(z_m: float, config: StereoConfig, delta_d_px: float = DELTA_D_PX) -> float:
|
||||
"""绝对误差估计 δZ ≈ Z²·δD/(F·B)（§7.1）。"""
|
||||
return z_m * z_m * delta_d_px / config.focal_baseline_px_m
|
||||
|
||||
|
||||
def range_error_rel(z_m: float, config: StereoConfig, delta_d_px: float = DELTA_D_PX) -> float:
|
||||
"""相对误差估计 δZ/Z ≈ Z·δD/(F·B)（§7.1）。"""
|
||||
return z_m * delta_d_px / config.focal_baseline_px_m
|
||||
|
||||
|
||||
def evaluate_validity(disparity_px: float, z_m: Optional[float], config: StereoConfig) -> ValidityDecision:
|
||||
"""有效性门①②（§6.4）：域内 + 误差预算；z_m=None（D ≤ 0）时预算判失败。"""
|
||||
in_domain = config.disparity_min_px <= disparity_px <= config.disparity_max_px
|
||||
budget_ok = (
|
||||
z_m is not None
|
||||
and math.isfinite(z_m)
|
||||
and range_error_abs_m(z_m, config) <= config.max_delta_z_m
|
||||
)
|
||||
return ValidityDecision(in_domain=in_domain, budget_ok=budget_ok, valid=in_domain and budget_ok)
|
||||
|
||||
|
||||
def confidence_from_cost(cost: float) -> float:
|
||||
"""匹配代价 → 置信度基值：线性映射 clamp(1 − cost, 0, 1)（§6.4 ⚠占位映射）。"""
|
||||
return min(1.0, max(0.0, 1.0 - cost))
|
||||
@@ -0,0 +1,82 @@
|
||||
"""左右匹配器（§6.2）：硬约束 + 代价升序贪心一一配对；接口注入。
|
||||
|
||||
默认 GreedyMatcher 为纯函数（§4 构件表）；调用方可通过
|
||||
StereoRanger(matcher=...) 注入匈牙利等替换实现而不动调用方（§10.3）。
|
||||
|
||||
硬约束（任一不满足直接拒绝配对，不进代价排序）：类别一致、|Δy| ≤ τ_y、
|
||||
框高相对差 ≤ τ_h。视差域 [D_min, D_max] 不参与配对：越界属有效性门
|
||||
（§6.2 已回填，与 §5 失败路径表/§6.3/§6.4/§2.3 一致）。
|
||||
"""
|
||||
|
||||
from dataclasses import dataclass
|
||||
from typing import List, Protocol, Sequence
|
||||
|
||||
from .config import StereoConfig
|
||||
from .sampling import SamplePoint
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class MatchPair:
|
||||
"""一一配对结果：左右下标 + 匹配代价（§6.2）。"""
|
||||
|
||||
left_index: int
|
||||
right_index: int
|
||||
cost: float
|
||||
|
||||
|
||||
class Matcher(Protocol):
|
||||
"""匹配器接口（§4：接口注入，默认贪心）。"""
|
||||
|
||||
def match(
|
||||
self,
|
||||
left: Sequence[SamplePoint],
|
||||
right: Sequence[SamplePoint],
|
||||
config: StereoConfig,
|
||||
) -> List[MatchPair]:
|
||||
...
|
||||
|
||||
|
||||
class GreedyMatcher:
|
||||
"""硬约束过滤 + 代价升序贪心：cost < 1 才接受，一一配对。"""
|
||||
|
||||
def match(
|
||||
self,
|
||||
left: Sequence[SamplePoint],
|
||||
right: Sequence[SamplePoint],
|
||||
config: StereoConfig,
|
||||
) -> List[MatchPair]:
|
||||
tau_y = config.tau_y_px
|
||||
tau_h = config.tau_h_ratio
|
||||
candidates = []
|
||||
for i, point_l in enumerate(left):
|
||||
if not point_l.finite:
|
||||
continue
|
||||
for j, point_r in enumerate(right):
|
||||
if not point_r.finite:
|
||||
continue
|
||||
if point_l.box.class_id != point_r.box.class_id:
|
||||
continue
|
||||
dy = abs(point_l.center_y - point_r.center_y)
|
||||
if dy > tau_y:
|
||||
continue
|
||||
height_max = max(point_l.box.height, point_r.box.height)
|
||||
if height_max <= 0.0:
|
||||
continue
|
||||
dh_ratio = abs(point_l.box.height - point_r.box.height) / height_max
|
||||
if dh_ratio > tau_h:
|
||||
continue
|
||||
cost = dy / tau_y + dh_ratio / tau_h
|
||||
if cost >= 1.0:
|
||||
continue
|
||||
candidates.append((cost, i, j))
|
||||
candidates.sort(key=lambda item: (item[0], item[1], item[2]))
|
||||
used_left = set()
|
||||
used_right = set()
|
||||
pairs: List[MatchPair] = []
|
||||
for cost, i, j in candidates:
|
||||
if i in used_left or j in used_right:
|
||||
continue
|
||||
pairs.append(MatchPair(left_index=i, right_index=j, cost=cost))
|
||||
used_left.add(i)
|
||||
used_right.add(j)
|
||||
return pairs
|
||||
@@ -0,0 +1,73 @@
|
||||
"""视差取点器（§6.1）：同侧（左）边缘为主方案，边缘视差越窗时回退框中心。
|
||||
|
||||
纯函数、无状态（§4 构件表）。为什么用同侧边缘：同一物理目标在两目中的
|
||||
检测框宽并不严格相等，框中心视差会混入框宽差的一半，同侧边缘视差可抵消
|
||||
框宽项。
|
||||
|
||||
注：§6.1 的"左右两边缘各算一次视差取中值"精化在数学上恒等于框中心视差
|
||||
（两支的 median = 均值），与"中心视差混入框宽差"的理由矛盾，属原文笔误，
|
||||
v1.0 不实现；规格 §6.1 已删除该项并注明（见包 README「实现决策」）。
|
||||
"""
|
||||
|
||||
import math
|
||||
from dataclasses import dataclass
|
||||
|
||||
from .config import StereoConfig
|
||||
from .types import Detection, DisparitySource
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SamplePoint:
|
||||
"""单目单框取点结果：同侧边缘候选与框中心（§6.1/§6.6）。
|
||||
|
||||
finite=False 表示坐标非有限值：该框不可参与解算，由调用方丢弃并告警。
|
||||
"""
|
||||
|
||||
box: Detection
|
||||
edge_x: float # 同侧（左）边缘 x = box.x1
|
||||
center_x: float
|
||||
center_y: float
|
||||
finite: bool
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Disparity:
|
||||
"""配对后的视差取点结果。"""
|
||||
|
||||
d_px: float
|
||||
source: DisparitySource
|
||||
fallback_used: bool # 边缘视差越窗、回退中心（需告警节流，§6.1）
|
||||
in_domain: bool # d ∈ [D_min, D_max]（含边界，§11.2）
|
||||
positive: bool # d > 0（正视差是物理前提，§6.3）
|
||||
|
||||
|
||||
def sample_point(detection: Detection) -> SamplePoint:
|
||||
"""逐框取点（§5 第 ② 步，每目每框一次）。"""
|
||||
finite = all(math.isfinite(value) for value in (detection.x1, detection.y1, detection.x2, detection.y2))
|
||||
return SamplePoint(
|
||||
box=detection,
|
||||
edge_x=detection.x1,
|
||||
center_x=detection.cx,
|
||||
center_y=detection.cy,
|
||||
finite=finite,
|
||||
)
|
||||
|
||||
|
||||
def select_disparity(left: SamplePoint, right: SamplePoint, config: StereoConfig) -> Disparity:
|
||||
"""在同侧边缘视差与框中心视差之间选择（§6.1 主取点 + 越窗回退）。
|
||||
|
||||
规则：边缘视差在域内取边缘；否则若中心视差在域内则回退中心（告警节流）；
|
||||
两者均越窗时保留边缘值（供诊断与违例告警），由有效性门判无效。
|
||||
"""
|
||||
d_edge = left.edge_x - right.edge_x
|
||||
if _in_domain(d_edge, config):
|
||||
return Disparity(d_edge, DisparitySource.LEFT_EDGE, False, True, d_edge > 0.0)
|
||||
d_center = left.center_x - right.center_x
|
||||
if _in_domain(d_center, config):
|
||||
return Disparity(d_center, DisparitySource.CENTER_FALLBACK, True, True, d_center > 0.0)
|
||||
d = d_edge if math.isfinite(d_edge) else d_center
|
||||
return Disparity(d, DisparitySource.LEFT_EDGE, False, False, d > 0.0)
|
||||
|
||||
|
||||
def _in_domain(d_px: float, config: StereoConfig) -> bool:
|
||||
return config.disparity_min_px <= d_px <= config.disparity_max_px
|
||||
@@ -1,215 +1,227 @@
|
||||
"""StereoRanger —— 双目稀疏目标测距（纯算法，规格 docs/双目测距方案.md）。
|
||||
"""StereoRanger —— 双目稀疏目标测距（规格 docs/双目测距技术路线.md）。
|
||||
|
||||
输入为左右目各自、校正后原生坐标系下的检测框列表；不做目标识别，不做
|
||||
图像采集/立体校正。单帧主流程：关联（§6）→ 视差取点（§5.1）→
|
||||
Z=F·B/D 与 (X, Y, R)（§4.2）→ 有效性（§5.4）→ 可选时间滤波（§7）。
|
||||
依赖仅为标准库。
|
||||
单周期五步（§5）：① 同帧契约检查 → ② 取点 → ③ 左右匹配 →
|
||||
④ 配对目标：测距 + 方位源 + 有效性 + 滤波 → ⑤ 单目退化发布。
|
||||
|
||||
输入为左右目各自、校正后原生坐标系下的检测框；不做目标识别，不做图像
|
||||
采集/立体校正。纯计算单元（§11.3）：单周期处理无阻塞、无 IO；同一实例
|
||||
不得并发调用 update()。
|
||||
"""
|
||||
|
||||
import logging
|
||||
import math
|
||||
import time
|
||||
from typing import Dict, List, Optional, Sequence, Tuple
|
||||
from typing import List, Optional, Sequence
|
||||
|
||||
from .config import StereoConfig
|
||||
from .types import BBoxXYXY, Detection, RangeReport
|
||||
from .geometry import confidence_from_cost, evaluate_validity, solve_range, virtual_center_px
|
||||
from .matching import GreedyMatcher, Matcher, MatchPair
|
||||
from .sampling import SamplePoint, sample_point, select_disparity
|
||||
from .target_store import TargetStore
|
||||
from .types import Degradation, Detection, DetectionFrame, Eye, RangeRecord
|
||||
from .warn_throttle import WarnThrottle
|
||||
|
||||
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)
|
||||
_CONFIDENCE_DEGRADE_FACTOR = 0.5 # ⚠占位：连续跳变 ≥ N 帧的置信度降级系数（§6.4）
|
||||
|
||||
|
||||
class StereoRanger:
|
||||
"""双目稀疏目标测距器。
|
||||
"""双目稀疏目标测距器：每帧调用一次 update()，传入同帧左右目检测结果。"""
|
||||
|
||||
用法：每帧调用一次 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] = {}
|
||||
def __init__(self, config: Optional[StereoConfig] = None, matcher: Optional[Matcher] = None) -> None:
|
||||
self._config = config if config is not None else StereoConfig()
|
||||
self._matcher = matcher if matcher is not None else GreedyMatcher()
|
||||
self._store = TargetStore(self._config)
|
||||
self._warn = WarnThrottle()
|
||||
self._frame_index = 0
|
||||
|
||||
@property
|
||||
def config(self) -> StereoConfig:
|
||||
return self._cfg
|
||||
return self._config
|
||||
|
||||
@property
|
||||
def matcher(self) -> Matcher:
|
||||
return self._matcher
|
||||
|
||||
def reset(self) -> None:
|
||||
self._z_state.clear()
|
||||
self._warn_at.clear()
|
||||
"""清空全部运行期状态（滤波 / 一致性 / 隔离 / 节流计数）。"""
|
||||
self._store.clear()
|
||||
self._warn.clear()
|
||||
self._frame_index = 0
|
||||
|
||||
def update(
|
||||
self,
|
||||
left: Sequence[Detection],
|
||||
right: Sequence[Detection],
|
||||
frame_sequence: int = 0,
|
||||
timestamp_s: Optional[float] = None,
|
||||
) -> List[RangeReport]:
|
||||
"""单帧主流程：返回报告列表（每个左目检测一条，保持左目输入顺序）。
|
||||
def update(self, left: DetectionFrame, right: DetectionFrame, now_ms: int = 0) -> List[RangeRecord]:
|
||||
"""单周期处理（§5）：返回本帧记录列表。
|
||||
|
||||
配对成功且测距有效的报告 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))
|
||||
config = self._config
|
||||
if left.frame_id != right.frame_id:
|
||||
self._warn_emit(
|
||||
"frame_mismatch",
|
||||
"左右帧标识不一致（L=%s, R=%s），拒收本帧",
|
||||
left.frame_id,
|
||||
right.frame_id,
|
||||
)
|
||||
return []
|
||||
|
||||
self._frame_index += 1
|
||||
frame_index = self._frame_index
|
||||
left_points = self._drop_invalid([sample_point(det) for det in left.detections])
|
||||
right_points = self._drop_invalid([sample_point(det) for det in right.detections])
|
||||
|
||||
pairs = self._matcher.match(left_points, right_points, config)
|
||||
pair_of_left = {pair.left_index: pair for pair in pairs}
|
||||
paired_right = {pair.right_index for pair in pairs}
|
||||
|
||||
records: List[RangeRecord] = []
|
||||
for index, point_l in enumerate(left_points):
|
||||
pair = pair_of_left.get(index)
|
||||
if pair is None:
|
||||
records.append(self._bearing_only_record(point_l, left.frame_id, now_ms, Eye.LEFT))
|
||||
continue
|
||||
report = self._build_report(det_l, right[r_idx], frame_sequence, timestamp_s)
|
||||
reports.append(report)
|
||||
return reports
|
||||
records.append(
|
||||
self._complete_record(
|
||||
point_l,
|
||||
right_points[pair.right_index],
|
||||
pair,
|
||||
left.frame_id,
|
||||
now_ms,
|
||||
frame_index,
|
||||
)
|
||||
)
|
||||
for index, point_r in enumerate(right_points):
|
||||
if index in paired_right:
|
||||
continue
|
||||
records.append(self._bearing_only_record(point_r, left.frame_id, now_ms, Eye.RIGHT))
|
||||
|
||||
self._store.end_frame(frame_index)
|
||||
return records
|
||||
|
||||
# ---- 内部实现 ----
|
||||
|
||||
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 _complete_record(
|
||||
self,
|
||||
point_l: SamplePoint,
|
||||
point_r: SamplePoint,
|
||||
pair: MatchPair,
|
||||
frame_id: int,
|
||||
now_ms: int,
|
||||
frame_index: int,
|
||||
) -> RangeRecord:
|
||||
"""配对成功的记录：测距 + 方位源 + 有效性（门①②）+ 一致性（门③）+ 滤波。"""
|
||||
config = self._config
|
||||
box_l = point_l.box
|
||||
box_r = point_r.box
|
||||
disparity = select_disparity(point_l, point_r, config)
|
||||
center_mid = virtual_center_px(box_l, box_r)
|
||||
|
||||
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),
|
||||
if disparity.fallback_used:
|
||||
self._warn_emit("center_fallback", "边缘视差越窗，回退框中心（框宽差可能混入视差）")
|
||||
if not disparity.positive:
|
||||
self._warn_emit("disparity_violation", "视差违例 D=%.2f px（须为正），该目标判无效", disparity.d_px)
|
||||
elif not disparity.in_domain:
|
||||
self._warn_emit(
|
||||
"disparity_violation",
|
||||
"视差超界 D=%.2f px（域 [%.2f, %.2f]），该目标判无效",
|
||||
disparity.d_px,
|
||||
config.disparity_min_px,
|
||||
config.disparity_max_px,
|
||||
)
|
||||
|
||||
key = self._store.resolve(
|
||||
box_l.class_id,
|
||||
_shared_track_id(box_l, box_r),
|
||||
center_mid,
|
||||
frame_index,
|
||||
)
|
||||
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)
|
||||
status = self._store.consistency.advance(key, disparity.d_px)
|
||||
if status.invalid:
|
||||
self._warn_emit(
|
||||
"consistency_invalid",
|
||||
"连续 %d 帧视差跳变超门限（%.1f px），质量判无效",
|
||||
status.streak,
|
||||
config.consistency_jump_px,
|
||||
)
|
||||
|
||||
z_raw = config.focal_baseline_px_m / disparity.d_px if disparity.positive else None
|
||||
solution = solve_range(box_l, box_r, disparity.d_px, config) if z_raw is not None else None
|
||||
decision = evaluate_validity(disparity.d_px, z_raw, config)
|
||||
valid = decision.valid and not status.invalid
|
||||
|
||||
confidence = confidence_from_cost(pair.cost)
|
||||
if status.confidence_degraded:
|
||||
confidence *= _CONFIDENCE_DEGRADE_FACTOR
|
||||
if status.invalid:
|
||||
confidence = 0.0
|
||||
|
||||
if valid:
|
||||
z_out = self._store.distance_filter.advance(key, z_raw)
|
||||
else:
|
||||
report.error_est_m = err
|
||||
self._fill_xyz(report, det_l.cx)
|
||||
return report
|
||||
self._store.distance_filter.invalidate(key)
|
||||
z_out = None
|
||||
|
||||
def _fill_xyz(self, report: RangeReport, x_l_px: float) -> None:
|
||||
"""由 Z 与左目（参考目）框中心像素回算左目系 X, Y, R（§4.2）。
|
||||
# 参考斜距与已发布的前向距离同基准：X/Y/Z 均 ∝ Z，按 z_out/z_raw 缩放即精确换基
|
||||
slant_range_m = None
|
||||
if solution is not None:
|
||||
slant_range_m = solution.r_m if z_out is None else solution.r_m * (z_out / z_raw)
|
||||
|
||||
注意：方位源 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)
|
||||
return RangeRecord(
|
||||
frame_id=frame_id,
|
||||
class_id=box_l.class_id,
|
||||
degradation=Degradation.COMPLETE,
|
||||
valid=valid,
|
||||
track_id=box_l.track_id if box_l.track_id is not None else box_r.track_id,
|
||||
forward_distance_m=z_out,
|
||||
slant_range_m=slant_range_m,
|
||||
disparity_px=disparity.d_px,
|
||||
center_mid_px=center_mid,
|
||||
reference_box=box_l.bbox,
|
||||
reference_eye=Eye.LEFT,
|
||||
confidence=confidence,
|
||||
timestamp_ms=now_ms,
|
||||
)
|
||||
|
||||
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,
|
||||
def _bearing_only_record(
|
||||
self,
|
||||
point: SamplePoint,
|
||||
frame_id: int,
|
||||
now_ms: int,
|
||||
eye: Eye,
|
||||
) -> RangeRecord:
|
||||
"""单目退化记录（§5 第 ⑤ 步）：仅方位，valid = false，无距离。"""
|
||||
detection = point.box
|
||||
return RangeRecord(
|
||||
frame_id=frame_id,
|
||||
class_id=detection.class_id,
|
||||
degradation=Degradation.BEARING_ONLY,
|
||||
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),
|
||||
track_id=detection.track_id,
|
||||
center_mid_px=(detection.cx, detection.cy),
|
||||
reference_box=detection.bbox,
|
||||
reference_eye=eye,
|
||||
confidence=detection.confidence,
|
||||
timestamp_ms=now_ms,
|
||||
)
|
||||
|
||||
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:
|
||||
def _drop_invalid(self, points: Sequence[SamplePoint]) -> List[SamplePoint]:
|
||||
"""丢弃坐标非有限值的框（不可参与匹配，也不发布记录），告警节流。"""
|
||||
kept: List[SamplePoint] = []
|
||||
for point in points:
|
||||
if point.finite:
|
||||
kept.append(point)
|
||||
else:
|
||||
self._warn_emit("invalid_box", "检测框坐标非法（非有限值），已丢弃该框")
|
||||
return kept
|
||||
|
||||
def _warn_emit(self, category: str, message: str, *args: object) -> None:
|
||||
"""异常日志（§5 节流规范：第 1 次 + 每满 100 次，带累计计数）。"""
|
||||
if not self._warn.should_emit(category):
|
||||
return
|
||||
self._warn_at[category] = now
|
||||
logger.warning("[%s] %s", category, message)
|
||||
logger.warning("[%s] " + message + "（累计 %d 次）", category, *args, self._warn.count(category))
|
||||
|
||||
|
||||
def _shared_track_id(box_l: Detection, box_r: Detection) -> Optional[int]:
|
||||
"""左右目轨道关联一致且有效时返回该 id（目标隔离键优先路径，§4）。"""
|
||||
if box_l.track_id is not None and box_l.track_id > 0 and box_l.track_id == box_r.track_id:
|
||||
return box_l.track_id
|
||||
return None
|
||||
|
||||
@@ -0,0 +1,119 @@
|
||||
"""目标状态隔离（§4）：track_id 优先 + 框位置最近邻兜底。
|
||||
|
||||
目标隔离键：track_id 可用（左右一致且 > 0）时优先以 (track, 类别, track_id)
|
||||
为键——上游 target_tracker 已在链路中，规格 §4 已按此冻结；否则回退类别 +
|
||||
框位置最近邻（isolation_gate_px 门限，见包 README「实现决策」）。
|
||||
|
||||
连续 filter_reset_frames 帧未再出现的目标状态整体清理（位置 + 距离滤波 +
|
||||
一致性），避免最近邻把新目标接到陈旧状态上。
|
||||
"""
|
||||
|
||||
import math
|
||||
from dataclasses import dataclass
|
||||
from typing import Dict, Hashable, Optional, Tuple
|
||||
|
||||
from .config import StereoConfig
|
||||
from .consistency import ConsistencyMonitor
|
||||
from .distance_filter import DistanceFilter
|
||||
|
||||
_KIND_TRACK = "track"
|
||||
_KIND_NEAREST = "nn"
|
||||
|
||||
|
||||
@dataclass
|
||||
class _TargetState:
|
||||
class_id: int
|
||||
track_id: Optional[int]
|
||||
center_px: Tuple[float, float]
|
||||
last_seen_frame: int
|
||||
|
||||
|
||||
class TargetStore:
|
||||
"""每目标状态容器：隔离键解析 + 帧末老化清理（§11.1 状态清单）。"""
|
||||
|
||||
def __init__(self, config: StereoConfig) -> None:
|
||||
self._config = config
|
||||
self._distance_filter = DistanceFilter(
|
||||
config.smooth_alpha, config.jump_reset_m, config.filter_reset_frames
|
||||
)
|
||||
self._consistency = ConsistencyMonitor(
|
||||
config.consistency_jump_px, config.consistency_N_frames, config.consistency_M_frames
|
||||
)
|
||||
self._states: Dict[Hashable, _TargetState] = {}
|
||||
self._next_nearest_id = 0
|
||||
|
||||
@property
|
||||
def distance_filter(self) -> DistanceFilter:
|
||||
return self._distance_filter
|
||||
|
||||
@property
|
||||
def consistency(self) -> ConsistencyMonitor:
|
||||
return self._consistency
|
||||
|
||||
@property
|
||||
def count(self) -> int:
|
||||
"""当前存活的目标状态数。"""
|
||||
return len(self._states)
|
||||
|
||||
def resolve(
|
||||
self,
|
||||
class_id: int,
|
||||
track_id: Optional[int],
|
||||
center_px: Tuple[float, float],
|
||||
frame_index: int,
|
||||
) -> Hashable:
|
||||
"""解析（或新建）目标 key，并刷新该目标最近位置与出现帧。"""
|
||||
if track_id is not None and track_id > 0:
|
||||
key: Hashable = (_KIND_TRACK, class_id, track_id)
|
||||
self._states[key] = _TargetState(class_id, track_id, center_px, frame_index)
|
||||
return key
|
||||
key = self._nearest_key(class_id, center_px, frame_index)
|
||||
if key is None:
|
||||
self._next_nearest_id += 1
|
||||
key = (_KIND_NEAREST, class_id, self._next_nearest_id)
|
||||
self._states[key] = _TargetState(class_id, None, center_px, frame_index)
|
||||
else:
|
||||
state = self._states[key]
|
||||
state.center_px = center_px
|
||||
state.last_seen_frame = frame_index
|
||||
return key
|
||||
|
||||
def end_frame(self, frame_index: int) -> None:
|
||||
"""帧末收尾：未出现的目标记滤波失效，到期整体清理三类状态。"""
|
||||
stale: List[Hashable] = []
|
||||
for key, state in self._states.items():
|
||||
if state.last_seen_frame == frame_index:
|
||||
continue
|
||||
self._distance_filter.invalidate(key)
|
||||
if frame_index - state.last_seen_frame >= self._config.filter_reset_frames:
|
||||
stale.append(key)
|
||||
for key in stale:
|
||||
del self._states[key]
|
||||
self._distance_filter.drop(key)
|
||||
self._consistency.drop(key)
|
||||
|
||||
def clear(self) -> None:
|
||||
self._states.clear()
|
||||
self._distance_filter.clear()
|
||||
self._consistency.clear()
|
||||
self._next_nearest_id = 0
|
||||
|
||||
def _nearest_key(
|
||||
self,
|
||||
class_id: int,
|
||||
center_px: Tuple[float, float],
|
||||
frame_index: int,
|
||||
) -> Optional[Hashable]:
|
||||
"""同类别、isolation_gate_px 内、本帧尚未被占用的最近状态。"""
|
||||
gate = self._config.isolation_gate_px
|
||||
best_key: Optional[Hashable] = None
|
||||
best_distance: Optional[float] = None
|
||||
for key, state in self._states.items():
|
||||
if key[0] != _KIND_NEAREST or state.class_id != class_id:
|
||||
continue
|
||||
if state.last_seen_frame == frame_index:
|
||||
continue # 本帧已被其他框接管，一一归属
|
||||
distance = math.hypot(center_px[0] - state.center_px[0], center_px[1] - state.center_px[1])
|
||||
if distance <= gate and (best_distance is None or distance < best_distance):
|
||||
best_key, best_distance = key, distance
|
||||
return best_key
|
||||
@@ -1,29 +1,50 @@
|
||||
"""stereo_ranger 输入输出数据结构：Detection / RangeReport。
|
||||
"""stereo_ranger 输入输出数据结构（纯值语义）。
|
||||
|
||||
规格：docs/双目测距方案.md §10 结果记录。
|
||||
规格：docs/双目测距技术路线.md —— §2 输入输出接口、§3 符号约定、§6.6 方位源。
|
||||
|
||||
约定：所有像素坐标位于**已立体校正**的图像坐标系，检测框已映射回该基准
|
||||
（§2.1 校正契约 / 坐标基准一致）。本包仅依赖标准库。
|
||||
"""
|
||||
|
||||
from dataclasses import dataclass
|
||||
from typing import Optional, Tuple
|
||||
from dataclasses import dataclass, field
|
||||
from enum import Enum, auto
|
||||
from typing import Optional, Sequence, Tuple
|
||||
|
||||
BBoxXYXY = Tuple[float, float, float, float]
|
||||
|
||||
|
||||
class Eye(Enum):
|
||||
"""物理目（参考目框来源，§2.2）。"""
|
||||
|
||||
LEFT = auto() # 左目（解算参考系）
|
||||
RIGHT = auto() # 右目
|
||||
|
||||
|
||||
class DisparitySource(Enum):
|
||||
"""视差取点来源（§6.1）。"""
|
||||
|
||||
LEFT_EDGE = auto() # 同侧（左）边缘：v1.0 主方案
|
||||
CENTER_FALLBACK = auto() # 边缘视差越窗后回退框中心
|
||||
|
||||
|
||||
class Degradation(Enum):
|
||||
"""记录退化标记（§2.2）。描述记录的来源形态，质量由 valid 单独表达。"""
|
||||
|
||||
COMPLETE = auto() # 完整：配对成功（距离 + 方位两路都在）
|
||||
BEARING_ONLY = auto() # 仅方位：单目检出，无距离
|
||||
|
||||
|
||||
@dataclass
|
||||
class Detection:
|
||||
"""校正后原生坐标系下的一个检测框（单位 px）。
|
||||
|
||||
调用方负责：左右目预处理几何完全一致，且框坐标已映射回原生分辨率
|
||||
（规格 §4.3）。x1/y1 为左上角，x2/y2 为右下角（x1 即"左边缘"）。
|
||||
"""
|
||||
"""校正后图像坐标系下的一个检测框（px；x1/y1 左上角，x2/y2 右下角）。"""
|
||||
|
||||
x1: float
|
||||
y1: float
|
||||
x2: float
|
||||
y2: float
|
||||
conf: float = 1.0
|
||||
cls_id: int = 0
|
||||
track_id: int = 0 # 未知为 0
|
||||
confidence: float = 1.0
|
||||
class_id: int = 0
|
||||
track_id: Optional[int] = None # 检测侧轨道关联；None = 无（§2.2，v1.0 可无）
|
||||
|
||||
@property
|
||||
def width(self) -> float:
|
||||
@@ -41,31 +62,46 @@ class Detection:
|
||||
def cy(self) -> float:
|
||||
return (self.y1 + self.y2) / 2.0
|
||||
|
||||
@property
|
||||
def bbox(self) -> BBoxXYXY:
|
||||
return (self.x1, self.y1, self.x2, self.y2)
|
||||
|
||||
|
||||
@dataclass
|
||||
class RangeReport:
|
||||
"""单目标单帧测距结果（一个左目检测对应一条记录）。
|
||||
class DetectionFrame:
|
||||
"""单目一帧：帧标识 + 检测框列表。
|
||||
|
||||
matched=False 表示该左目检测本帧未配对：valid 恒为 False，距离字段为
|
||||
None，centre_pixel_x/y_mid 为单目自身中心（方位回退，规格 §11.3）。
|
||||
matched=True 且 valid=False 表示已配对但距离超出有效工作域
|
||||
（规格 §5.4）：距离字段仍给出估计值供诊断，方位照常发布。
|
||||
同帧契约（§2.1）：左右两帧的 frame_id 必须一致，否则整帧拒收、输出空。
|
||||
"""
|
||||
|
||||
frame_sequence: int
|
||||
frame_id: int
|
||||
detections: Sequence[Detection] = field(default_factory=tuple)
|
||||
|
||||
|
||||
@dataclass
|
||||
class RangeRecord:
|
||||
"""目标距离记录（§2.2，字段与文档表格逐项对应）。
|
||||
|
||||
- forward_distance_m（Z）：唯一供距离通道消费的量；valid=False 或单目
|
||||
退化时为 None（不得强行估计，§12.3 红线 3）；
|
||||
- slant_range_m（R）：参考值，不进控制回路；无效但仍可解算时保留原始
|
||||
测量值供诊断；
|
||||
- disparity_px（D）：本次视差（调试与交叉校验用）；
|
||||
- center_mid_px（c_mid）：方位源；配对记录为两目框中心等权平均（虚拟
|
||||
双目中心，§6.6），单目退化为该目框中心；
|
||||
- reference_box / reference_eye：参考目框（配对记录恒为左目）。
|
||||
"""
|
||||
|
||||
frame_id: int
|
||||
class_id: int
|
||||
track_id: int
|
||||
matched: bool
|
||||
degradation: Degradation
|
||||
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"
|
||||
track_id: Optional[int] = None
|
||||
forward_distance_m: Optional[float] = None
|
||||
slant_range_m: Optional[float] = None
|
||||
disparity_px: Optional[float] = None
|
||||
center_mid_px: Tuple[float, float] = (0.0, 0.0)
|
||||
reference_box: Optional[BBoxXYXY] = None
|
||||
reference_eye: Eye = Eye.LEFT
|
||||
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
|
||||
timestamp_ms: int = 0
|
||||
|
||||
@@ -0,0 +1,30 @@
|
||||
"""告警节流器（§4/§5）：每类异常"第 1 次 + 每满 100 次"打印，带累计计数。
|
||||
|
||||
高频热路径（每帧数学）不打日志；所有异常日志走本通道，且不落在数学热路径
|
||||
的决策上（§11.3 单周期处理无阻塞、无 IO）。
|
||||
"""
|
||||
|
||||
from typing import Dict
|
||||
|
||||
|
||||
class WarnThrottle:
|
||||
"""按异常类独立计数与限频。"""
|
||||
|
||||
def __init__(self, every: int = 100) -> None:
|
||||
if every < 1:
|
||||
raise ValueError("every 必须 ≥ 1")
|
||||
self._every = every
|
||||
self._counts: Dict[str, int] = {}
|
||||
|
||||
def should_emit(self, category: str) -> bool:
|
||||
"""推进该类异常计数并返回本次是否应打印（第 1 次 + 每满 every 次）。"""
|
||||
count = self._counts.get(category, 0) + 1
|
||||
self._counts[category] = count
|
||||
return count == 1 or count % self._every == 0
|
||||
|
||||
def count(self, category: str) -> int:
|
||||
"""该类异常的累计次数。"""
|
||||
return self._counts.get(category, 0)
|
||||
|
||||
def clear(self) -> None:
|
||||
self._counts.clear()
|
||||
@@ -1,76 +0,0 @@
|
||||
"""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))
|
||||
@@ -1,302 +0,0 @@
|
||||
"""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)
|
||||
@@ -3,8 +3,9 @@
|
||||
移植自 reference-c++/visual_track_control/tests/tracking_control_law_test.cpp。
|
||||
覆盖（物理追踪思路 §2.3/§4/§5/§6/§7/§9）：超龄降级（姿态缺失/过期、视觉
|
||||
超龄、降级优先级）、判丢零速刹车、Coast 清积分与标志、速率式/位置式航向
|
||||
通道（含绝对航向合成与回绕，A2）、垂直统一速度支路（A1）、距离通道三种
|
||||
无距离降级动作、首拍变化率限幅（S6）、机体系→NED 输出旋转（A4）。
|
||||
通道（含绝对航向合成与回绕，A2）、垂直统一速度支路（A1）、距离通道无距离
|
||||
降级动作（含 EXIT 门限/grace 重置/零门限立即退出）、距离超龄回落、首拍变化率
|
||||
限幅（S6）、机体系→NED 输出旋转（A4）。
|
||||
"""
|
||||
|
||||
import math
|
||||
@@ -26,7 +27,7 @@ def make_config() -> TrackingConfig:
|
||||
cfg = TrackingConfig()
|
||||
cfg.control.frequency_hz = 20.0
|
||||
cfg.control.visual_stale_ms = 200
|
||||
cfg.control.radar_stale_ms = 500
|
||||
cfg.control.distance_stale_ms = 500
|
||||
cfg.control.attitude_stale_ms = 300
|
||||
cfg.camera.image_width = 1000
|
||||
cfg.camera.image_height = 1000
|
||||
@@ -55,7 +56,7 @@ def make_config() -> TrackingConfig:
|
||||
|
||||
|
||||
def make_snapshot(now_ms: int) -> ControlSnapshot:
|
||||
"""新鲜快照：目标在画面中心，姿态/追踪/雷达均新鲜。"""
|
||||
"""新鲜快照：目标在画面中心，姿态/追踪/距离均新鲜。"""
|
||||
return ControlSnapshot(
|
||||
track=TrackState(tracked=True, is_predicted=False,
|
||||
center=(500.0, 500.0)),
|
||||
@@ -213,9 +214,9 @@ def test_vertical_velocity_clamped():
|
||||
def test_distance_approach_when_far():
|
||||
law = TrackingControlLaw(make_config())
|
||||
snap = make_snapshot(1000)
|
||||
snap.radar_distance_m = 30.0 # Δd = +20 → kp 输出 10 → 限幅 3
|
||||
snap.associated_to_target = True
|
||||
snap.radar_time_ms = 1000
|
||||
snap.target_distance_m = 30.0 # Δd = +20 → kp 输出 10 → 限幅 3
|
||||
snap.distance_valid = True
|
||||
snap.distance_time_ms = 1000
|
||||
out = law.update(snap, 1000)
|
||||
assert out.vx_mps == pytest.approx(3.0, abs=1e-9)
|
||||
|
||||
@@ -223,17 +224,17 @@ def test_distance_approach_when_far():
|
||||
def test_distance_retreat_clamped_when_too_close():
|
||||
law = TrackingControlLaw(make_config())
|
||||
snap = make_snapshot(1000)
|
||||
snap.radar_distance_m = 5.0 # Δd = -5 → kp 输出 -2.5 → 后退限幅 -1
|
||||
snap.associated_to_target = True
|
||||
snap.radar_time_ms = 1000
|
||||
snap.target_distance_m = 5.0 # Δd = -5 → kp 输出 -2.5 → 后退限幅 -1
|
||||
snap.distance_valid = True
|
||||
snap.distance_time_ms = 1000
|
||||
out = law.update(snap, 1000)
|
||||
assert out.vx_mps == pytest.approx(-1.0, abs=1e-9)
|
||||
|
||||
|
||||
def test_no_distance_hold_zeros_vx():
|
||||
law = TrackingControlLaw(make_config()) # 缺省 kHold
|
||||
law = TrackingControlLaw(make_config()) # 基准配置 HOLD
|
||||
snap = make_snapshot(1000)
|
||||
snap.associated_to_target = False
|
||||
snap.distance_valid = False
|
||||
out = law.update(snap, 1000)
|
||||
assert out.mode is ControlMode.VELOCITY_HEADING
|
||||
assert out.vx_mps == 0.0
|
||||
@@ -244,29 +245,79 @@ def test_no_distance_slow_approach():
|
||||
cfg.distance.no_distance_action = NoDistanceAction.SLOW_APPROACH
|
||||
law = TrackingControlLaw(cfg)
|
||||
snap = make_snapshot(1000)
|
||||
snap.associated_to_target = False
|
||||
snap.distance_valid = False
|
||||
out = law.update(snap, 1000)
|
||||
assert out.vx_mps == pytest.approx(0.5, abs=1e-9)
|
||||
|
||||
|
||||
def test_no_distance_exit_degrades():
|
||||
def test_no_distance_exit_waits_grace_then_degrades():
|
||||
# S1 拍板：EXIT 需持续无距离达门限才降级；grace 期间按 SLOW_APPROACH 行为
|
||||
cfg = make_config()
|
||||
cfg.distance.no_distance_action = NoDistanceAction.EXIT
|
||||
cfg.distance.no_distance_exit_grace_ms = 200
|
||||
law = TrackingControlLaw(cfg)
|
||||
snap = make_snapshot(1000)
|
||||
snap.associated_to_target = False
|
||||
snap.distance_valid = False
|
||||
out1 = law.update(snap, 1000)
|
||||
assert out1.mode is ControlMode.VELOCITY_HEADING
|
||||
assert out1.vx_mps == pytest.approx(0.5, abs=1e-9)
|
||||
snap.track_time_ms = 1200 # 保持视觉/姿态新鲜，聚焦距离通道
|
||||
snap.attitude_time_ms = 1200
|
||||
out2 = law.update(snap, 1200) # 恰达门限 → 降级
|
||||
assert out2.mode is ControlMode.DEGRADED_HOLD
|
||||
assert out2.degraded_reason is DegradedReason.NO_DISTANCE_EXIT
|
||||
assert not out2.valid
|
||||
|
||||
|
||||
def test_no_distance_exit_grace_resets_on_valid_frame():
|
||||
cfg = make_config()
|
||||
cfg.distance.no_distance_action = NoDistanceAction.EXIT
|
||||
cfg.distance.no_distance_exit_grace_ms = 200
|
||||
law = TrackingControlLaw(cfg)
|
||||
snap = make_snapshot(1000)
|
||||
snap.distance_valid = False
|
||||
assert law.update(snap, 1000).mode is ControlMode.VELOCITY_HEADING
|
||||
|
||||
snap.track_time_ms = 1100 # 有效距离帧：重置计时
|
||||
snap.attitude_time_ms = 1100
|
||||
snap.distance_valid = True
|
||||
snap.target_distance_m = 30.0
|
||||
snap.distance_time_ms = 1100
|
||||
assert law.update(snap, 1100).mode is ControlMode.VELOCITY_HEADING
|
||||
|
||||
snap.track_time_ms = 1250 # 再次无效：重新计时
|
||||
snap.attitude_time_ms = 1250
|
||||
snap.distance_valid = False
|
||||
snap.target_distance_m = 0.0
|
||||
assert law.update(snap, 1250).mode is ControlMode.VELOCITY_HEADING # 0 ms < 200 ms
|
||||
|
||||
snap.track_time_ms = 1500
|
||||
snap.attitude_time_ms = 1500
|
||||
out = law.update(snap, 1500) # 250 ms ≥ 200 ms → 降级
|
||||
assert out.mode is ControlMode.DEGRADED_HOLD
|
||||
assert out.degraded_reason is DegradedReason.NO_DISTANCE_EXIT
|
||||
|
||||
|
||||
def test_no_distance_exit_zero_grace_degrades_immediately():
|
||||
# grace = 0 保留旧"首个无效拍即退出"行为
|
||||
cfg = make_config()
|
||||
cfg.distance.no_distance_action = NoDistanceAction.EXIT
|
||||
cfg.distance.no_distance_exit_grace_ms = 0
|
||||
law = TrackingControlLaw(cfg)
|
||||
snap = make_snapshot(1000)
|
||||
snap.distance_valid = False
|
||||
out = law.update(snap, 1000)
|
||||
assert out.mode is ControlMode.DEGRADED_HOLD
|
||||
assert out.degraded_reason is DegradedReason.NO_DISTANCE_EXIT
|
||||
assert not out.valid
|
||||
|
||||
|
||||
def test_stale_radar_falls_back_to_no_distance():
|
||||
law = TrackingControlLaw(make_config()) # kHold
|
||||
def test_stale_distance_falls_back_to_no_distance():
|
||||
law = TrackingControlLaw(make_config()) # HOLD
|
||||
snap = make_snapshot(1000)
|
||||
snap.radar_distance_m = 30.0
|
||||
snap.associated_to_target = True
|
||||
snap.radar_time_ms = 400 # 600 ms 前 > 500 ms 阈值 → 距离不可用
|
||||
snap.target_distance_m = 30.0
|
||||
snap.distance_valid = True
|
||||
snap.distance_time_ms = 400 # 600 ms 前 > 500 ms 阈值 → 距离不可用
|
||||
out = law.update(snap, 1000)
|
||||
assert out.mode is ControlMode.VELOCITY_HEADING
|
||||
assert out.vx_mps == 0.0
|
||||
@@ -280,9 +331,9 @@ def test_first_tick_vx_slews_from_zero():
|
||||
cfg.accel_limit.ax_mps2 = 2.0
|
||||
law = TrackingControlLaw(cfg)
|
||||
snap = make_snapshot(1000)
|
||||
snap.radar_distance_m = 30.0 # 通道输出本应为 3.0
|
||||
snap.associated_to_target = True
|
||||
snap.radar_time_ms = 1000
|
||||
snap.target_distance_m = 30.0 # 通道输出本应为 3.0
|
||||
snap.distance_valid = True
|
||||
snap.distance_time_ms = 1000
|
||||
out = law.update(snap, 1000) # 首拍：dt 按 1/20 Hz 兜底
|
||||
assert out.vx_mps == pytest.approx(2.0 * (1.0 / 20.0), abs=1e-9) # 0.1
|
||||
|
||||
@@ -293,9 +344,9 @@ def test_output_rotated_to_ned_by_yaw():
|
||||
law = TrackingControlLaw(make_config())
|
||||
snap = make_snapshot(1000)
|
||||
snap.yaw_rad = math.pi / 2.0 # 机头朝东
|
||||
snap.radar_distance_m = 30.0
|
||||
snap.associated_to_target = True
|
||||
snap.radar_time_ms = 1000
|
||||
snap.target_distance_m = 30.0
|
||||
snap.distance_valid = True
|
||||
snap.distance_time_ms = 1000
|
||||
out = law.update(snap, 1000)
|
||||
# 机体前向 3.0 → 东向 3.0，北向 ≈ 0
|
||||
assert out.vx_mps == pytest.approx(0.0, abs=1e-9)
|
||||
@@ -306,9 +357,9 @@ def test_zero_yaw_keeps_forward_as_north():
|
||||
law = TrackingControlLaw(make_config())
|
||||
snap = make_snapshot(1000)
|
||||
snap.yaw_rad = 0.0
|
||||
snap.radar_distance_m = 30.0
|
||||
snap.associated_to_target = True
|
||||
snap.radar_time_ms = 1000
|
||||
snap.target_distance_m = 30.0
|
||||
snap.distance_valid = True
|
||||
snap.distance_time_ms = 1000
|
||||
out = law.update(snap, 1000)
|
||||
assert out.vx_mps == pytest.approx(3.0, abs=1e-9)
|
||||
assert out.vy_mps == pytest.approx(0.0, abs=1e-9)
|
||||
@@ -321,17 +372,17 @@ def test_reset_restores_slew_from_zero():
|
||||
cfg.accel_limit.ax_mps2 = 2.0
|
||||
law = TrackingControlLaw(cfg)
|
||||
snap = make_snapshot(1000)
|
||||
snap.radar_distance_m = 30.0
|
||||
snap.associated_to_target = True
|
||||
snap.radar_time_ms = 1000
|
||||
snap.target_distance_m = 30.0
|
||||
snap.distance_valid = True
|
||||
snap.distance_time_ms = 1000
|
||||
for t in range(1000, 3000, 50):
|
||||
snap.track_time_ms = t
|
||||
snap.radar_time_ms = t
|
||||
snap.distance_time_ms = t
|
||||
snap.attitude_time_ms = t
|
||||
law.update(snap, t) # 若干拍后 vx 已爬升到限幅 3.0
|
||||
law.reset()
|
||||
snap.track_time_ms = 3000
|
||||
snap.radar_time_ms = 3000
|
||||
snap.distance_time_ms = 3000
|
||||
snap.attitude_time_ms = 3000
|
||||
out = law.update(snap, 3000)
|
||||
# Reset 后限幅器归零：又从 0.1 开始渐变
|
||||
|
||||
@@ -15,7 +15,7 @@ from pathlib import Path
|
||||
import pytest
|
||||
|
||||
from visual_track_control.config import (HeadingMode, NoDistanceAction,
|
||||
load_from_json)
|
||||
TrackingConfig, load_from_json)
|
||||
|
||||
_REPO_ROOT = Path(__file__).resolve().parents[3]
|
||||
REAL_CONFIG = _REPO_ROOT / "reference-c++" / "visual_track_control" / "config" / "track_control.json"
|
||||
@@ -27,7 +27,7 @@ def full_config() -> dict:
|
||||
"control": {
|
||||
"frequency_hz": 20.0,
|
||||
"visual_stale_ms": 200,
|
||||
"radar_stale_ms": 500,
|
||||
"distance_stale_ms": 500,
|
||||
"attitude_stale_ms": 300,
|
||||
},
|
||||
"camera": {
|
||||
@@ -57,6 +57,7 @@ def full_config() -> dict:
|
||||
"retreat_velocity_limit_mps": 1.0,
|
||||
"no_distance_action": "hold",
|
||||
"no_distance_approach_limit_mps": 0.5,
|
||||
"no_distance_exit_grace_ms": 3000,
|
||||
},
|
||||
"accel_limit": {
|
||||
"ax_mps2": 2.0,
|
||||
@@ -84,7 +85,9 @@ def test_load_real_config_succeeds():
|
||||
assert cfg.vertical.vz_gain_mps_per_deg == 0.1
|
||||
assert cfg.vertical.vz_limit_mps == 2.0
|
||||
assert cfg.distance.d_exp_m == 10.0
|
||||
assert cfg.distance.no_distance_action is NoDistanceAction.HOLD
|
||||
assert cfg.distance.no_distance_action is NoDistanceAction.SLOW_APPROACH
|
||||
assert cfg.distance.no_distance_exit_grace_ms == 3000
|
||||
assert cfg.control.distance_stale_ms == 500
|
||||
assert cfg.accel_limit.ax_mps2 == 2.0
|
||||
|
||||
|
||||
@@ -169,3 +172,18 @@ def test_fov_out_of_range_throws(tmp_path):
|
||||
with pytest.raises(ValueError) as exc_info:
|
||||
load_from_json(path)
|
||||
assert "camera.fov_h_deg" in str(exc_info.value)
|
||||
|
||||
|
||||
def test_default_no_distance_action_is_slow_approach():
|
||||
# S1 拍板：双目有效域外无距离是常态，默认定速接近（纪要 D4/S1）
|
||||
assert (TrackingConfig().distance.no_distance_action
|
||||
is NoDistanceAction.SLOW_APPROACH)
|
||||
|
||||
|
||||
def test_negative_exit_grace_throws(tmp_path):
|
||||
config = full_config()
|
||||
config["distance"]["no_distance_exit_grace_ms"] = -1
|
||||
path = write_temp_json(tmp_path, config, "vtc_cfg_badgrace.json")
|
||||
with pytest.raises(ValueError) as exc_info:
|
||||
load_from_json(path)
|
||||
assert "no_distance_exit_grace_ms" in str(exc_info.value)
|
||||
|
||||
@@ -9,7 +9,7 @@
|
||||
| 名称 | 说明 |
|
||||
| --- | --- |
|
||||
| `TrackingConfig` / `load_from_json(path)` | 配置加载（启动期校验，错误携带字段名，抛 `ValueError`） |
|
||||
| `HeadingMode` / `NoDistanceAction` | 配置枚举（rate/position；hold/slow_approach/exit） |
|
||||
| `HeadingMode` / `NoDistanceAction` | 配置枚举（rate/position；hold/slow_approach/exit，默认 slow_approach；exit 需持续无距离达 `no_distance_exit_grace_ms` 才降级） |
|
||||
| `TrackingControlLaw(cfg)` | 主控律 |
|
||||
| `.update(snapshot, now_ms) -> ControlOutput` | 单周期八步流程（姿态/视觉超龄 → 判丢 → Coast → 三通道 → NED 旋转） |
|
||||
| `.reset()` | 停止闭环时清全部内部状态 |
|
||||
|
||||
@@ -23,18 +23,19 @@ class HeadingMode(Enum):
|
||||
|
||||
|
||||
class NoDistanceAction(Enum):
|
||||
"""雷达距离不可用（未关联/超龄）时的距离通道降级动作（§6.3）。"""
|
||||
"""目标距离不可用（无效/超龄）时的距离通道降级动作（§6.3）。"""
|
||||
|
||||
HOLD = "hold" # 保持：前向速度指令清零
|
||||
SLOW_APPROACH = "slow_approach" # 限速接近：按 no_distance_approach_limit_mps 缓速
|
||||
EXIT = "exit" # 退出近距操作：输出降级保持
|
||||
EXIT = "exit" # 退出近距操作：持续无距离达 no_distance_exit_grace_ms
|
||||
# 才降级保持；grace 期间按 SLOW_APPROACH 行为
|
||||
|
||||
|
||||
@dataclass
|
||||
class ControlGroupConfig:
|
||||
frequency_hz: float = 20.0
|
||||
visual_stale_ms: int = 200
|
||||
radar_stale_ms: int = 500
|
||||
distance_stale_ms: int = 500
|
||||
attitude_stale_ms: int = 300
|
||||
|
||||
|
||||
@@ -70,8 +71,11 @@ class DistanceGroupConfig:
|
||||
derivative_filter_coef: float = 0.0
|
||||
approach_velocity_limit_mps: float = 3.0
|
||||
retreat_velocity_limit_mps: float = 1.0
|
||||
no_distance_action: NoDistanceAction = NoDistanceAction.HOLD
|
||||
# 默认定速接近（D4 远场策略）：双目有效域外无距离是常态，退出仅留给
|
||||
# 持续无距离（传感器级故障）场景
|
||||
no_distance_action: NoDistanceAction = NoDistanceAction.SLOW_APPROACH
|
||||
no_distance_approach_limit_mps: float = 0.5
|
||||
no_distance_exit_grace_ms: int = 3000 # EXIT 门限：持续无距离达此时长才退出（毫秒，占位）
|
||||
|
||||
|
||||
@dataclass
|
||||
@@ -155,9 +159,9 @@ def _load_control(root: dict) -> ControlGroupConfig:
|
||||
c.frequency_hz = _require_positive(
|
||||
_require_number(s, "control", "frequency_hz"), "control.frequency_hz")
|
||||
c.visual_stale_ms = _require_integer(s, "control", "visual_stale_ms")
|
||||
c.radar_stale_ms = _require_integer(s, "control", "radar_stale_ms")
|
||||
c.distance_stale_ms = _require_integer(s, "control", "distance_stale_ms")
|
||||
c.attitude_stale_ms = _require_integer(s, "control", "attitude_stale_ms")
|
||||
if c.visual_stale_ms <= 0 or c.radar_stale_ms <= 0 or c.attitude_stale_ms <= 0:
|
||||
if c.visual_stale_ms <= 0 or c.distance_stale_ms <= 0 or c.attitude_stale_ms <= 0:
|
||||
raise ValueError("配置非法: control.*_stale_ms 必须为正")
|
||||
return c
|
||||
|
||||
@@ -240,6 +244,10 @@ def _load_distance(root: dict) -> DistanceGroupConfig:
|
||||
d.no_distance_approach_limit_mps = _require_non_negative(
|
||||
_require_number(s, "distance", "no_distance_approach_limit_mps"),
|
||||
"distance.no_distance_approach_limit_mps")
|
||||
# 0 容许：等效旧版"首个无效拍即退出"行为
|
||||
d.no_distance_exit_grace_ms = _require_non_negative(
|
||||
_require_integer(s, "distance", "no_distance_exit_grace_ms"),
|
||||
"distance.no_distance_exit_grace_ms")
|
||||
return d
|
||||
|
||||
|
||||
|
||||
@@ -10,7 +10,8 @@
|
||||
④ 像素偏移 → 视线角 β/α（全程针孔模型）
|
||||
⑤ 水平航向（速率式/位置式，A2：位置式合成绝对 NED 航向）
|
||||
⑥ 垂直统一速度支路（A1：不切姿态微调模式）
|
||||
⑦ 距离通道 PID → 前向速度；无距离按配置降级（hold/slow_approach/exit）
|
||||
⑦ 距离通道 PID → 前向速度；无距离按配置降级（hold/slow_approach/exit，
|
||||
exit 需持续无距离达 no_distance_exit_grace_ms 门限）
|
||||
⑧ 机体系 → 局部 NED 旋转（A4）
|
||||
|
||||
日志纪律与 C++ 版一致：仅状态切换点打印，热路径零日志。
|
||||
@@ -78,6 +79,10 @@ class TrackingControlLaw:
|
||||
self._vz_limiter = SlewRateLimiter(cfg.accel_limit.az_mps2)
|
||||
self._last_now_ms = 0
|
||||
self._last_mode = ControlMode.DEGRADED_HOLD
|
||||
# EXIT 无距离计时（§6.3：持续无距离达 no_distance_exit_grace_ms 才退出，
|
||||
# 任一有效距离帧重置）
|
||||
self._no_distance_active = False
|
||||
self._no_distance_since_ms = 0
|
||||
|
||||
def update(self, snapshot: ControlSnapshot, now_ms: int) -> ControlOutput:
|
||||
# ---- 节拍推算（§7.1 固定周期；异常节拍用配置频率兜底） ----
|
||||
@@ -107,6 +112,7 @@ class TrackingControlLaw:
|
||||
|
||||
# ---- ② 判丢 → 零速刹车（§6.4：tracked == false 执行 BRAKE_HOVER） ----
|
||||
if not snapshot.track.tracked:
|
||||
self._no_distance_active = False # 刹车期间不计 EXIT 无距离时长
|
||||
if self._last_mode is not ControlMode.BRAKE_HOVER:
|
||||
# 进入刹车：清积分与限幅器历史，恢复跟踪后指令从零渐变（S6 首条限幅）
|
||||
self._distance_pid.reset()
|
||||
@@ -156,14 +162,16 @@ class TrackingControlLaw:
|
||||
self._cfg.vertical.vz_limit_mps)
|
||||
|
||||
# ---- ⑦ 距离通道（§6.2 / §6.3） ----
|
||||
# 雷达距离不得直接当目标距离：仅关联成立且未超龄才可用（§6.3、FR-041）
|
||||
distance_available = (snapshot.associated_to_target
|
||||
and snapshot.radar_distance_m > 0.0
|
||||
and not is_stale(snapshot.radar_time_ms, now_ms,
|
||||
self._cfg.control.radar_stale_ms))
|
||||
# 目标距离来自检测框自身的双目测距：仅 distance_valid 且未超龄才可用
|
||||
# （§6.3、FR-041）
|
||||
distance_available = (snapshot.distance_valid
|
||||
and snapshot.target_distance_m > 0.0
|
||||
and not is_stale(snapshot.distance_time_ms, now_ms,
|
||||
self._cfg.control.distance_stale_ms))
|
||||
if distance_available:
|
||||
self._no_distance_active = False # 任一有效距离帧重置 EXIT 计时
|
||||
# Δd = d − d_exp：d > d_exp 前向接近，d < d_exp 减速/后退（§6.2）
|
||||
delta_d = snapshot.radar_distance_m - self._cfg.distance.d_exp_m
|
||||
delta_d = snapshot.target_distance_m - self._cfg.distance.d_exp_m
|
||||
vx_raw = self._distance_pid.update(delta_d, dt_s)
|
||||
slewed = self._vx_limiter.limit(vx_raw, dt_s) # 加速度限幅（§8）
|
||||
out.vx_mps = clamp(slewed,
|
||||
@@ -182,10 +190,20 @@ class TrackingControlLaw:
|
||||
out.vx_mps = self._vx_limiter.limit(
|
||||
self._cfg.distance.no_distance_approach_limit_mps, dt_s)
|
||||
else: # NoDistanceAction.EXIT
|
||||
# 退出近距操作：整体降级保持
|
||||
out.mode = ControlMode.DEGRADED_HOLD
|
||||
out.degraded_reason = DegradedReason.NO_DISTANCE_EXIT
|
||||
return self._finish_and_log(out, "距离未关联，退出近距操作")
|
||||
# 退出近距操作：双目有效域外无距离是常态（D4 远场定速接近），
|
||||
# 首个无效拍不退出；持续无距离达 no_distance_exit_grace_ms
|
||||
# （疑似传感器级故障）才降级。grace 期间按 SLOW_APPROACH 行为
|
||||
# 执行（经加速度限幅，vx 平滑过渡）；grace=0 保留旧"立即退出"行为
|
||||
if not self._no_distance_active:
|
||||
self._no_distance_active = True
|
||||
self._no_distance_since_ms = now_ms
|
||||
if (now_ms - self._no_distance_since_ms
|
||||
>= self._cfg.distance.no_distance_exit_grace_ms):
|
||||
out.mode = ControlMode.DEGRADED_HOLD
|
||||
out.degraded_reason = DegradedReason.NO_DISTANCE_EXIT
|
||||
return self._finish_and_log(out, "持续无距离达门限，退出近距操作")
|
||||
out.vx_mps = self._vx_limiter.limit(
|
||||
self._cfg.distance.no_distance_approach_limit_mps, dt_s)
|
||||
|
||||
# ---- ⑧ 机体系 → 局部 NED 旋转（§2.3，A4 修复） ----
|
||||
# 各通道限幅均在机体系完成（旋转为正交变换，合速度不变，限幅约束
|
||||
@@ -207,6 +225,7 @@ class TrackingControlLaw:
|
||||
self._yaw_rate_limiter.reset()
|
||||
self._vx_limiter.reset()
|
||||
self._vz_limiter.reset()
|
||||
self._no_distance_active = False
|
||||
self._last_now_ms = 0
|
||||
self._last_mode = ControlMode.DEGRADED_HOLD
|
||||
|
||||
|
||||
@@ -55,9 +55,10 @@ class ControlSnapshot:
|
||||
track: TrackState = field(default_factory=TrackState)
|
||||
track_time_ms: int = 0 # 取样单调时钟（毫秒），<=0 视为从未取样
|
||||
|
||||
radar_distance_m: float = 0.0 # 机头前向单点距离（米）
|
||||
associated_to_target: bool = False # 相机—雷达空间关联成立标志（未关联不得当目标距离）
|
||||
radar_time_ms: int = 0
|
||||
# 目标距离（双目立体测距，§6.3：distance_valid 为 False 时不得当作目标距离）
|
||||
target_distance_m: float = 0.0 # 目标距离（米，双目前向分量 Z）
|
||||
distance_valid: bool = False # 距离有效标志（测距来自检测框自身，无跨模态关联）
|
||||
distance_time_ms: int = 0 # 取样单调时钟（毫秒）
|
||||
|
||||
attitude_present: bool = False # 姿态源当前是否可用（过期 → 禁止控制）
|
||||
attitude_time_ms: int = 0
|
||||
|
||||
Reference in New Issue
Block a user