精简仓库结构并增强任务链路：归档双目可选后端、新增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。
2026-10-21 14:48:41 +08:00
parent 53fdd3b13f
commit df91b1110e
159 changed files with 6121 additions and 1617 deletions
@@ -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