新分支加入了标靶判断

2026-05-22 09:45:49 +08:00
parent c754dff4ad
commit 46508e4b31
17 changed files with 356 additions and 34 deletions
--- a/vision.py
+++ b/vision.py
@@ -10,6 +10,7 @@ import os
 import math
 import threading
 import queue
+import time
 from maix import image
 import config
 from logger_manager import logger_manager
@@ -531,6 +532,9 @@ def detect_circle_v3(frame, laser_point=None, img_cv=None):
    if img_cv is None:
        img_cv = image.image2cv(frame, False, False)
    logger = logger_manager.logger
+    _timing_on = bool(getattr(config, "VISION_TIMING_ENABLE", True))
+    _t0 = time.perf_counter() if _timing_on else None
+    _t1 = _t2 = _t3 = _t4 = _t5 = None
    from datetime import datetime
    logger.debug(f"[detect_circle_v3] begin {datetime.now()}")
    # -- 1. 缩图加速（与三角形路径保持一致）
@@ -554,6 +558,8 @@ def detect_circle_v3(frame, laser_point=None, img_cv=None):
    ellipse_params = None
    
    logger.debug(f"[detect_circle_v3] step 1 fin {datetime.now()}")
+    if _timing_on:
+        _t1 = time.perf_counter()

    # -- 2. HSV + 黄色掩码
    hsv = cv2.cvtColor(img_det, cv2.COLOR_RGB2HSV)
@@ -567,6 +573,9 @@ def detect_circle_v3(frame, laser_point=None, img_cv=None):
    mask_yellow = cv2.morphologyEx(mask_yellow, cv2.MORPH_CLOSE, kernel)

    logger.debug(f"[detect_circle_v3] step 2 fin {datetime.now()}")
+    if _timing_on:
+        _t2 = time.perf_counter()
+        _t3 = time.perf_counter()

    # -- 3. 红色掩码：在循环外只算一次
    mask_red = cv2.bitwise_or(
@@ -593,6 +602,9 @@ def detect_circle_v3(frame, laser_point=None, img_cv=None):
            red_candidates.append({"center": (int(xr), int(yr)), "radius": int(rr)})

    logger.debug(f"[detect_circle_v3] step 3 fin {datetime.now()}")
+    if _timing_on:
+        _t3 = time.perf_counter()
+        _t4 = time.perf_counter()

    # -- 4. 黄色轮廓循环（复用上面的红色候选列表）
    contours_yellow, _ = cv2.findContours(mask_yellow, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
@@ -642,6 +654,9 @@ def detect_circle_v3(frame, laser_point=None, img_cv=None):
            logger.debug("Debug -> 未找到匹配的红色圆圈，可能是误识别")

    logger.debug(f"[detect_circle_v3] step 4 fin {datetime.now()}")
+    if _timing_on:
+        _t4 = time.perf_counter()
+        _t5 = time.perf_counter()

    # -- 5. 选最佳目标，坐标还原到原始分辨率
    if valid_targets:
@@ -669,7 +684,20 @@ def detect_circle_v3(frame, laser_point=None, img_cv=None):
        ellipse_params = be
        best_radius1 = best_radius * 5
    result_img = image.cv2image(img_cv, False, False)
-    logger.debug(f"[detect_circle_v3] step 5 fin {datetime.now()}")
+    if _timing_on:
+        _t5 = time.perf_counter()
+        _t_all = (_t5 - _t0) * 1000
+        _ms1 = (_t1 - _t0) * 1000
+        _ms2 = (_t2 - _t1) * 1000
+        _ms3 = (_t3 - _t2) * 1000
+        _ms4 = (_t4 - _t3) * 1000
+        _ms5 = (_t5 - _t4) * 1000
+        logger.info(
+            f"[VISION timing] total={_t_all:.1f}ms  "
+            f"resize={_ms1:.1f}  hsv_yellow={_ms2:.1f}  "
+            f"red_mask={_ms3:.1f}  yellow_loop={_ms4:.1f}  "
+            f"select_cv2img={_ms5:.1f}"
+        )
    return result_img, best_center, best_radius, method, best_radius1, ellipse_params

 def estimate_distance(pixel_radius):
@@ -1010,3 +1038,63 @@ def detect_target(frame, laser_point=None):
        logger.debug("[VISION] 使用传统黄色靶心检测")
    return detect_circle_v3(frame, laser_point)

+
+def sample_target_rgb_at_physical_radius(frame, target_center, target_radius_px, radius_cm=None, angles_deg=None, patch_half_px=None, black_thresh=None, timing=False):
+    """
+    在物方半径位置采样 RGB，判断黑/白靶。
+    返回: dict {ok, is_black, mean_rgb, samples, black_ratio, elapsed_ms}
+    """
+    logger = logger_manager.logger
+    if target_center is None or target_radius_px is None:
+        return {"ok": False, "reason": "no_target", "is_black": None, "elapsed_ms": 0.0}
+
+    radius_cm = float(radius_cm if radius_cm is not None else getattr(config, "TRIANGLE_SAMPLE_RADIUS_CM", 15.0))
+    angles_deg = tuple(angles_deg if angles_deg is not None else getattr(config, "TRIANGLE_SAMPLE_ANGLES_DEG", (0, 90, 180, 270)))
+    patch_half_px = int(patch_half_px if patch_half_px is not None else getattr(config, "TRIANGLE_SAMPLE_PATCH_HALF_PX", 2))
+    black_thresh = float(black_thresh if black_thresh is not None else getattr(config, "TRIANGLE_SAMPLE_BLACK_THRESH", 30.0))
+    timing_on = bool(timing) and bool(getattr(config, "TRIANGLE_SAMPLE_TIMING_ENABLE", True))
+    t0 = time.perf_counter() if timing_on else None
+
+    try:
+        img_cv = image.image2cv(frame, False, False)
+        h, w = img_cv.shape[:2]
+        cx, cy = float(target_center[0]), float(target_center[1])
+        scale = float(target_radius_px) / max(radius_cm, 1e-6)
+        samples = []
+        black_count = 0
+        for ang in angles_deg:
+            rad = math.radians(float(ang))
+            sx = int(round(cx + math.cos(rad) * radius_cm * scale))
+            sy = int(round(cy + math.sin(rad) * radius_cm * scale))
+            x0 = max(0, sx - patch_half_px)
+            y0 = max(0, sy - patch_half_px)
+            x1 = min(w, sx + patch_half_px + 1)
+            y1 = min(h, sy + patch_half_px + 1)
+            if x1 <= x0 or y1 <= y0:
+                continue
+            patch = img_cv[y0:y1, x0:x1]
+            mean_rgb = patch.reshape(-1, 3).mean(axis=0)
+            is_black = bool(np.all(mean_rgb < black_thresh))
+            black_count += 1 if is_black else 0
+            samples.append({"angle": float(ang), "xy": (sx, sy), "mean_rgb": tuple(float(v) for v in mean_rgb), "is_black": is_black})
+        black_ratio = float(black_count) / float(len(samples) or 1)
+        out = {
+            "ok": len(samples) > 0,
+            "is_black": black_ratio >= 0.5,
+            "mean_rgb": tuple(float(v) for v in (np.mean([s["mean_rgb"] for s in samples], axis=0) if samples else (0, 0, 0))),
+            "samples": samples,
+            "black_ratio": black_ratio,
+            "elapsed_ms": (time.perf_counter() - t0) * 1000.0 if timing_on else 0.0,
+        }
+        if logger:
+            logger.info(
+                f"[TRI-SAMPLE] radius_cm={radius_cm:.1f} black_thresh={black_thresh:.1f} "
+                f"black_ratio={black_ratio:.2f} is_black={out['is_black']} "
+                f"elapsed_ms={out['elapsed_ms']:.1f} samples={len(samples)}"
+            )
+        return out
+    except Exception as e:
+        if logger:
+            logger.error(f"[TRI-SAMPLE] 采样失败: {e}")
+        return {"ok": False, "reason": str(e), "is_black": None, "elapsed_ms": 0.0}
+