双目测距方案重构与其他小修改

test.py

@@ -9,7 +9,7 @@ PX4 SITL Offboard 视觉跟踪集成测试 (MAVSDK-Python)
 适用于 PX4 1.17
 
 YOLO 检测未接入：VirtualTarget 用无人机真实位置/航向做针孔反投影生成
-检测框与激光测距（几何一致闭环），无人机会实际飞向该虚拟目标。
+检测框与双目目标距离（几何一致闭环），无人机会实际飞向该虚拟目标。
 """
 
 import asyncio
@@ -116,7 +116,7 @@ TRACK_NORMAL_SECS = 20.0               # 虚拟目标保持可见时长（之后
 TRACK_BRAKE_SECS = 5.0                 # 判丢后零速刹车时长
 TRACK_TIMEOUT_SECS = 120.0             # 跟踪段整体安全超时
 STATUS_EVERY_TICKS = 10                # 每 0.5 s 打印一行状态
-PLOT_TRAJECTORY = False                 # 跟踪结束后绘制无人机与虚拟目标轨迹
+PLOT_TRAJECTORY = True                 # 跟踪结束后绘制无人机与虚拟目标轨迹
 TRAJECTORY_SAVE_PATH = Path(__file__).resolve().parent / "trajectory_plot.png"
 
 # 虚拟目标：相对悬停锚点、按锚点机头朝向放置的机体系偏移
@@ -219,7 +219,7 @@ def plot_trajectories(
 
 
 class VirtualTarget:
-    """几何一致虚拟目标 —— YOLO 检测与激光雷达的替身。
+    """几何一致虚拟目标 —— YOLO 检测与双目测距的替身。
 
     目标初始位置：
         以起飞后的悬停锚点为参考，
@@ -387,7 +387,7 @@ class VirtualTarget:
     ):
         """返回：
 
-            (detections, radar_distance_m, associated)
+            (detections, target_distance_m, distance_valid)
 
         不可见时：
 
@@ -487,16 +487,10 @@ class VirtualTarget:
         )
 
         # ============================================================
-        # 9. 计算模拟激光雷达距离
+        # 9. 模拟双目目标距离（前向分量 Z，与检测框几何一致）
         # ============================================================
 
-        slant = math.sqrt(
-            fwd * fwd
-            + right * right
-            + rel_d * rel_d
-        )
-
-        return [box], slant, True
+        return [box], fwd, True
 
 
 class TelemetryHub:
@@ -616,7 +610,7 @@ async def run_tracking(drone, anchor_ned: tuple[float, float, float],
                 target.set_visible(False)
                 print("-- 模拟目标消失，追踪器进入 Coast 预测")
 
-            detections, radar_m, associated = target.observe(
+            detections, distance_m, distance_valid = target.observe(
                 hub.north_m, hub.east_m, hub.down_m, hub.yaw_rad)
 
             # 记录本拍位置（目标位置在 observe 内已 update）
@@ -628,9 +622,9 @@ async def run_tracking(drone, anchor_ned: tuple[float, float, float],
             snap = ControlSnapshot(
                 track=track_result_to_state(result),
                 track_time_ms=now,                 # 每拍取样即打戳（视觉始终新鲜）
-                radar_distance_m=radar_m,
-                associated_to_target=associated,
-                radar_time_ms=now if associated else 0,
+                target_distance_m=distance_m,
+                distance_valid=distance_valid,
+                distance_time_ms=now,
                 attitude_present=True,
                 attitude_time_ms=hub.attitude_time_ms,
                 yaw_rad=hub.yaw_rad,
@@ -644,7 +638,7 @@ async def run_tracking(drone, anchor_ned: tuple[float, float, float],
                 print(f"[跟踪 t={t_s:5.1f}s] "
                       f"检测={len(detections)} "
                       f"追踪[trk={result.tracked} pred={result.is_predicted} "
-                      f"lost={result.lost_frames}] 雷达={radar_m:5.1f}m | "
+                      f"lost={result.lost_frames}] 距离={distance_m:5.1f}m | "
                       f"[{out.mode.name}] vx_body={out.vx_body_mps:+.2f} "
                       f"vz={out.vz_mps:+.2f} yaw_rate={out.yaw_rate_dps:+.2f}°/s "
                       f"coast={out.coast_active}")