#!/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()