新增双目测距模块v1.0（未测试）

2026-09-09 15:38:25 +08:00
parent e689271621
commit 3325465359
11 changed files with 1382 additions and 0 deletions
@@ -0,0 +1,215 @@
+"""StereoRanger —— 双目稀疏目标测距（纯算法，规格 docs/双目测距方案.md）。
+
+输入为左右目各自、校正后原生坐标系下的检测框列表；不做目标识别，不做
+图像采集/立体校正。单帧主流程：关联（§6）→ 视差取点（§5.1）→
+Z=F·B/D 与 (X, Y, R)（§4.2）→ 有效性（§5.4）→ 可选时间滤波（§7）。
+依赖仅为标准库。
+"""
+
+import logging
+import math
+import time
+from typing import Dict, List, Optional, Sequence, Tuple
+
+from .config import StereoConfig
+from .types import BBoxXYXY, Detection, RangeReport
+
+logger = logging.getLogger("stereo_ranger")
+
+DISP_SOURCE_EDGE = "left_edge"
+DISP_SOURCE_CENTER = "center"
+
+
+def focal_from_hfov(image_width_px: float, hfov_deg: float) -> float:
+    """由水平视场与图像宽近似像素焦距：F = (W/2) / tan(HFOV/2)（§5.3）。"""
+    return (image_width_px / 2.0) / math.tan(math.radians(hfov_deg) / 2.0)
+
+
+def range_error_abs(z_m: float, fx_px: float, baseline_m: float, delta_d_px: float = 1.0) -> float:
+    """绝对距离误差估计 δZ ≈ Z^2·δD/(F·B)（§5.2）。"""
+    return z_m * z_m * delta_d_px / (fx_px * baseline_m)
+
+
+def range_error_rel(z_m: float, fx_px: float, baseline_m: float, delta_d_px: float = 1.0) -> float:
+    """相对距离误差 δZ/Z ≈ Z·δD/(F·B)（§5.2）。"""
+    return z_m * delta_d_px / (fx_px * baseline_m)
+
+
+class StereoRanger:
+    """双目稀疏目标测距器。
+
+    用法：每帧调用一次 update()，传入同一逻辑帧的左右目检测结果。
+    """
+
+    def __init__(self, config: Optional[StereoConfig] = None):
+        self._cfg = config if config is not None else StereoConfig()
+        self._z_state: Dict[Tuple[int, int], float] = {}  # (cls_id, track_id) -> 滤波 Z
+        self._warn_at: Dict[str, float] = {}
+
+    @property
+    def config(self) -> StereoConfig:
+        return self._cfg
+
+    def reset(self) -> None:
+        self._z_state.clear()
+        self._warn_at.clear()
+
+    def update(
+        self,
+        left: Sequence[Detection],
+        right: Sequence[Detection],
+        frame_sequence: int = 0,
+        timestamp_s: Optional[float] = None,
+    ) -> List[RangeReport]:
+        """单帧主流程：返回报告列表（每个左目检测一条，保持左目输入顺序）。
+
+        配对成功且测距有效的报告 valid=True；配对成功但超出有效工作域的
+        valid=False（距离仍给出，§5.4）；未配对的左目检测 matched=False，
+        仅发布单目方位（§6 失败处理）。
+        """
+        pairs = self._match(left, right)
+        matched_r: Dict[int, int] = {l_idx: r_idx for l_idx, r_idx, _ in pairs}
+        reports: List[RangeReport] = []
+        for l_idx, det_l in enumerate(left):
+            r_idx = matched_r.get(l_idx)
+            if r_idx is None:
+                reports.append(self._unmatched_report(det_l, frame_sequence, timestamp_s))
+                continue
+            report = self._build_report(det_l, right[r_idx], frame_sequence, timestamp_s)
+            reports.append(report)
+        return reports
+
+    # ---- 内部实现 ----
+
+    def _match(self, left: Sequence[Detection], right: Sequence[Detection]) -> List[Tuple[int, int, Detection]]:
+        """硬约束过滤 + 代价升序贪心一一配对（§6）。返回 (左下标, 右下标, 左框)。"""
+        cfg = self._cfg
+        d_lo, d_hi = cfg.disparity_bounds_px()
+        candidates: List[Tuple[int, int, Detection, float]] = []
+        for i, det_l in enumerate(left):
+            for j, det_r in enumerate(right):
+                if det_l.cls_id != det_r.cls_id:
+                    continue
+                dy = abs(det_l.cy - det_r.cy)
+                if dy > cfg.vertical_tau_px:
+                    continue
+                h_max = max(det_l.height, det_r.height)
+                if h_max <= 0.0:
+                    continue
+                dh_ratio = abs(det_l.height - det_r.height) / h_max
+                if dh_ratio > cfg.height_ratio_tau:
+                    continue
+                cost = dy / cfg.vertical_tau_px + dh_ratio / cfg.height_ratio_tau
+                if cost >= 1.0:
+                    continue
+                d_edge = det_l.x1 - det_r.x1
+                if not (d_lo <= d_edge <= d_hi):
+                    d_center = det_l.cx - det_r.cx
+                    if cfg.use_center_fallback and (d_lo <= d_center <= d_hi):
+                        self._warn("center_fallback", "左边缘视差越出窗口，回退中心视差（宽度差可能混入）")
+                    else:
+                        continue
+                same_track = det_l.track_id > 0 and det_l.track_id == det_r.track_id
+                candidates.append((0 if same_track else 1, cost, i, j, det_l))
+        candidates.sort(key=lambda c: (c[0], c[1]))
+        matched_l: set = set()
+        used_r: set = set()
+        pairs: List[Tuple[int, int, Detection]] = []
+        for _, _, i, j, det_l in candidates:
+            if i in matched_l or j in used_r:
+                continue
+            pairs.append((i, j, det_l))
+            matched_l.add(i)
+            used_r.add(j)
+        return pairs
+
+    def _build_report(self, det_l: Detection, det_r: Detection, seq: int, ts: Optional[float]) -> RangeReport:
+        cfg = self._cfg
+        d_edge = det_l.x1 - det_r.x1
+        d_lo, d_hi = cfg.disparity_bounds_px()
+        if d_lo <= d_edge <= d_hi:
+            d = d_edge
+            source = DISP_SOURCE_EDGE
+        else:
+            d = det_l.cx - det_r.cx
+            source = DISP_SOURCE_CENTER
+        z = cfg.fx_px * cfg.baseline_m / d
+        report = RangeReport(
+            frame_sequence=seq,
+            timestamp_s=ts,
+            class_id=det_l.cls_id,
+            track_id=det_l.track_id if det_l.track_id != 0 else det_r.track_id,
+            matched=True,
+            valid=False,  # §5.4 判定见下
+            forward_distance_m=z,
+            disparity_px=d,
+            disparity_source=source,
+            confidence=min(det_l.conf, det_r.conf),
+            centre_pixel_x_mid=(det_l.cx + det_r.cx) / 2.0,
+            centre_pixel_y_mid=det_l.cy,
+            ref_bbox=(det_l.x1, det_l.y1, det_l.x2, det_l.y2),
+        )
+        err = range_error_abs(z, cfg.fx_px, cfg.baseline_m, cfg.disp_noise_px)
+        if d >= cfg.disparity_min_px and err <= cfg.range_tol_m:
+            report.valid = True
+            z_out = self._filter_z(report.class_id, report.track_id, z)
+            report.forward_distance_m = z_out
+            report.error_est_m = range_error_abs(z_out, cfg.fx_px, cfg.baseline_m, cfg.disp_noise_px)
+        else:
+            report.error_est_m = err
+        self._fill_xyz(report, det_l.cx)
+        return report
+
+    def _fill_xyz(self, report: RangeReport, x_l_px: float) -> None:
+        """由 Z 与左目（参考目）框中心像素回算左目系 X, Y, R（§4.2）。
+
+        注意：方位源 centre_pixel_x_mid 属于基线中点虚拟相机（§11.3），
+        而 X 定义在左目坐标系，二者相差 B/2，不得混用。
+        """
+        cfg = self._cfg
+        z = report.forward_distance_m
+        x = (x_l_px - cfg.cx_px) * z / cfg.fx_px
+        y = (report.centre_pixel_y_mid - cfg.cy_px) * z / cfg.fy_px
+        report.lateral_m = x
+        report.vertical_m = y
+        report.slant_range_m = math.sqrt(x * x + y * y + z * z)
+
+    def _filter_z(self, cls_id: int, track_id: int, z: float) -> float:
+        """可选一阶低通 + 跳变重置（§7）。track_id 为 0（无轨迹）时不过滤。"""
+        cfg = self._cfg
+        if not cfg.filter_enabled or track_id == 0:
+            return z
+        key = (cls_id, track_id)
+        prev = self._z_state.get(key)
+        if prev is None:
+            self._z_state[key] = z
+            return z
+        if abs(z - prev) / prev > cfg.filter_jump_ratio:
+            self._z_state[key] = z  # 跳变：重置
+            return z
+        z_out = (1.0 - cfg.filter_alpha) * prev + cfg.filter_alpha * z
+        self._z_state[key] = z_out
+        return z_out
+
+    def _unmatched_report(self, det: Detection, seq: int, ts: Optional[float]) -> RangeReport:
+        return RangeReport(
+            frame_sequence=seq,
+            timestamp_s=ts,
+            class_id=det.cls_id,
+            track_id=det.track_id,
+            matched=False,
+            valid=False,
+            confidence=det.conf,
+            centre_pixel_x_mid=det.cx,
+            centre_pixel_y_mid=det.cy,
+            ref_bbox=(det.x1, det.y1, det.x2, det.y2),
+        )
+
+    def _warn(self, category: str, message: str) -> None:
+        """节流诊断（§7：失败日志须节流）。"""
+        now = time.monotonic()
+        last = self._warn_at.get(category)
+        if last is not None and now - last < self._cfg.warn_interval_s:
+            return
+        self._warn_at[category] = now
+        logger.warning("[%s] %s", category, message)