feat: 根据激光找出图片中心点坐标

test/test_laser_center_point.py

@@ -0,0 +1,541 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+激光中心点检测单元测试（单文件，无项目依赖）
+直接使用 maix 标准库，实现红色激光点坐标检测
+
+运行方式：
+  python3 test/test_laser_center_point.py
+
+Ctrl+C 退出，按 s 保存截图
+"""
+
+from maix import camera, display, image, time, app, uart, pinmap
+import os
+import struct
+import select
+
+_USE_CV = False
+try:
+    import cv2
+    import numpy as np
+    _USE_CV = True
+except ImportError:
+    pass
+
+WIDTH = 640
+HEIGHT = 480
+THRESHOLD = 140
+SEARCH_RADIUS = 50
+
+
+def read_key_ev():
+    """非阻塞读取 /dev/input/event0 按键（返回 key_code 或 -1）"""
+    try:
+        r, _, _ = select.select([_key_fd], [], [], 0)
+        if r:
+            event = _key_fd.read(16)
+            if len(event) == 16:
+                _, _, etype, code, value = struct.unpack("IIHHI", event)
+                if etype == 1 and value == 1:
+                    return code
+    except Exception:
+        pass
+    return -1
+
+
+def find_ellipse(img_cv, cx, cy, roi_r, th):
+    x1 = max(0, cx - roi_r)
+    x2 = min(WIDTH, cx + roi_r)
+    y1 = max(0, cy - roi_r)
+    y2 = min(HEIGHT, cy + roi_r)
+    roi = img_cv[y1:y2, x1:x2]
+    if roi.size == 0:
+        return None
+    r = roi[:, :, 0].astype(np.int32)
+    g = roi[:, :, 1].astype(np.int32)
+    b = roi[:, :, 2].astype(np.int32)
+    mask = (r > th) & (r > g * 1.5) & (r > b * 1.5)
+    oe = (r > 200) & (g > 200) & (b > 200) & (r >= g) & (r >= b) & ((r - g) > 10) & ((r - b) > 10)
+    combined = (mask | oe).astype(np.uint8) * 255
+    contours, _ = cv2.findContours(combined, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+    if not contours:
+        return None
+    largest = max(contours, key=cv2.contourArea)
+    if cv2.contourArea(largest) < 5:
+        return None
+    cnt = largest.copy()
+    for pt in cnt:
+        pt[0][0] += x1
+        pt[0][1] += y1
+    if len(cnt) >= 5:
+        (ex, ey), (ew, eh), ang = cv2.fitEllipse(cnt)
+        mask_ellipse = np.zeros((HEIGHT, WIDTH), dtype=np.uint8)
+        cv2.ellipse(mask_ellipse, (int(ex), int(ey)), (int(ew / 2), int(eh / 2)), ang, 0, 360, 255, -1)
+        brightness = img_cv[:, :, 0].astype(np.int32) + img_cv[:, :, 1].astype(np.int32) + img_cv[:, :, 2].astype(np.int32)
+        masked = np.where(mask_ellipse > 0, brightness, 0)
+        vals = masked[masked > 0]
+        if len(vals) > 0:
+            bth = np.percentile(vals, 90)
+            bmask = (masked >= bth).astype(np.uint8) * 255
+            bcontours, _ = cv2.findContours(bmask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+            if bcontours:
+                blargest = max(bcontours, key=cv2.contourArea)
+                if cv2.contourArea(blargest) >= 3 and len(blargest) >= 5:
+                    (ix, iy), _, _ = cv2.fitEllipse(blargest)
+                    return (float(ix), float(iy))
+                M = cv2.moments(blargest)
+                if M["m00"] > 0:
+                    return (float(M["m10"] / M["m00"]), float(M["m01"] / M["m00"]))
+        return (float(ex), float(ey))
+    M = cv2.moments(cnt)
+    if M["m00"] > 0:
+        return (float(M["m10"] / M["m00"]), float(M["m01"] / M["m00"]))
+    return None
+
+
+def find_brightest(img_cv, cx, cy, roi_r, th):
+    x1 = max(0, cx - roi_r)
+    x2 = min(WIDTH, cx + roi_r)
+    y1 = max(0, cy - roi_r)
+    y2 = min(HEIGHT, cy + roi_r)
+    best_score = 0
+    best_pos = None
+    for y in range(y1, y2):
+        for x in range(x1, x2):
+            r, g, b = int(img_cv[y, x, 0]), int(img_cv[y, x, 1]), int(img_cv[y, x, 2])
+            is_red = (r > th and r > g * 1.5 and r > b * 1.5)
+            is_oe = (r > 200 and g > 200 and b > 200 and r >= g and r >= b and (r - g) > 10 and (r - b) > 10)
+            if is_red or is_oe:
+                score = r + g + b
+                dx, dy = x - cx, y - cy
+                dist = (dx * dx + dy * dy) ** 0.5
+                score *= max(0.5, 1.0 - (dist / roi_r) * 0.5)
+                if score > best_score:
+                    best_score = score
+                    best_pos = (float(x), float(y))
+    return best_pos
+
+
+# 打开键盘输入设备
+_key_fd = None
+try:
+    _key_fd = open("/dev/input/event0", "rb")
+except Exception:
+    try:
+        _key_fd = open("/dev/input/event1", "rb")
+    except Exception:
+        _key_fd = None
+
+print("=" * 50)
+print("激光中心点检测单元测试")
+print("=" * 50)
+print()
+
+cam = camera.Camera(WIDTH, HEIGHT)
+disp = display.Display()
+print("[OK] 摄像头和显示初始化完成")
+
+# 初始化激光串口
+_laser_on = False
+_laser_uart = None
+try:
+    pinmap.set_pin_function("A18", "UART1_RX")
+    pinmap.set_pin_function("A19", "UART1_TX")
+    _laser_uart = uart.UART("/dev/ttyS1", 9600)
+    _laser_uart.read(-1)
+    print("[OK] 激光串口初始化完成")
+except Exception as e:
+    print(f"[WARN] 激光串口初始化失败: {e}")
+
+LASER_ON = bytes([0xAA, 0x00, 0x01, 0xBE, 0x00, 0x01, 0x00, 0x01, 0xC1])
+LASER_OFF = bytes([0xAA, 0x00, 0x01, 0xBE, 0x00, 0x01, 0x00, 0x00, 0xC0])
+
+# 默认开启激光
+if _laser_uart:
+    try:
+        _laser_uart.write(LASER_ON)
+        time.sleep_ms(50)
+        _laser_uart.read(-1)
+        _laser_on = True
+        print("[OK] 激光已开启")
+    except Exception as e:
+        print(f"[WARN] 开启激光失败: {e}")
+print()
+
+pos_ellipse = None
+pos_bright = None
+frame_count = 0
+use_ellipse = True
+
+while not app.need_exit():
+    frame = cam.read()
+    if frame is None:
+        time.sleep_ms(10)
+        continue
+
+    frame_count += 1
+
+    if _USE_CV:
+        img_cv = image.image2cv(frame, False, False)
+        cx, cy = WIDTH // 2, HEIGHT // 2
+
+        t0 = time.ticks_ms()
+        pos_ellipse = find_ellipse(img_cv, cx, cy, SEARCH_RADIUS, THRESHOLD)
+        t1 = time.ticks_ms()
+        pos_bright = find_brightest(img_cv, cx, cy, SEARCH_RADIUS, THRESHOLD)
+        t2 = time.ticks_ms()
+
+        dt_e = abs(time.ticks_diff(t0, t1))
+        dt_b = abs(time.ticks_diff(t1, t2))
+
+        if frame_count % 5 == 0:
+            e_str = f"({pos_ellipse[0]:.1f},{pos_ellipse[1]:.1f})" if pos_ellipse else "None"
+            b_str = f"({pos_bright[0]:.1f},{pos_bright[1]:.1f})" if pos_bright else "None"
+            print(f"[LASER] ellipse={e_str} ({dt_e}ms)  brightest={b_str} ({dt_b}ms)  "
+                  f"th={THRESHOLD}  radius={SEARCH_RADIUS}")
+
+        # 叠加显示
+        pos = pos_ellipse if use_ellipse else pos_bright
+        h, w = img_cv.shape[:2]
+        cv2.circle(img_cv, (cx, cy), SEARCH_RADIUS, (0, 255, 0), 1)
+        cv2.circle(img_cv, (cx, cy), 2, (0, 255, 0), -1)
+        if pos:
+            x, y = int(pos[0]), int(pos[1])
+            cv2.circle(img_cv, (x, y), 6, (0, 0, 255), 2)
+            cv2.line(img_cv, (x - 14, y), (x + 14, y), (0, 0, 255), 1)
+            cv2.line(img_cv, (x, y - 14), (x, y + 14), (0, 0, 255), 1)
+            cv2.putText(img_cv, f"({x},{y})", (x + 10, y - 10),
+                        cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 1, cv2.LINE_AA)
+        info = [
+            f"pos={pos if pos else 'None'}",
+            f"method={'ellipse' if use_ellipse else 'brightest'}  th={THRESHOLD}",
+            f"laser={'ON' if _laser_on else 'OFF'}",
+        ]
+        for i, line in enumerate(info):
+            cv2.putText(img_cv, line, (8, 20 + i * 22),
+                        cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1, cv2.LINE_AA)
+
+        display_frame = image.cv2image(img_cv, False, False)
+    else:
+        display_frame = frame
+
+    disp.show(display_frame)
+
+    # 按键处理（非阻塞）
+    key = read_key_ev()
+    if key > 0:
+        c = chr(key & 0xFF) if key < 256 else ""
+        if key == 113 or key == 81 or key == 0x1b:  # q/Q/ESC
+            break
+        if c == "e" or key == 18:  # e
+            use_ellipse = not use_ellipse
+            print(f"[KEY] Method: {'ellipse' if use_ellipse else 'brightest'}")
+        if c == "l" or key == 12:  # l
+            _laser_on = not _laser_on
+            if _laser_uart:
+                try:
+                    _laser_uart.write(LASER_ON if _laser_on else LASER_OFF)
+                    time.sleep_ms(30)
+                    _laser_uart.read(-1)
+                    print(f"[KEY] Laser: {'ON' if _laser_on else 'OFF'}")
+                except Exception as e:
+                    print(f"[KEY] Laser error: {e}")
+            else:
+                print("[KEY] Laser UART not available")
+    time.sleep_ms(30)
+
+# 关闭激光
+if _laser_on and _laser_uart:
+    try:
+        _laser_uart.write(LASER_OFF)
+        _laser_uart.read(-1)
+        print("[EXIT] 激光已关闭")
+    except Exception:
+        pass
+print("[EXIT] 测试结束")
+if _key_fd:
+    _key_fd.close()
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+激光中心点检测单元测试（单文件，无项目依赖）
+直接使用 maix 标准库，实现红色激光点坐标检测
+
+运行方式：
+  python3 test/test_laser_center_point.py
+
+Ctrl+C 退出，按 s 保存截图
+"""
+
+from maix import camera, display, image, time, app, uart, pinmap
+import os
+import struct
+import select
+
+_USE_CV = False
+try:
+    import cv2
+    import numpy as np
+    _USE_CV = True
+except ImportError:
+    pass
+
+WIDTH = 640
+HEIGHT = 480
+THRESHOLD = 120
+RED_RATIO = 1.3
+SEARCH_RADIUS = 60
+
+
+def read_key_ev():
+    """非阻塞读取 /dev/input/event0 按键（返回 key_code 或 -1）"""
+    try:
+        r, _, _ = select.select([_key_fd], [], [], 0)
+        if r:
+            event = _key_fd.read(16)
+            if len(event) == 16:
+                _, _, etype, code, value = struct.unpack("IIHHI", event)
+                if etype == 1 and value == 1:
+                    return code
+    except Exception:
+        pass
+    return -1
+
+
+def find_ellipse(img_cv, cx, cy, roi_r, th, ratio):
+    x1 = max(0, cx - roi_r)
+    x2 = min(WIDTH, cx + roi_r)
+    y1 = max(0, cy - roi_r)
+    y2 = min(HEIGHT, cy + roi_r)
+    roi = img_cv[y1:y2, x1:x2]
+    if roi.size == 0:
+        return None
+    r = roi[:, :, 0].astype(np.int32)
+    g = roi[:, :, 1].astype(np.int32)
+    b = roi[:, :, 2].astype(np.int32)
+    mask = (r > th) & (r > g * ratio) & (r > b * ratio)
+    oe = (r > 200) & (g > 200) & (b > 200) & (r >= g) & (r >= b) & ((r - g) > 10) & ((r - b) > 10)
+    combined = (mask | oe).astype(np.uint8) * 255
+    contours, _ = cv2.findContours(combined, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+    if not contours:
+        return None
+    largest = max(contours, key=cv2.contourArea)
+    if cv2.contourArea(largest) < 5:
+        return None
+    cnt = largest.copy()
+    for pt in cnt:
+        pt[0][0] += x1
+        pt[0][1] += y1
+    if len(cnt) >= 5:
+        (ex, ey), (ew, eh), ang = cv2.fitEllipse(cnt)
+        mask_ellipse = np.zeros((HEIGHT, WIDTH), dtype=np.uint8)
+        cv2.ellipse(mask_ellipse, (int(ex), int(ey)), (int(ew / 2), int(eh / 2)), ang, 0, 360, 255, -1)
+        brightness = img_cv[:, :, 0].astype(np.int32) + img_cv[:, :, 1].astype(np.int32) + img_cv[:, :, 2].astype(np.int32)
+        masked = np.where(mask_ellipse > 0, brightness, 0)
+        vals = masked[masked > 0]
+        if len(vals) > 0:
+            bth = np.percentile(vals, 90)
+            bmask = (masked >= bth).astype(np.uint8) * 255
+            bcontours, _ = cv2.findContours(bmask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+            if bcontours:
+                blargest = max(bcontours, key=cv2.contourArea)
+                if cv2.contourArea(blargest) >= 3 and len(blargest) >= 5:
+                    (ix, iy), _, _ = cv2.fitEllipse(blargest)
+                    return (float(ix), float(iy))
+                M = cv2.moments(blargest)
+                if M["m00"] > 0:
+                    return (float(M["m10"] / M["m00"]), float(M["m01"] / M["m00"]))
+        return (float(ex), float(ey))
+    M = cv2.moments(cnt)
+    if M["m00"] > 0:
+        return (float(M["m10"] / M["m00"]), float(M["m01"] / M["m00"]))
+    return None
+
+
+def find_brightest_bytes(frame, cx, cy, roi_r, th, ratio):
+    """使用 frame.to_bytes() 两阶段搜索，避免 cv2 转换"""
+    x1 = max(0, cx - roi_r)
+    x2 = min(WIDTH, cx + roi_r)
+    y1 = max(0, cy - roi_r)
+    y2 = min(HEIGHT, cy + roi_r)
+    data = frame.to_bytes()
+    best_score = 0
+    best_pos = None
+    # 第一阶段：隔点粗搜
+    for y in range(y1, y2, 2):
+        for x in range(x1, x2, 2):
+            idx = (y * WIDTH + x) * 3
+            r = data[idx]; g = data[idx+1]; b = data[idx+2]
+            if (r > th and r > g * ratio and r > b * ratio) or \
+               (r > 200 and g > 200 and b > 200 and r >= g and r >= b and (r - g) > 10 and (r - b) > 10):
+                score = r + g + b
+                dx = x - cx; dy = y - cy
+                score *= max(0.5, 1.0 - ((dx*dx + dy*dy) ** 0.5 / roi_r) * 0.5)
+                if score > best_score:
+                    best_score = score
+                    best_pos = (x, y)
+    if best_pos is None:
+        return None
+    # 第二阶段：候选点 7x7 精细搜索
+    fx, fy = best_pos
+    x1f = max(0, fx - 3); x2f = min(WIDTH, fx + 4)
+    y1f = max(0, fy - 3); y2f = min(HEIGHT, fy + 4)
+    best_bright = 0
+    final_pos = best_pos
+    for y in range(y1f, y2f):
+        for x in range(x1f, x2f):
+            idx = (y * WIDTH + x) * 3
+            r = data[idx]; g = data[idx+1]; b = data[idx+2]
+            if (r > th and r > g * ratio and r > b * ratio) or \
+               (r > 200 and g > 200 and b > 200 and r >= g and r >= b and (r - g) > 10 and (r - b) > 10):
+                rgb_sum = r + g + b
+                if rgb_sum > best_bright:
+                    best_bright = rgb_sum
+                    final_pos = (float(x), float(y))
+    return final_pos
+
+
+# 打开键盘输入设备
+_key_fd = None
+try:
+    _key_fd = open("/dev/input/event0", "rb")
+except Exception:
+    try:
+        _key_fd = open("/dev/input/event1", "rb")
+    except Exception:
+        _key_fd = None
+
+print("=" * 50)
+print("激光中心点检测单元测试")
+print("=" * 50)
+print()
+
+cam = camera.Camera(WIDTH, HEIGHT)
+disp = display.Display()
+print("[OK] 摄像头和显示初始化完成")
+
+# 初始化激光串口
+_laser_on = False
+_laser_uart = None
+try:
+    pinmap.set_pin_function("A18", "UART1_RX")
+    pinmap.set_pin_function("A19", "UART1_TX")
+    _laser_uart = uart.UART("/dev/ttyS1", 9600)
+    _laser_uart.read(-1)
+    print("[OK] 激光串口初始化完成")
+except Exception as e:
+    print(f"[WARN] 激光串口初始化失败: {e}")
+
+LASER_ON = bytes([0xAA, 0x00, 0x01, 0xBE, 0x00, 0x01, 0x00, 0x01, 0xC1])
+LASER_OFF = bytes([0xAA, 0x00, 0x01, 0xBE, 0x00, 0x01, 0x00, 0x00, 0xC0])
+
+# 默认开启激光
+if _laser_uart:
+    try:
+        _laser_uart.write(LASER_ON)
+        time.sleep_ms(50)
+        _laser_uart.read(-1)
+        _laser_on = True
+        print("[OK] 激光已开启")
+    except Exception as e:
+        print(f"[WARN] 开启激光失败: {e}")
+print()
+
+pos_ellipse = None
+pos_bright = None
+frame_count = 0
+use_ellipse = True
+
+while not app.need_exit():
+    frame = cam.read()
+    if frame is None:
+        time.sleep_ms(10)
+        continue
+
+    frame_count += 1
+
+    cx, cy = WIDTH // 2, HEIGHT // 2
+
+    t0 = time.ticks_ms()
+    pos_bright = find_brightest_bytes(frame, cx, cy, SEARCH_RADIUS, THRESHOLD, RED_RATIO)
+    t1 = time.ticks_ms()
+
+    pos_ellipse = None
+    if _USE_CV:
+        img_cv = image.image2cv(frame, False, False)
+        t2 = time.ticks_ms()
+        pos_ellipse = find_ellipse(img_cv, cx, cy, SEARCH_RADIUS, THRESHOLD, RED_RATIO)
+        t3 = time.ticks_ms()
+    else:
+        img_cv = None
+        t3 = t2 = t1
+
+    dt_b = abs(time.ticks_diff(t0, t1))
+    dt_e = abs(time.ticks_diff(t2, t3))
+
+    if frame_count % 5 == 0:
+        e_str = f"({pos_ellipse[0]:.1f},{pos_ellipse[1]:.1f})" if pos_ellipse else "None"
+        b_str = f"({pos_bright[0]:.1f},{pos_bright[1]:.1f})" if pos_bright else "None"
+        print(f"[LASER] ellipse={e_str} ({dt_e}ms)  brightest={b_str} ({dt_b}ms)  "
+              f"th={THRESHOLD}  ratio={RED_RATIO}  radius={SEARCH_RADIUS}")
+
+    pos = pos_ellipse if use_ellipse else pos_bright
+    if img_cv is not None:
+        cv2.circle(img_cv, (cx, cy), SEARCH_RADIUS, (0, 255, 0), 1)
+        cv2.circle(img_cv, (cx, cy), 2, (0, 255, 0), -1)
+        if pos:
+            x, y = int(pos[0]), int(pos[1])
+            cv2.circle(img_cv, (x, y), 6, (0, 0, 255), 2)
+            cv2.line(img_cv, (x - 14, y), (x + 14, y), (0, 0, 255), 1)
+            cv2.line(img_cv, (x, y - 14), (x, y + 14), (0, 0, 255), 1)
+            cv2.putText(img_cv, f"({x},{y})", (x + 10, y - 10),
+                        cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 1, cv2.LINE_AA)
+        info = [
+            f"pos={pos if pos else 'None'}",
+            f"method={'ellipse' if use_ellipse else 'brightest'}  th={THRESHOLD}  ratio={RED_RATIO}",
+            f"laser={'ON' if _laser_on else 'OFF'}",
+        ]
+        for i, line in enumerate(info):
+            cv2.putText(img_cv, line, (8, 20 + i * 22),
+                        cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1, cv2.LINE_AA)
+        display_frame = image.cv2image(img_cv, False, False)
+    else:
+        display_frame = frame
+
+    disp.show(display_frame)
+
+    # 按键处理（非阻塞）
+    key = read_key_ev()
+    if key > 0:
+        c = chr(key & 0xFF) if key < 256 else ""
+        if key == 113 or key == 81 or key == 0x1b:  # q/Q/ESC
+            break
+        if c == "e" or key == 18:  # e
+            use_ellipse = not use_ellipse
+            print(f"[KEY] Method: {'ellipse' if use_ellipse else 'brightest'}")
+        if c == "l" or key == 12:  # l
+            _laser_on = not _laser_on
+            if _laser_uart:
+                try:
+                    _laser_uart.write(LASER_ON if _laser_on else LASER_OFF)
+                    time.sleep_ms(30)
+                    _laser_uart.read(-1)
+                    print(f"[KEY] Laser: {'ON' if _laser_on else 'OFF'}")
+                except Exception as e:
+                    print(f"[KEY] Laser error: {e}")
+            else:
+                print("[KEY] Laser UART not available")
+    time.sleep_ms(30)
+
+# 关闭激光
+if _laser_on and _laser_uart:
+    try:
+        _laser_uart.write(LASER_OFF)
+        _laser_uart.read(-1)
+        print("[EXIT] 激光已关闭")
+    except Exception:
+        pass
+print("[EXIT] 测试结束")
+if _key_fd:
+    _key_fd.close()