diff --git a/laser_manager.py b/laser_manager.py index 5b40b8f..66d5065 100644 --- a/laser_manager.py +++ b/laser_manager.py @@ -175,23 +175,189 @@ class LaserManager: if logger: logger.error(f"闪激光失败: {e}") - def find_red_laser(self, frame, threshold=150): - """在图像中查找最亮的红色激光点（基于 RGB 阈值）""" + # def find_red_laser(self, frame, threshold=150): + # """在图像中查找最亮的红色激光点（基于 RGB 阈值）""" + # w, h = frame.width(), frame.height() + # img_bytes = frame.to_bytes() + # max_sum = 0 + # best_pos = None + # for y in range(0, h, 2): + # for x in range(0, w, 2): + # idx = (y * w + x) * 3 + # r, g, b = img_bytes[idx], img_bytes[idx+1], img_bytes[idx+2] + # if r > threshold and r > g * 2 and r > b * 2: + # rgb_sum = r + g + b + # if rgb_sum > max_sum: + # max_sum = rgb_sum + # best_pos = (x, y) + # return best_pos + + # def find_red_laser(self, frame, threshold=150, search_radius=50): + # """ + # 在图像中心附近查找最亮的红色激光点（基于 RGB 阈值） + + # Args: + # frame: 图像帧 + # threshold: 红色通道阈值（默认150） + # search_radius: 搜索半径（像素），从图像中心开始搜索（默认150） + + # Returns: + # (x, y) 坐标，如果未找到则返回 None + # """ + # w, h = frame.width(), frame.height() + # center_x, center_y = w // 2, h // 2 + + # # 只在中心区域搜索 + # x_min = max(0, center_x - search_radius) + # x_max = min(w, center_x + search_radius) + # y_min = max(0, center_y - search_radius) + # y_max = min(h, center_y + search_radius) + + # img_bytes = frame.to_bytes() + # max_score = 0 + # best_pos = None + + # for y in range(y_min, y_max, 2): + # for x in range(x_min, x_max, 2): + # idx = (y * w + x) * 3 + # r, g, b = img_bytes[idx], img_bytes[idx+1], img_bytes[idx+2] + + # # 判断是否为红色或过曝的红色（发白） + # is_red = False + # is_overexposed_red = False + + # # 情况1：正常红色（r 明显大于 g 和 b） + # if r > threshold and r > g * 2 and r > b * 2: + # is_red = True + + # # 情况2：过曝的红色（发白，r, g, b 都接近255，但 r 仍然最大） + # # 过曝时，r, g, b 都接近 255，但 r 应该仍然是最高的 + # elif r > 200 and g > 200 and b > 200: # 接近白色 + # if r >= g and r >= b and (r - g) > 10 and (r - b) > 10: # r 仍然明显最大 + # is_overexposed_red = True + + # if is_red or is_overexposed_red: + # # 计算得分：RGB 总和 + 距离中心权重（越靠近中心得分越高） + # rgb_sum = r + g + b + # # 计算到中心的距离 + # dx = x - center_x + # dy = y - center_y + # distance_from_center = (dx * dx + dy * dy) ** 0.5 + # # 距离权重：距离越近，权重越高（最大权重为 1.0，距离为 0 时） + # # 当距离为 search_radius 时，权重为 0.5 + # distance_weight = 1.0 - (distance_from_center / search_radius) * 0.5 + # distance_weight = max(0.5, distance_weight) # 最小权重 0.5 + + # # 综合得分：RGB 总和 * 距离权重 + # score = rgb_sum * distance_weight + + # if score > max_score: + # max_score = score + # best_pos = (x, y) + # print("best_pos:", best_pos) + # return best_pos + def find_red_laser(self, frame, threshold=150, search_radius=150): + """ + 在图像中心附近查找最亮的红色激光点（基于 RGB 阈值） + 使用两阶段搜索：先粗搜索找到候选区域，再精细搜索找到最亮点 + + Args: + frame: 图像帧 + threshold: 红色通道阈值（默认150） + search_radius: 搜索半径（像素），从图像中心开始搜索（默认150） + + Returns: + (x, y) 坐标，如果未找到则返回 None + """ w, h = frame.width(), frame.height() + center_x, center_y = w // 2, h // 2 + + # 只在中心区域搜索 + x_min = max(0, center_x - search_radius) + x_max = min(w, center_x + search_radius) + y_min = max(0, center_y - search_radius) + y_max = min(h, center_y + search_radius) + img_bytes = frame.to_bytes() - max_sum = 0 - best_pos = None - for y in range(0, h, 2): - for x in range(0, w, 2): + max_score = 0 + candidate_pos = None + + # 第一阶段：粗搜索（每2像素采样），找到候选点 + for y in range(y_min, y_max, 2): + for x in range(x_min, x_max, 2): idx = (y * w + x) * 3 r, g, b = img_bytes[idx], img_bytes[idx+1], img_bytes[idx+2] + + # 判断是否为红色或过曝的红色（发白） + is_red = False + is_overexposed_red = False + + # 情况1：正常红色（r 明显大于 g 和 b） if r > threshold and r > g * 2 and r > b * 2: + is_red = True + + # 情况2：过曝的红色（发白，r, g, b 都接近255，但 r 仍然最大） + elif r > 200 and g > 200 and b > 200: # 接近白色 + if r >= g and r >= b and (r - g) > 10 and (r - b) > 10: # r 仍然明显最大 + is_overexposed_red = True + + if is_red or is_overexposed_red: + # 计算得分：RGB 总和 + 距离中心权重 rgb_sum = r + g + b - if rgb_sum > max_sum: - max_sum = rgb_sum + dx = x - center_x + dy = y - center_y + distance_from_center = (dx * dx + dy * dy) ** 0.5 + distance_weight = 1.0 - (distance_from_center / search_radius) * 0.5 + distance_weight = max(0.5, distance_weight) + + score = rgb_sum * distance_weight + + if score > max_score: + max_score = score + candidate_pos = (x, y) + + # 如果没有找到候选点，直接返回 + if candidate_pos is None: + return None + + # 第二阶段：在候选点周围进行精细搜索（1像素间隔） + # 在候选点周围 5x5 或 7x7 区域内找最亮的点 + refine_radius = 3 # 精细搜索半径（像素） + cx, cy = candidate_pos + + x_min_fine = max(0, cx - refine_radius) + x_max_fine = min(w, cx + refine_radius + 1) + y_min_fine = max(0, cy - refine_radius) + y_max_fine = min(h, cy + refine_radius + 1) + + max_brightness = 0 + best_pos = candidate_pos + + # 精细搜索：1像素间隔，只考虑亮度（RGB总和） + for y in range(y_min_fine, y_max_fine, 1): + for x in range(x_min_fine, x_max_fine, 1): + idx = (y * w + x) * 3 + r, g, b = img_bytes[idx], img_bytes[idx+1], img_bytes[idx+2] + + # 判断是否为红色或过曝的红色 + is_red = False + is_overexposed_red = False + + if r > threshold and r > g * 2 and r > b * 2: + is_red = True + elif r > 200 and g > 200 and b > 200: + if r >= g and r >= b and (r - g) > 10 and (r - b) > 10: + is_overexposed_red = True + + if is_red or is_overexposed_red: + rgb_sum = r + g + b + # 精细搜索阶段只考虑亮度，不考虑距离权重 + if rgb_sum > max_brightness: + max_brightness = rgb_sum best_pos = (x, y) + return best_pos - + def calibrate_laser_position(self): """执行一次激光校准：拍照 → 找红点 → 保存坐标""" time.sleep_ms(80) diff --git a/main.py b/main.py index ade8f48..9fc06d5 100644 --- a/main.py +++ b/main.py @@ -23,17 +23,91 @@ from logger_manager import logger_manager from time_sync import sync_system_time_from_4g from power import init_ina226, get_bus_voltage, voltage_to_percent from laser_manager import laser_manager -from vision import detect_circle_v3, estimate_distance, compute_laser_position, save_shot_image +from vision import detect_circle_v3, estimate_distance, compute_laser_position, save_shot_image, save_calibration_image from network import network_manager from ota_manager import ota_manager from hardware import hardware_manager +# def laser_calibration_worker(): +# """后台线程：持续检测是否需要执行激光校准""" +# from maix import camera +# from laser_manager import laser_manager +# from ota_manager import ota_manager + +# logger = logger_manager.logger +# if logger: +# logger.info("[LASER] 激光校准线程启动") + +# while True: +# try: +# try: +# if ota_manager.ota_in_progress: +# time.sleep_ms(200) +# continue +# except Exception as e: +# logger = logger_manager.logger +# if logger: +# logger.error(f"[LASER] OTA检查异常: {e}") +# time.sleep_ms(200) +# continue + +# if laser_manager.calibration_active: +# cam = None +# try: +# cam = camera.Camera(640, 480) +# start = time.ticks_ms() +# timeout_ms = 8000 +# while laser_manager.calibration_active and time.ticks_diff(time.ticks_ms(), start) < timeout_ms: +# frame = cam.read() +# pos = laser_manager.find_red_laser(frame) +# if pos: +# laser_manager.set_calibration_result(pos) +# laser_manager.stop_calibration() +# laser_manager.save_laser_point(pos) +# logger = logger_manager.logger +# if logger: +# logger.info(f"✅ 后台校准成功: {pos}") +# break +# time.sleep_ms(60) +# except Exception as e: +# logger = logger_manager.logger +# if logger: +# logger.error(f"[LASER] calibration error: {e}") +# import traceback +# logger.error(traceback.format_exc()) +# time.sleep_ms(200) +# finally: +# try: +# if cam is not None: +# del cam +# except Exception as e: +# logger = logger_manager.logger +# if logger: +# logger.error(f"[LASER] 释放相机资源异常: {e}") + +# if laser_manager.calibration_active: +# time.sleep_ms(300) +# else: +# time.sleep_ms(50) +# except Exception as e: +# # 线程顶层异常捕获，防止线程静默退出 +# logger = logger_manager.logger +# if logger: +# logger.error(f"[LASER] 校准线程异常: {e}") +# import traceback +# logger.error(traceback.format_exc()) +# else: +# print(f"[LASER] 校准线程异常: {e}") +# import traceback +# traceback.print_exc() +# time.sleep_ms(1000) # 等待1秒后继续 def laser_calibration_worker(): """后台线程：持续检测是否需要执行激光校准""" from maix import camera from laser_manager import laser_manager from ota_manager import ota_manager + from vision import save_calibration_image # 添加导入 logger = logger_manager.logger if logger: @@ -62,6 +136,14 @@ def laser_calibration_worker(): frame = cam.read() pos = laser_manager.find_red_laser(frame) if pos: + # 保存校准图像（带标注） + try: + save_calibration_image(frame, pos) + except Exception as e: + logger = logger_manager.logger + if logger: + logger.error(f"[LASER] 保存校准图像失败: {e}") + laser_manager.set_calibration_result(pos) laser_manager.stop_calibration() laser_manager.save_laser_point(pos) @@ -103,7 +185,6 @@ def laser_calibration_worker(): traceback.print_exc() time.sleep_ms(1000) # 等待1秒后继续 - def cmd_str(): """主程序入口""" # ==================== 第一阶段：硬件初始化 ==================== diff --git a/vision.py b/vision.py index dc38585..38847e8 100644 --- a/vision.py +++ b/vision.py @@ -12,6 +12,81 @@ from maix import image import config from logger_manager import logger_manager +def save_calibration_image(frame, laser_pos, photo_dir=None): + """ + 保存激光校准图像（带标注） + 在找到的激光点位置绘制圆圈，便于检查算法是否正确 + + Args: + frame: 原始图像帧 + laser_pos: 找到的激光点坐标 (x, y) + photo_dir: 照片存储目录，如果为None则使用 config.PHOTO_DIR + + Returns: + str: 保存的文件路径，如果保存失败则返回 None + """ + # 检查是否启用图像保存 + if not config.SAVE_IMAGE_ENABLED: + return None + + if photo_dir is None: + photo_dir = config.PHOTO_DIR + + try: + # 确保照片目录存在 + try: + if photo_dir not in os.listdir("/root"): + os.mkdir(photo_dir) + except: + pass + + # 生成文件名 + try: + all_images = [f for f in os.listdir(photo_dir) if f.endswith(('.bmp', '.jpg', '.jpeg'))] + img_count = len(all_images) + except: + img_count = 0 + + x, y = laser_pos + filename = f"{photo_dir}/calibration_{int(x)}_{int(y)}_{img_count:04d}.bmp" + + logger = logger_manager.logger + if logger: + logger.info(f"保存校准图像: {filename}, 激光点: ({x}, {y})") + + # 转换图像为 OpenCV 格式以便绘制 + img_cv = image.image2cv(frame, False, False) + + # 绘制激光点圆圈（用绿色圆圈标出找到的激光点） + cv2.circle(img_cv, (int(x), int(y)), 10, (0, 255, 0), 2) # 外圈：绿色，半径10 + cv2.circle(img_cv, (int(x), int(y)), 5, (0, 255, 0), 2) # 中圈：绿色，半径5 + cv2.circle(img_cv, (int(x), int(y)), 2, (0, 255, 0), -1) # 中心点：绿色实心 + + # 可选：绘制十字线帮助定位 + cv2.line(img_cv, + (int(x - 20), int(y)), + (int(x + 20), int(y)), + (0, 255, 0), 1) # 水平线 + cv2.line(img_cv, + (int(x), int(y - 20)), + (int(x), int(y + 20)), + (0, 255, 0), 1) # 垂直线 + + # 转换回 MaixPy 图像格式并保存 + result_img = image.cv2image(img_cv, False, False) + result_img.save(filename) + + if logger: + logger.debug(f"校准图像已保存: {filename}") + + return filename + except Exception as e: + logger = logger_manager.logger + if logger: + logger.error(f"保存校准图像失败: {e}") + import traceback + logger.error(traceback.format_exc()) + return None def detect_circle_v3(frame, laser_point=None): """检测图像中的靶心（优先清晰轮廓，其次黄色区域）- 返回椭圆参数版本