diff --git a/test/test_laser_center_point.py b/test/test_laser_center_point.py new file mode 100644 index 0000000..372ed8e --- /dev/null +++ b/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()