所有

test/test_algo_preview_live.py

@@ -0,0 +1,403 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+实时摄像头预览：叠加与射箭存图相同的算法标注（YOLO ROI、三角/圆心、激光十字等），默认不写盘。
+
+在 MaixCAM 上从项目根目录运行：
+  python3 test/test_algo_preview_live.py
+  python3 test/test_algo_preview_live.py --interval 1.5
+  python3 test/test_algo_preview_live.py --every-frame
+
+说明：
+  - 完整算法走 shoot_manager.analyze_shot（与 process_shot 一致，含 YOLO + 三角/圆心）。
+  - 画面标注对齐 process_shot 存图前绘制 + vision._draw_yolo_roi_on_rgb_numpy / 圆心存图线。
+  - 预览模式会关闭 Stage2 裁切 JPEG 落盘，避免写满 /root/phot。
+"""
+
+from __future__ import annotations
+
+import argparse
+import math
+import os
+import sys
+import threading
+import time
+
+_ROOT = os.path.dirname(os.path.dirname(os.path.abspath(__file__)))
+if _ROOT not in sys.path:
+    sys.path.insert(0, _ROOT)
+
+import cv2
+import numpy as np
+from maix import image, time as maix_time
+
+import config
+from camera_manager import camera_manager
+from laser_manager import laser_manager
+from shoot_manager import analyze_shot, preload_triangle_calib
+from target_roi_yolo import preload_yolo_detector
+from vision import _draw_yolo_roi_on_rgb_numpy
+
+
+def _copy_maix_frame(frame):
+    """相机下一帧可能复用缓冲区，异步分析前先复制。"""
+    img_cv = image.image2cv(frame, False, False)
+    return image.cv2image(np.ascontiguousarray(img_cv), False, False)
+
+
+def _patch_preview_config():
+    """预览不写调试 JPEG，避免刷屏占存储。"""
+    config.TRIANGLE_BLACK_YOLO_SAVE_ROI_CROP = False
+    config.TRIANGLE_SAVE_DEBUG_IMAGE = False
+
+
+def _annotate_like_saved_shot(analysis: dict):
+    """
+    将 analyze_shot 结果绘制成与 process_shot -> enqueue_save_shot 存盘前一致的 Maix 图。
+    """
+    result_img = analysis.get("result_img")
+    if result_img is None:
+        return None
+
+    center = analysis.get("center")
+    radius = analysis.get("radius")
+    method = analysis.get("method")
+    ellipse_params = analysis.get("ellipse_params")
+    laser_point = analysis.get("laser_point")
+    dx = analysis.get("dx")
+    dy = analysis.get("dy")
+    distance_m = analysis.get("distance_m")
+    offset_method = analysis.get("offset_method", "")
+    distance_method = analysis.get("distance_method", "")
+    tri_markers = analysis.get("tri_markers") or []
+    tri_markers_completed = analysis.get("tri_markers_completed") or []
+    tri_homography = analysis.get("tri_homography")
+    yolo_roi_xyxy = analysis.get("yolo_roi_xyxy")
+
+    if laser_point is None:
+        return result_img
+
+    x, y = laser_point
+    draw_yolo_roi = (
+        yolo_roi_xyxy is not None
+        and getattr(config, "TRIANGLE_YOLO_DRAW_ROI_ON_SHOT", True)
+    )
+
+    if tri_markers:
+        img_cv = image.image2cv(result_img, False, False).copy()
+
+        if draw_yolo_roi:
+            _draw_yolo_roi_on_rgb_numpy(img_cv, yolo_roi_xyxy)
+
+        for m in tri_markers:
+            corners = np.array(m["corners"], dtype=np.int32)
+            cv2.polylines(img_cv, [corners], True, (0, 255, 0), 2)
+            cx, cy = int(m["center"][0]), int(m["center"][1])
+            cv2.circle(img_cv, (cx, cy), 4, (0, 0, 255), -1)
+            cv2.putText(
+                img_cv,
+                f"T{m['id']}",
+                (cx - 18, cy - 12),
+                cv2.FONT_HERSHEY_SIMPLEX,
+                0.55,
+                (0, 255, 0),
+                1,
+            )
+
+        for m in tri_markers_completed:
+            if not m.get("is_virtual"):
+                continue
+            cx, cy = int(m["center"][0]), int(m["center"][1])
+            cv2.circle(img_cv, (cx, cy), 6, (255, 0, 255), 2)
+            cv2.putText(
+                img_cv,
+                f"VT{m['id']}",
+                (cx - 22, cy - 12),
+                cv2.FONT_HERSHEY_SIMPLEX,
+                0.55,
+                (255, 0, 255),
+                1,
+            )
+
+        if tri_homography is not None:
+            try:
+                H_inv = np.linalg.inv(tri_homography)
+                c_img = cv2.perspectiveTransform(
+                    np.array([[[0.0, 0.0]]], dtype=np.float32), H_inv
+                )[0][0]
+                ocx, ocy = int(c_img[0]), int(c_img[1])
+                cv2.circle(img_cv, (ocx, ocy), 5, (0, 0, 255), -1)
+                cv2.circle(img_cv, (ocx, ocy), 9, (0, 0, 255), 1)
+            except Exception:
+                pass
+
+        lines = []
+        if dx is not None and dy is not None:
+            r_cm = math.hypot(float(dx), float(dy))
+            lines.append(f"offset=({float(dx):.2f},{float(dy):.2f})cm |r|={r_cm:.2f}cm")
+        if distance_m is not None:
+            lines.append(f"cam_dist={float(distance_m):.2f}m ({distance_method})")
+        if method:
+            lines.append(f"method={method} ({offset_method})")
+        y0 = 22
+        for i, t in enumerate(lines):
+            cv2.putText(
+                img_cv,
+                t,
+                (10, y0 + i * 18),
+                cv2.FONT_HERSHEY_SIMPLEX,
+                0.5,
+                (0, 255, 0),
+                1,
+            )
+
+        out = image.cv2image(img_cv, False, False)
+    else:
+        img_cv = image.image2cv(result_img, False, False).copy()
+        if draw_yolo_roi:
+            _draw_yolo_roi_on_rgb_numpy(img_cv, yolo_roi_xyxy)
+
+        if center and radius:
+            cx, cy = center
+            if ellipse_params:
+                (ell_center, (width, height), angle) = ellipse_params
+                cx_ell, cy_ell = int(ell_center[0]), int(ell_center[1])
+                cv2.ellipse(
+                    img_cv,
+                    (cx_ell, cy_ell),
+                    (int(width / 2), int(height / 2)),
+                    angle,
+                    0,
+                    360,
+                    (0, 255, 0),
+                    2,
+                )
+                cv2.circle(img_cv, (cx_ell, cy_ell), 3, (255, 0, 0), -1)
+                minor_length = min(width, height) / 2
+                minor_angle = angle + 90 if width >= height else angle
+                minor_angle_rad = math.radians(minor_angle)
+                dx_minor = minor_length * math.cos(minor_angle_rad)
+                dy_minor = minor_length * math.sin(minor_angle_rad)
+                pt1 = (int(cx_ell - dx_minor), int(cy_ell - dy_minor))
+                pt2 = (int(cx_ell + dx_minor), int(cy_ell + dy_minor))
+                cv2.line(img_cv, pt1, pt2, (0, 0, 255), 2)
+            else:
+                cv2.circle(img_cv, (int(cx), int(cy)), int(radius), (0, 0, 255), 2)
+                cv2.circle(img_cv, (int(cx), int(cy)), 2, (0, 0, 255), -1)
+            cv2.line(img_cv, (int(x), int(y)), (int(cx), int(cy)), (255, 255, 0), 1)
+
+        lines = []
+        if dx is not None and dy is not None:
+            lines.append(f"offset=({float(dx):.2f},{float(dy):.2f})cm")
+        if distance_m is not None:
+            lines.append(f"dist={float(distance_m):.2f}m ({distance_method})")
+        if method:
+            lines.append(f"method={method}")
+        for i, t in enumerate(lines):
+            cv2.putText(
+                img_cv,
+                t,
+                (10, 22 + i * 18),
+                cv2.FONT_HERSHEY_SIMPLEX,
+                0.5,
+                (0, 255, 0),
+                1,
+            )
+        out = image.cv2image(img_cv, False, False)
+
+    lc = image.Color(config.LASER_COLOR[0], config.LASER_COLOR[1], config.LASER_COLOR[2])
+    out.draw_line(
+        int(x - config.LASER_LENGTH),
+        int(y),
+        int(x + config.LASER_LENGTH),
+        int(y),
+        lc,
+        config.LASER_THICKNESS,
+    )
+    out.draw_line(
+        int(x),
+        int(y - config.LASER_LENGTH),
+        int(x),
+        int(y + config.LASER_LENGTH),
+        lc,
+        config.LASER_THICKNESS,
+    )
+    out.draw_circle(int(x), int(y), 1, lc, config.LASER_THICKNESS)
+    return out
+
+
+class _AlgoWorker:
+    def __init__(self):
+        self._lock = threading.Lock()
+        self._busy = False
+        self._latest_preview = None
+        self._latest_meta = ""
+        self._last_ms = 0.0
+
+    @property
+    def busy(self):
+        with self._lock:
+            return self._busy
+
+    @property
+    def last_ms(self):
+        with self._lock:
+            return self._last_ms
+
+    def get_preview(self):
+        with self._lock:
+            return self._latest_preview, self._latest_meta
+
+    def run_async(self, frame):
+        with self._lock:
+            if self._busy:
+                return False
+            self._busy = True
+
+        def _job():
+            t0 = time.perf_counter()
+            meta = ""
+            preview = None
+            try:
+                analysis = analyze_shot(frame)
+                if not analysis.get("success"):
+                    reason = analysis.get("reason", "unknown")
+                    meta = f"fail:{reason}"
+                else:
+                    preview = _annotate_like_saved_shot(analysis)
+                    dx, dy = analysis.get("dx"), analysis.get("dy")
+                    method = analysis.get("method") or "?"
+                    if dx is not None and dy is not None:
+                        meta = f"ok {method} ({dx:.2f},{dy:.2f})cm"
+                    else:
+                        meta = f"ok {method} no_offset"
+            except Exception as e:
+                meta = f"err:{e}"
+            elapsed = (time.perf_counter() - t0) * 1000.0
+            with self._lock:
+                self._latest_preview = preview
+                self._latest_meta = f"{meta} {elapsed:.0f}ms"
+                self._last_ms = elapsed
+                self._busy = False
+
+        threading.Thread(target=_job, daemon=True).start()
+        return True
+
+
+def _draw_status(frame, lines, color=None):
+    if color is None:
+        color = image.COLOR_YELLOW
+    y = 4
+    for line in lines:
+        frame.draw_string(4, y, line, color=color)
+        y += 16
+
+
+def _save_preview_jpeg(maix_img, out_dir):
+    os.makedirs(out_dir, exist_ok=True)
+    fn = os.path.join(out_dir, f"preview_{int(time.time() * 1000)}.jpg")
+    maix_img.save(fn)
+    return fn
+
+
+def main():
+    parser = argparse.ArgumentParser(description="实时预览射箭算法存图效果")
+    parser.add_argument(
+        "--interval",
+        type=float,
+        default=2.0,
+        help="两次完整 analyze_shot 的最小间隔（秒）；--every-frame 时忽略",
+    )
+    parser.add_argument(
+        "--every-frame",
+        action="store_true",
+        help="每帧都触发算法（很慢，仅调试用）",
+    )
+    parser.add_argument(
+        "--width",
+        type=int,
+        default=getattr(config, "CAMERA_WIDTH", 640),
+    )
+    parser.add_argument(
+        "--height",
+        type=int,
+        default=getattr(config, "CAMERA_HEIGHT", 480),
+    )
+    parser.add_argument(
+        "--save-dir",
+        default=config.PHOTO_DIR,
+        help="按板子按键无；用 --save-every N 每 N 次成功分析存一张",
+    )
+    parser.add_argument(
+        "--save-every",
+        type=int,
+        default=0,
+        help="每成功分析 N 次自动存一张到 --save-dir（0=不自动存）",
+    )
+    args = parser.parse_args()
+
+    _patch_preview_config()
+    print("[INFO] 预览模式：已关闭 TRIANGLE_BLACK_YOLO_SAVE_ROI_CROP / TRIANGLE_SAVE_DEBUG_IMAGE")
+
+    laser_manager.load_laser_point()
+    preload_triangle_calib()
+    if getattr(config, "TRIANGLE_YOLO_PRELOAD_ON_BOOT", False) or getattr(
+        config, "TRIANGLE_BLACK_YOLO_PRELOAD_ON_BOOT", False
+    ):
+        print("[INFO] 预加载 YOLO …")
+        preload_yolo_detector()
+
+    camera_manager.init_camera(args.width, args.height)
+    camera_manager.init_display()
+    worker = _AlgoWorker()
+
+    interval_s = 0.0 if args.every_frame else max(0.3, float(args.interval))
+    last_trigger = 0.0
+    ok_count = 0
+    frame_idx = 0
+
+    print(
+        f"[INFO] 摄像头 {args.width}x{args.height} "
+        f"interval={'每帧' if args.every_frame else f'{interval_s}s'}"
+    )
+    print("[INFO] 退出：Ctrl+C")
+
+    try:
+        while True:
+            frame = camera_manager.read_frame()
+            frame_idx += 1
+            now = time.perf_counter()
+
+            due = args.every_frame or (now - last_trigger >= interval_s)
+            if due and not worker.busy:
+                last_trigger = now
+                worker.run_async(_copy_maix_frame(frame))
+
+            preview, meta = worker.get_preview()
+            if preview is not None:
+                show_img = preview
+                status = [f"#{frame_idx}", meta]
+                if args.save_every > 0 and meta.startswith("ok"):
+                    ok_count += 1
+                    if ok_count % args.save_every == 0:
+                        try:
+                            fn = _save_preview_jpeg(preview, args.save_dir)
+                            status.append(f"saved:{fn}")
+                        except Exception as e:
+                            status.append(f"save_err:{e}")
+            else:
+                show_img = frame
+                if worker.busy:
+                    status = [f"#{frame_idx}", "analyzing…"]
+                else:
+                    status = [f"#{frame_idx}", "waiting…"]
+
+            _draw_status(show_img, status)
+            camera_manager.show(show_img)
+            maix_time.sleep_ms(1)
+    except KeyboardInterrupt:
+        print("[INFO] 已退出")
+
+
+if __name__ == "__main__":
+    main()