216 lines
8.7 KiB
Python
216 lines
8.7 KiB
Python
"""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)
|