精简仓库结构并增强任务链路：归档双目可选后端、新增LOS判据与自定义返航
归档 legacy/：stereo_yolo/stereo_ranger/target_tracker 三包 + 175 条测试 + demo/配置/权重/三份规格；运行链路不再依赖，默认 pytest 446 条，归档测试按需运行。 目标有效性第二判据改为 LOS 运动学一致性（pod_bridge/los_consistency.py），删除 require_vision；pod_locked() 叠加该判据，状态机随之后退重搜。 自定义返航（task_runner/return_home.py）：保持转场高度回起飞点 → 恢复初始航向 → 垂直降落，不再爬 RTL_RETURN_ALT；未起飞/无起飞点/超时/异常仍退回 PX4 原生 RTL。 任务链安全兜底：主循环异常→紧急返航、stop 幂等、landing_stop 保留遥测、in_air 流复用+超时、解锁/起飞与吊舱识别仅执行一次、rtl_timeout 上限。 入口共享件拆到 utils/sitl_support.py；pod_sitl_test.py→l1_sitl_test.py；删除旧架构 test.py；相机脚本移入 tools/。 新增 .gitignore 并剔除历史误跟踪的 59 个 .pyc；requirements 按实际依赖重写；文档全面对齐，新增 legacy/README.md 与 tools/README.md。
This commit is contained in:
keita
@@ -0,0 +1,29 @@
|
||||
# target_tracker（Python 版）
|
||||
|
||||
多目标追踪过滤器：8 维恒速卡尔曼滤波 + 贪婪关联 + 主目标锁定选举。
|
||||
移植自 `reference-c++/target_tracker/`（规格：docs/视觉追踪技术路线.md）。
|
||||
|
||||
## 接口清单
|
||||
|
||||
| 名称 | 说明 |
|
||||
| --- | --- |
|
||||
| `Detection(x1, y1, x2, y2, conf, cls_id=0)` | 单帧检测框输入 |
|
||||
| `FrameShape(height, width)` | 画面尺寸（仅 Coast 兜底选举取画面中心用） |
|
||||
| `TrackResult` | 每帧输出九字段：tracked / primary_id / center / box / confidence / is_predicted / lost_frames / n_active / raw |
|
||||
| `TrackerConfig(max_lost_frames=8, max_association_dist=200.0, min_hits=3, dist_weight=0.5, iou_weight=0.5)` | 追踪器配置 |
|
||||
| `TargetTracker(config=None)` | 追踪器主类 |
|
||||
| `.update(detections, frame_shape=None) -> TrackResult` | 单帧主流程（§7 六步） |
|
||||
| `.last_result() -> TrackResult` | 跨线程读取最近一帧结果（加锁拷贝） |
|
||||
| `.reset()` / `.active_count()` / `.active_tracks()` / `.get_config()` | 生命周期与查询 |
|
||||
| `KalmanBoxFilter` / `BoxCXCYWH` | 单目标滤波器（可独立使用：initiate / predict / update） |
|
||||
| `Tracklet` / `BoxXYXY` / `to_center_size` / `to_xyxy` / `compute_iou` | 轨道与几何工具 |
|
||||
|
||||
## 与 C++ 版的差异
|
||||
|
||||
- **依赖**：矩阵运算用 numpy（C++ 手写定长运算避免 Eigen）；关联/选举路径用纯标量，与 C++ 逐位同序。
|
||||
- **Optional 语义**：C++ `std::optional<T>` → Python `None`；`State()` → `state` 属性（返回拷贝）。
|
||||
- **线程模型**：与 C++ 一致——仅 `last_result()` 加 `threading.Lock`，`update()` 单线程调用。
|
||||
- **字段命名**：成员函数蛇形命名（`IsDead` → `is_dead` 属性，`MarkMiss` → `mark_miss`）。
|
||||
- 行为语义（死亡判定 `lost_count > max_lost` 严格大于、best_cost=1.0 严格小于、
|
||||
ID 严格递增不复用、Q 用状态原值 / R 用 abs×2 的刻意不对称、S 求逆前 1e-9 正则化）
|
||||
与 C++ 完全一致，由 27 条移植测试锁定。
|
||||
@@ -0,0 +1,15 @@
|
||||
"""target_tracker —— 轻量级多目标追踪过滤器（C++ 版 reference-c++/target_tracker 的 Python 移植）。
|
||||
|
||||
纯算法包，零第三方运行依赖之外的约束：仅依赖 numpy，不依赖 visual_track_control。
|
||||
"""
|
||||
|
||||
from .kalman_box_filter import STD_POS, STD_VEL, INI_POS, INI_VEL, R_SCALE, BoxCXCYWH, KalmanBoxFilter
|
||||
from .target_tracker import Detection, FrameShape, TargetTracker, TrackerConfig, TrackResult
|
||||
from .tracklet import BoxXYXY, Tracklet, compute_iou, to_center_size, to_xyxy
|
||||
|
||||
__all__ = [
|
||||
"STD_POS", "STD_VEL", "INI_POS", "INI_VEL", "R_SCALE",
|
||||
"BoxCXCYWH", "BoxXYXY", "KalmanBoxFilter", "Tracklet",
|
||||
"Detection", "FrameShape", "TargetTracker", "TrackerConfig", "TrackResult",
|
||||
"compute_iou", "to_center_size", "to_xyxy",
|
||||
]
|
||||
@@ -0,0 +1,111 @@
|
||||
"""单目标 8 维恒定速度卡尔曼滤波器。
|
||||
|
||||
对应 C++ 参考实现 reference-c++/target_tracker（规格：docs/视觉追踪技术路线.md §4）：
|
||||
状态 x[8] = [cx, cy, w, h, vx, vy, vw, vh]，观测 z[4] = [cx, cy, w, h]，dt = 1 帧。
|
||||
矩阵运算用 numpy 表达（C++ 版为避免 Eigen 手写定长运算，此处等价）。
|
||||
"""
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
|
||||
# §4.2 / §8.2 内部常量（经验值，针对 640×480 画面调优）
|
||||
STD_POS = 1.0 / 50.0 # 位置噪声比例系数
|
||||
STD_VEL = 1.0 / 200.0 # 速度噪声比例系数
|
||||
INI_POS = 2.0 # 初始位置协方差
|
||||
INI_VEL = 100.0 # 初始速度协方差
|
||||
R_SCALE = 2.0 # R 额外缩放因子（增强平滑）
|
||||
_S_REG = 1e-9 # S 求逆前的对角正则化（§9.3）
|
||||
|
||||
# 状态转移矩阵 F（§4.1）：I4 + 右上 I4
|
||||
_F = np.eye(8, dtype=float)
|
||||
_F[0:4, 4:8] = np.eye(4)
|
||||
|
||||
# 观测矩阵 H（§4.1）：[I4 | 0]，直接观测位置与尺寸、不观测速度
|
||||
_H_COLS = slice(0, 4)
|
||||
|
||||
|
||||
@dataclass
|
||||
class BoxCXCYWH:
|
||||
"""轴对齐边界框（中心点 + 宽高表示，卡尔曼观测空间）。"""
|
||||
|
||||
cx: float = 0.0
|
||||
cy: float = 0.0
|
||||
w: float = 0.0
|
||||
h: float = 0.0
|
||||
|
||||
|
||||
class KalmanBoxFilter:
|
||||
def __init__(self) -> None:
|
||||
self._x = np.zeros(8, dtype=float)
|
||||
self._p = np.zeros((8, 8), dtype=float)
|
||||
self._initialized = False
|
||||
|
||||
@property
|
||||
def initialized(self) -> bool:
|
||||
return self._initialized
|
||||
|
||||
@property
|
||||
def state(self) -> np.ndarray:
|
||||
"""状态向量 [cx, cy, w, h, vx, vy, vw, vh]（拷贝）。"""
|
||||
return self._x.copy()
|
||||
|
||||
def initiate(self, z: BoxCXCYWH) -> None:
|
||||
# §4.3：速度置零；P = diag(2,2,2,2,100,100,100,100)
|
||||
self._x = np.array([z.cx, z.cy, z.w, z.h, 0.0, 0.0, 0.0, 0.0])
|
||||
self._p = np.diag([INI_POS] * 4 + [INI_VEL] * 4)
|
||||
self._initialized = True
|
||||
|
||||
def predict(self) -> BoxCXCYWH | None:
|
||||
# §4.4：未初始化返回 None
|
||||
if not self._initialized:
|
||||
return None
|
||||
|
||||
# §4.2 动态过程噪声：Q = diag(σ_pos², σ_vel²)，σ 按状态中 w/h 缩放。
|
||||
# Q 直接使用状态值、不做 abs，与 R 的 abs 处理刻意不同（规格 §4.2 注记）。
|
||||
w = self._x[2]
|
||||
h = self._x[3]
|
||||
std_pos = np.array([w * STD_POS, h * STD_POS, w * STD_POS, h * STD_POS])
|
||||
std_vel = np.array([w * STD_VEL, h * STD_VEL, w * STD_VEL, h * STD_VEL])
|
||||
q = np.zeros((8, 8), dtype=float)
|
||||
q[np.arange(4), np.arange(4)] = std_pos * std_pos
|
||||
q[np.arange(4) + 4, np.arange(4) + 4] = std_vel * std_vel
|
||||
|
||||
# §4.4：x ← F·x；P ← F·P·Fᵀ + Q
|
||||
self._x = _F @ self._x
|
||||
self._p = _F @ self._p @ _F.T + q
|
||||
return BoxCXCYWH(float(self._x[0]), float(self._x[1]),
|
||||
float(self._x[2]), float(self._x[3]))
|
||||
|
||||
def update(self, z: BoxCXCYWH) -> None:
|
||||
# §4.5：未初始化直接返回
|
||||
if not self._initialized:
|
||||
return
|
||||
|
||||
# §4.2 动态观测噪声：R 用 abs(w)/abs(h) 缩放并额外乘 R_SCALE
|
||||
w = abs(self._x[2])
|
||||
h = abs(self._x[3])
|
||||
rw = R_SCALE * (w * STD_POS) * (w * STD_POS)
|
||||
rh = R_SCALE * (h * STD_POS) * (h * STD_POS)
|
||||
r = np.diag([rw, rh, rw, rh])
|
||||
|
||||
# §4.5：S = H·P·Hᵀ + R。H = [I4|0]，故 H·P·Hᵀ 即 P 左上 4×4 子块
|
||||
s = self._p[:4, :4] + r
|
||||
try:
|
||||
s_inv = np.linalg.inv(s + _S_REG * np.eye(4))
|
||||
except np.linalg.LinAlgError:
|
||||
return # 求逆失败：防御性跳过本次更新（C++ 版同样跳过）
|
||||
|
||||
# §4.5：K = P·Hᵀ·S⁻¹，P·Hᵀ 即 P 的前 4 列（8×4）
|
||||
k = self._p[:, _H_COLS] @ s_inv
|
||||
|
||||
# 残差 y = z − H·x（H·x 即状态前 4 维）
|
||||
y = np.array([z.cx, z.cy, z.w, z.h]) - self._x[:4]
|
||||
|
||||
# x ← x + K·y
|
||||
self._x = self._x + k @ y
|
||||
|
||||
# P ← (I8 − K·H)·P：K·H 是 K 的前 4 列铺在前 4 列上
|
||||
kh = np.zeros((8, 8), dtype=float)
|
||||
kh[:, :4] = k
|
||||
self._p = (np.eye(8) - kh) @ self._p
|
||||
@@ -0,0 +1,269 @@
|
||||
"""多目标追踪过滤器（TargetTracker）。
|
||||
|
||||
对应 C++ 参考实现 reference-c++/target_tracker/src/target_tracker.cpp
|
||||
（规格：docs/视觉追踪技术路线.md §5 关联 / §6 生命周期与锁定 / §7 六步主流程）。
|
||||
|
||||
线程模型（§9.5）：update() 由单线程（推理主循环）调用；仅对 last_result
|
||||
缓存加锁，last_result() 返回受保护拷贝。
|
||||
"""
|
||||
|
||||
import math
|
||||
import threading
|
||||
from dataclasses import dataclass, field
|
||||
from typing import Optional, Sequence
|
||||
|
||||
from .kalman_box_filter import BoxCXCYWH
|
||||
from .tracklet import BoxXYXY, Tracklet, compute_iou, to_center_size
|
||||
|
||||
|
||||
@dataclass
|
||||
class Detection:
|
||||
"""单个检测框（§2.2 输入元素：[x1,y1,x2,y2,conf,cls_id]）。"""
|
||||
|
||||
x1: float = 0.0
|
||||
y1: float = 0.0
|
||||
x2: float = 0.0
|
||||
y2: float = 0.0
|
||||
conf: float = 0.0
|
||||
cls_id: int = 0
|
||||
|
||||
|
||||
@dataclass
|
||||
class FrameShape:
|
||||
"""画面尺寸（height, width）：仅 Coast 兜底选举时用于取画面中心。"""
|
||||
|
||||
height: int = 0
|
||||
width: int = 0
|
||||
|
||||
|
||||
@dataclass
|
||||
class TrackResult:
|
||||
"""每帧追踪输出（§2.3 九字段）。"""
|
||||
|
||||
tracked: bool = False
|
||||
primary_id: Optional[int] = None
|
||||
center: tuple[float, float] = (0.0, 0.0)
|
||||
box: Optional[BoxXYXY] = None
|
||||
confidence: float = 0.0
|
||||
is_predicted: bool = False
|
||||
lost_frames: int = 0
|
||||
n_active: int = 0
|
||||
raw: Optional[BoxXYXY] = None
|
||||
|
||||
|
||||
@dataclass
|
||||
class TrackerConfig:
|
||||
"""追踪器配置参数（§8.1 默认值取自规格）。"""
|
||||
|
||||
max_lost_frames: int = 8
|
||||
max_association_dist: float = 200.0
|
||||
min_hits: int = 3
|
||||
dist_weight: float = 0.5
|
||||
iou_weight: float = 0.5
|
||||
|
||||
|
||||
@dataclass
|
||||
class _AssociationResult:
|
||||
matches: list[tuple[int, int]] = field(default_factory=list)
|
||||
unmatched_track_ids: list[int] = field(default_factory=list)
|
||||
unmatched_det_indices: list[int] = field(default_factory=list)
|
||||
|
||||
|
||||
class TargetTracker:
|
||||
def __init__(self, config: Optional[TrackerConfig] = None) -> None:
|
||||
self._config = config if config is not None else TrackerConfig()
|
||||
self._next_id = 0
|
||||
self._tracks: dict[int, Tracklet] = {}
|
||||
self._frame_count = 0
|
||||
self._locked_primary_id: Optional[int] = None
|
||||
self._last_result = TrackResult()
|
||||
self._last_result_mutex = threading.Lock()
|
||||
|
||||
@property
|
||||
def config(self) -> TrackerConfig:
|
||||
return self._config
|
||||
|
||||
def get_config(self) -> TrackerConfig:
|
||||
return self._config
|
||||
|
||||
def update(self, detections: Sequence[Detection],
|
||||
frame_shape: Optional[FrameShape] = None) -> TrackResult:
|
||||
# §7 六步主流程
|
||||
self._frame_count += 1
|
||||
|
||||
# 入口防御：过滤坐标或置信度含 NaN/Inf 的检测（非有限值一旦进入
|
||||
# 卡尔曼状态将永久污染轨道并传导到主目标输出）
|
||||
valid_detections = [
|
||||
det for det in detections
|
||||
if all(math.isfinite(v) for v in (det.x1, det.y1, det.x2, det.y2, det.conf))
|
||||
]
|
||||
|
||||
# 步骤 1：所有存活轨道先行预测（推进一帧）
|
||||
for track in self._tracks.values():
|
||||
track.predict()
|
||||
|
||||
# 步骤 2：贪婪关联（无检测时全部轨道进入 Coast 计数）
|
||||
if valid_detections:
|
||||
assoc = self._associate_detections(valid_detections)
|
||||
else:
|
||||
assoc = _AssociationResult(unmatched_track_ids=list(self._tracks.keys()))
|
||||
|
||||
# 步骤 3：用匹配到的检测更新轨道
|
||||
for track_id, det_idx in assoc.matches:
|
||||
det = valid_detections[det_idx]
|
||||
raw_box = BoxXYXY(det.x1, det.y1, det.x2, det.y2)
|
||||
self._tracks[track_id].update(raw_box, det.conf, det.cls_id)
|
||||
|
||||
# 步骤 4：未匹配轨道计丢失，死亡者回收（锁定唯一解除途径之一）
|
||||
for track_id in assoc.unmatched_track_ids:
|
||||
track = self._tracks.get(track_id)
|
||||
if track is None:
|
||||
continue
|
||||
track.mark_miss()
|
||||
if track.is_dead:
|
||||
if self._locked_primary_id == track_id:
|
||||
self._locked_primary_id = None
|
||||
del self._tracks[track_id]
|
||||
|
||||
# 步骤 5：未匹配检测 → 创建新轨道（next_id 从 0 递增永不复用）
|
||||
for det_idx in assoc.unmatched_det_indices:
|
||||
det = valid_detections[det_idx]
|
||||
measurement = to_center_size(BoxXYXY(det.x1, det.y1, det.x2, det.y2))
|
||||
self._tracks[self._next_id] = Tracklet(
|
||||
self._next_id, measurement, det.conf, det.cls_id,
|
||||
self._config.max_lost_frames)
|
||||
self._next_id += 1
|
||||
|
||||
# 步骤 6：选取/延续主目标，组装输出并缓存
|
||||
result = self._select_primary(frame_shape)
|
||||
with self._last_result_mutex:
|
||||
self._last_result = result
|
||||
return result
|
||||
|
||||
def last_result(self) -> TrackResult:
|
||||
with self._last_result_mutex:
|
||||
return self._last_result
|
||||
|
||||
def reset(self) -> None:
|
||||
# §2.1：清空所有轨道、ID 计数器、锁定状态与缓存结果（next_id 归零）
|
||||
self._tracks.clear()
|
||||
self._next_id = 0
|
||||
self._frame_count = 0
|
||||
self._locked_primary_id = None
|
||||
with self._last_result_mutex:
|
||||
self._last_result = TrackResult()
|
||||
|
||||
def active_count(self) -> int:
|
||||
return len(self._tracks)
|
||||
|
||||
def active_tracks(self) -> dict[int, Tracklet]:
|
||||
return dict(self._tracks)
|
||||
|
||||
def _associate_detections(self, detections: Sequence[Detection]) -> _AssociationResult:
|
||||
result = _AssociationResult()
|
||||
|
||||
# §5.3：按轨道 confidence 降序（平局按 ID 升序保证确定性）。
|
||||
# 非有限置信度按 −∞ 处理（排最后），避免 NaN 破坏全序。
|
||||
def sort_key(track_id: int) -> tuple[float, int]:
|
||||
raw = self._tracks[track_id].confidence
|
||||
conf = raw if math.isfinite(raw) else -math.inf
|
||||
return (-conf, track_id)
|
||||
|
||||
sorted_ids = sorted(self._tracks.keys(), key=sort_key)
|
||||
det_assigned = [False] * len(detections)
|
||||
|
||||
for track_id in sorted_ids:
|
||||
track = self._tracks[track_id]
|
||||
# §5.3：最优代价初始 1.0，仅 cost 严格小于才匹配（cost ≥ 1.0 判为不可匹配）
|
||||
best_cost = 1.0
|
||||
best_det = -1
|
||||
for d, det in enumerate(detections):
|
||||
if det_assigned[d]:
|
||||
continue
|
||||
det_cx = (det.x1 + det.x2) / 2.0
|
||||
det_cy = (det.y1 + det.y2) / 2.0
|
||||
dist = math.hypot(track.last_center[0] - det_cx,
|
||||
track.last_center[1] - det_cy)
|
||||
norm_dist = dist / max(self._config.max_association_dist, 1e-3)
|
||||
iou = (compute_iou(track.last_box,
|
||||
BoxXYXY(det.x1, det.y1, det.x2, det.y2))
|
||||
if track.last_box is not None else 0.0)
|
||||
cost = (self._config.dist_weight * norm_dist +
|
||||
self._config.iou_weight * (1.0 - iou))
|
||||
if cost < best_cost:
|
||||
best_cost = cost
|
||||
best_det = d
|
||||
if best_det >= 0:
|
||||
result.matches.append((track_id, best_det))
|
||||
det_assigned[best_det] = True
|
||||
else:
|
||||
result.unmatched_track_ids.append(track_id)
|
||||
|
||||
result.unmatched_det_indices = [
|
||||
d for d, assigned in enumerate(det_assigned) if not assigned
|
||||
]
|
||||
return result
|
||||
|
||||
def _select_primary(self, frame_shape: Optional[FrameShape]) -> TrackResult:
|
||||
result = TrackResult(n_active=len(self._tracks))
|
||||
|
||||
# §6.3：轨道表为空 → 清除锁定，返回 tracked=false
|
||||
if not self._tracks:
|
||||
self._locked_primary_id = None
|
||||
return result
|
||||
|
||||
# §6.3 ① 锁定延续：锁定轨道存活则直接输出（Coast 中照样输出，
|
||||
# 不被高置信度目标抢占）
|
||||
chosen: Optional[Tracklet] = None
|
||||
is_predicted = False
|
||||
if self._locked_primary_id is not None:
|
||||
track = self._tracks.get(self._locked_primary_id)
|
||||
if track is not None:
|
||||
chosen = track
|
||||
is_predicted = track.is_coasting
|
||||
|
||||
# §6.3 ② 重新选举（无锁定 / 锁定目标已死亡被回收）
|
||||
if chosen is None:
|
||||
measured = [t for t in self._tracks.values() if not t.is_coasting]
|
||||
coasting = [t for t in self._tracks.values() if t.is_coasting]
|
||||
|
||||
if measured:
|
||||
# confirmed ← measured 中 hits ≥ min_hits 者；为空则退回 measured
|
||||
# （min_hits 只影响候选池优先级，不阻止输出）
|
||||
pool = [t for t in measured if t.hits >= self._config.min_hits]
|
||||
if not pool:
|
||||
pool = measured
|
||||
chosen = pool[0]
|
||||
for t in pool:
|
||||
if t.confidence > chosen.confidence:
|
||||
chosen = t
|
||||
is_predicted = False
|
||||
elif coasting:
|
||||
# 无实测轨道：选离画面中心最近者（缺省按 640×480 中心 (320,240)）
|
||||
center_x = frame_shape.width / 2.0 if frame_shape and frame_shape.width > 0 else 320.0
|
||||
center_y = frame_shape.height / 2.0 if frame_shape and frame_shape.height > 0 else 240.0
|
||||
best_dist_sq = math.inf
|
||||
for t in coasting:
|
||||
dx = t.last_center[0] - center_x
|
||||
dy = t.last_center[1] - center_y
|
||||
dist_sq = dx * dx + dy * dy
|
||||
if dist_sq < best_dist_sq:
|
||||
best_dist_sq = dist_sq
|
||||
chosen = t
|
||||
is_predicted = True
|
||||
else:
|
||||
# 理论上不可达（轨道非空则 measured/coasting 必居其一）；防御清锁
|
||||
self._locked_primary_id = None
|
||||
return result
|
||||
self._locked_primary_id = chosen.track_id # 选举后锁定
|
||||
|
||||
# 组装输出（§2.3 九字段）；raw 仅实测时有值，Coast 时为空
|
||||
result.tracked = True
|
||||
result.primary_id = chosen.track_id
|
||||
result.center = chosen.last_center
|
||||
result.box = chosen.last_box
|
||||
result.confidence = chosen.confidence
|
||||
result.is_predicted = is_predicted
|
||||
result.lost_frames = chosen.lost_count
|
||||
result.raw = None if is_predicted else chosen.last_raw_box
|
||||
return result
|
||||
@@ -0,0 +1,110 @@
|
||||
"""轨道（Tracklet）与基础几何工具。
|
||||
|
||||
对应 C++ 参考实现 reference-c++/target_tracker/include/target_tracker/tracklet.h
|
||||
（规格：docs/视觉追踪技术路线.md §3.2 / §5.2 / §7）。
|
||||
"""
|
||||
|
||||
from dataclasses import dataclass
|
||||
from typing import Optional
|
||||
|
||||
from .kalman_box_filter import BoxCXCYWH, KalmanBoxFilter
|
||||
|
||||
|
||||
@dataclass
|
||||
class BoxXYXY:
|
||||
"""轴对齐边界框（像素坐标，左上/右下角表示）。"""
|
||||
|
||||
x1: float = 0.0
|
||||
y1: float = 0.0
|
||||
x2: float = 0.0
|
||||
y2: float = 0.0
|
||||
|
||||
|
||||
def to_center_size(box: BoxXYXY) -> BoxCXCYWH:
|
||||
# §7 TRACKLET_UPDATE：cx=(x1+x2)/2；cy=(y1+y2)/2；w=x2−x1；h=y2−y1
|
||||
return BoxCXCYWH((box.x1 + box.x2) / 2.0, (box.y1 + box.y2) / 2.0,
|
||||
box.x2 - box.x1, box.y2 - box.y1)
|
||||
|
||||
|
||||
def to_xyxy(box: BoxCXCYWH) -> BoxXYXY:
|
||||
# §7 TRACKLET_PREDICT 的 center_size 逆变换
|
||||
return BoxXYXY(box.cx - box.w / 2.0, box.cy - box.h / 2.0,
|
||||
box.cx + box.w / 2.0, box.cy + box.h / 2.0)
|
||||
|
||||
|
||||
def compute_iou(a: BoxXYXY, b: BoxXYXY) -> float:
|
||||
"""标准轴对齐 IoU（§5.2）：任一框为空、交集 ≤ 0 或并集 ≤ 0 时返回 0。"""
|
||||
area_a = (a.x2 - a.x1) * (a.y2 - a.y1)
|
||||
area_b = (b.x2 - b.x1) * (b.y2 - b.y1)
|
||||
if area_a <= 0.0 or area_b <= 0.0:
|
||||
return 0.0
|
||||
|
||||
inter_w = min(a.x2, b.x2) - max(a.x1, b.x1)
|
||||
inter_h = min(a.y2, b.y2) - max(a.y1, b.y1)
|
||||
if inter_w <= 0.0 or inter_h <= 0.0:
|
||||
return 0.0
|
||||
|
||||
inter_area = inter_w * inter_h
|
||||
union_area = area_a + area_b - inter_area
|
||||
if union_area <= 0.0:
|
||||
return 0.0
|
||||
return inter_area / union_area
|
||||
|
||||
|
||||
class Tracklet:
|
||||
"""单条轨道：生命周期计数 + 辅助状态 + 卡尔曼滤波器（§3.2）。
|
||||
|
||||
构造即创建轨道（§7 步骤 5）：用首次观测初始化滤波器，age=1、hits=1、
|
||||
lost_count=0，last_center / last_box 立即由观测重建（创建当帧即可参与输出）。
|
||||
"""
|
||||
|
||||
def __init__(self, track_id: int, measurement: BoxCXCYWH, conf: float,
|
||||
cls_id: int, max_lost_frames: int) -> None:
|
||||
self.track_id = track_id
|
||||
self.kf = KalmanBoxFilter()
|
||||
self.lost_count = 0
|
||||
self.max_lost = max_lost_frames
|
||||
self.confidence = conf
|
||||
self.cls_id = cls_id
|
||||
self.age = 1
|
||||
self.hits = 1
|
||||
self.last_center: tuple[float, float] = (measurement.cx, measurement.cy)
|
||||
self.last_box: Optional[BoxXYXY] = to_xyxy(measurement)
|
||||
# 创建即源于实测检测：同步记录原始检测框（Coast 期间保留不更新）
|
||||
self.last_raw_box: Optional[BoxXYXY] = self.last_box
|
||||
self.kf.initiate(measurement)
|
||||
|
||||
@property
|
||||
def is_dead(self) -> bool:
|
||||
# §6.1 注记 2：lost_count > max_lost（严格大于）
|
||||
return self.lost_count > self.max_lost
|
||||
|
||||
@property
|
||||
def is_coasting(self) -> bool:
|
||||
# §3.2：lost_count > 0 即处于纯预测状态
|
||||
return self.lost_count > 0
|
||||
|
||||
def predict(self) -> None:
|
||||
# §7 TRACKLET_PREDICT：预测成功时更新 last_center / last_box 且 age+1
|
||||
pred = self.kf.predict()
|
||||
if pred is not None:
|
||||
self.last_center = (pred.cx, pred.cy)
|
||||
self.last_box = to_xyxy(pred)
|
||||
self.age += 1
|
||||
|
||||
def update(self, box: BoxXYXY, conf: float, cls_id: int) -> None:
|
||||
# §7 TRACKLET_UPDATE：框 → [cx,cy,w,h] → kf.update
|
||||
self.kf.update(to_center_size(box))
|
||||
self.last_center = ((box.x1 + box.x2) / 2.0, (box.y1 + box.y2) / 2.0)
|
||||
self.last_box = box
|
||||
self.last_raw_box = box
|
||||
self.confidence = conf
|
||||
self.cls_id = cls_id
|
||||
self.lost_count = 0
|
||||
self.hits += 1
|
||||
self.age += 1
|
||||
|
||||
def mark_miss(self) -> None:
|
||||
# §7 步骤 4：未匹配计丢失
|
||||
self.lost_count += 1
|
||||
self.age += 1
|
||||
Reference in New Issue
Block a user