add ArUco but no activated

aruco_detector.py

@@ -0,0 +1,420 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+ArUco标记检测模块
+提供基于ArUco标记的靶心标定和激光点定位功能
+"""
+import cv2
+import numpy as np
+import math
+import config
+from logger_manager import logger_manager
+
+
+class ArUcoDetector:
+    """ArUco标记检测器"""
+    
+    def __init__(self):
+        self.logger = logger_manager.logger
+        # 创建ArUco字典和检测器参数
+        self.aruco_dict = cv2.aruco.getPredefinedDictionary(config.ARUCO_DICT_TYPE)
+        self.detector_params = cv2.aruco.DetectorParameters()
+        
+        # 设置检测参数
+        self.detector_params.minMarkerPerimeterRate = config.ARUCO_MIN_MARKER_PERIMETER_RATE
+        self.detector_params.cornerRefinementMethod = config.ARUCO_CORNER_REFINEMENT_METHOD
+        
+        # 创建检测器
+        self.detector = cv2.aruco.ArucoDetector(self.aruco_dict, self.detector_params)
+        
+        # 预定义靶纸上的标记位置（物理坐标，毫米）
+        self.marker_positions_mm = config.ARUCO_MARKER_POSITIONS_MM
+        self.marker_ids = config.ARUCO_MARKER_IDS
+        self.marker_size_mm = config.ARUCO_MARKER_SIZE_MM
+        self.target_paper_size_mm = config.TARGET_PAPER_SIZE_MM
+        
+        # 靶心偏移（相对于靶纸中心）
+        self.target_center_offset_mm = config.TARGET_CENTER_OFFSET_MM
+        
+        if self.logger:
+            self.logger.info(f"[ARUCO] ArUco检测器初始化完成，字典类型: {config.ARUCO_DICT_TYPE}")
+    
+    def detect_markers(self, frame):
+        """
+        检测图像中的ArUco标记
+        
+        Args:
+            frame: MaixPy图像帧对象
+            
+        Returns:
+            (corners, ids, rejected) - 检测到的标记角点、ID列表、被拒绝的候选
+            如果检测失败返回 (None, None, None)
+        """
+        try:
+            # 转换为OpenCV格式
+            from maix import image
+            img_cv = image.image2cv(frame, False, False)
+            
+            # 转换为灰度图（ArUco检测需要）
+            if len(img_cv.shape) == 3:
+                gray = cv2.cvtColor(img_cv, cv2.COLOR_RGB2GRAY)
+            else:
+                gray = img_cv
+            
+            # 检测标记
+            corners, ids, rejected = self.detector.detectMarkers(gray)
+            
+            if self.logger and ids is not None:
+                self.logger.debug(f"[ARUCO] 检测到 {len(ids)} 个标记: {ids.flatten().tolist()}")
+            
+            return corners, ids, rejected
+            
+        except Exception as e:
+            if self.logger:
+                self.logger.error(f"[ARUCO] 标记检测失败: {e}")
+            return None, None, None
+    
+    def get_target_center_from_markers(self, corners, ids):
+        """
+        从检测到的ArUco标记计算靶心位置
+        
+        Args:
+            corners: 标记角点列表
+            ids: 标记ID列表
+            
+        Returns:
+            (center_x, center_y, radius, ellipse_params) 或 (None, None, None, None)
+            center_x, center_y: 靶心像素坐标
+            radius: 估计的靶心半径（像素）
+            ellipse_params: 椭圆参数用于透视校正
+        """
+        if ids is None or len(ids) < 3:
+            if self.logger:
+                self.logger.debug(f"[ARUCO] 检测到的标记数量不足: {len(ids) if ids is not None else 0} < 3")
+            return None, None, None, None
+        
+        try:
+            # 将ID转换为列表便于查找
+            detected_ids = ids.flatten().tolist()
+            
+            # 收集检测到的标记中心点和对应的物理坐标
+            image_points = []  # 图像坐标 (像素)
+            object_points = []  # 物理坐标 (毫米)
+            marker_centers = {}  # 存储每个标记的中心
+            
+            for i, marker_id in enumerate(detected_ids):
+                if marker_id not in self.marker_ids:
+                    continue
+                
+                # 计算标记中心（四个角的平均值）
+                corner = corners[i][0]  # shape: (4, 2)
+                center_x = np.mean(corner[:, 0])
+                center_y = np.mean(corner[:, 1])
+                marker_centers[marker_id] = (center_x, center_y)
+                
+                # 添加到点列表
+                image_points.append([center_x, center_y])
+                object_points.append(self.marker_positions_mm[marker_id])
+            
+            if len(image_points) < 3:
+                if self.logger:
+                    self.logger.debug(f"[ARUCO] 有效标记数量不足: {len(image_points)} < 3")
+                return None, None, None, None
+            
+            # 转换为numpy数组
+            image_points = np.array(image_points, dtype=np.float32)
+            object_points = np.array(object_points, dtype=np.float32)
+            
+            # 计算单应性矩阵（Homography）
+            # 这建立了物理坐标到图像坐标的映射
+            H, status = cv2.findHomography(object_points, image_points, cv2.RANSAC, 5.0)
+            
+            if H is None:
+                if self.logger:
+                    self.logger.warning("[ARUCO] 无法计算单应性矩阵")
+                return None, None, None, None
+            
+            # 计算靶心在图像中的位置
+            # 靶心物理坐标 = 靶纸中心 + 偏移
+            target_center_mm = np.array([[self.target_center_offset_mm[0], 
+                                          self.target_center_offset_mm[1]]], dtype=np.float32)
+            target_center_mm = target_center_mm.reshape(-1, 1, 2)
+            
+            # 使用单应性矩阵投影到图像坐标
+            target_center_img = cv2.perspectiveTransform(target_center_mm, H)
+            center_x = target_center_img[0][0][0]
+            center_y = target_center_img[0][0][1]
+            
+            # 计算靶心半径（像素）
+            # 使用已知物理距离和像素距离的比例
+            # 选择两个标记计算比例尺
+            if len(marker_centers) >= 2:
+                # 使用对角线上的标记计算比例尺
+                if 0 in marker_centers and 2 in marker_centers:
+                    p1_img = np.array(marker_centers[0])
+                    p2_img = np.array(marker_centers[2])
+                    p1_mm = np.array(self.marker_positions_mm[0])
+                    p2_mm = np.array(self.marker_positions_mm[2])
+                elif 1 in marker_centers and 3 in marker_centers:
+                    p1_img = np.array(marker_centers[1])
+                    p2_img = np.array(marker_centers[3])
+                    p1_mm = np.array(self.marker_positions_mm[1])
+                    p2_mm = np.array(self.marker_positions_mm[3])
+                else:
+                    # 使用任意两个标记
+                    keys = list(marker_centers.keys())
+                    p1_img = np.array(marker_centers[keys[0]])
+                    p2_img = np.array(marker_centers[keys[1]])
+                    p1_mm = np.array(self.marker_positions_mm[keys[0]])
+                    p2_mm = np.array(self.marker_positions_mm[keys[1]])
+                
+                pixel_distance = np.linalg.norm(p1_img - p2_img)
+                mm_distance = np.linalg.norm(p1_mm - p2_mm)
+                
+                if mm_distance > 0:
+                    pixels_per_mm = pixel_distance / mm_distance
+                    # 标准靶心半径：10环半径约1.22cm = 12.2mm
+                    # 但这里我们返回一个估计值，实际环数计算在laser_manager中
+                    radius_mm = 122.0  # 整个靶纸的半径约200mm，但靶心区域较小
+                    radius = int(radius_mm * pixels_per_mm)
+                else:
+                    radius = 100  # 默认值
+            else:
+                radius = 100  # 默认值
+            
+            # 计算椭圆参数（用于透视校正）
+            # 从单应性矩阵可以推导出透视变形
+            ellipse_params = self._compute_ellipse_params(H, center_x, center_y)
+            
+            if self.logger:
+                self.logger.info(f"[ARUCO] 靶心计算成功: 中心=({center_x:.1f}, {center_y:.1f}), "
+                               f"半径={radius}px, 检测到{len(marker_centers)}个标记")
+            
+            return (int(center_x), int(center_y)), radius, "aruco", ellipse_params
+            
+        except Exception as e:
+            if self.logger:
+                self.logger.error(f"[ARUCO] 计算靶心失败: {e}")
+                import traceback
+                self.logger.error(traceback.format_exc())
+            return None, None, None, None
+    
+    def _compute_ellipse_params(self, H, center_x, center_y):
+        """
+        从单应性矩阵计算椭圆参数，用于透视校正
+        
+        Args:
+            H: 单应性矩阵 (3x3)
+            center_x, center_y: 靶心图像坐标
+            
+        Returns:
+            ellipse_params: ((center_x, center_y), (width, height), angle)
+        """
+        try:
+            # 在物理坐标系中画一个圆，投影到图像中看变成什么形状
+            # 物理圆：半径10mm
+            r_mm = 10.0
+            angles = np.linspace(0, 2*np.pi, 16)
+            circle_mm = np.array([[self.target_center_offset_mm[0] + r_mm * np.cos(a),
+                                   self.target_center_offset_mm[1] + r_mm * np.sin(a)] 
+                                  for a in angles], dtype=np.float32)
+            circle_mm = circle_mm.reshape(-1, 1, 2)
+            
+            # 投影到图像
+            circle_img = cv2.perspectiveTransform(circle_mm, H)
+            circle_img = circle_img.reshape(-1, 2)
+            
+            # 拟合椭圆
+            if len(circle_img) >= 5:
+                ellipse = cv2.fitEllipse(circle_img.astype(np.float32))
+                return ellipse
+            else:
+                # 从单应性矩阵近似估计
+                # 提取缩放和旋转
+                # H = K * [R|t] 的近似
+                # 这里简化处理：假设没有严重变形
+                scale_x = np.linalg.norm(H[0, :2])
+                scale_y = np.linalg.norm(H[1, :2])
+                avg_scale = (scale_x + scale_y) / 2
+                
+                width = r_mm * 2 * scale_x
+                height = r_mm * 2 * scale_y
+                angle = np.degrees(np.arctan2(H[1, 0], H[0, 0]))
+                
+                return ((center_x, center_y), (width, height), angle)
+                
+        except Exception as e:
+            if self.logger:
+                self.logger.debug(f"[ARUCO] 计算椭圆参数失败: {e}")
+            return None
+    
+    def transform_laser_point(self, laser_point, corners, ids):
+        """
+        将激光点从图像坐标转换到物理坐标（毫米），再计算相对于靶心的偏移
+        
+        Args:
+            laser_point: (x, y) 激光点在图像中的坐标
+            corners: 检测到的标记角点
+            ids: 检测到的标记ID
+            
+        Returns:
+            (dx_mm, dy_mm) 激光点相对于靶心的偏移（毫米），或 (None, None)
+        """
+        if laser_point is None or ids is None or len(ids) < 3:
+            return None, None
+        
+        try:
+            # 重新计算单应性矩阵（可以优化为缓存）
+            detected_ids = ids.flatten().tolist()
+            image_points = []
+            object_points = []
+            
+            for i, marker_id in enumerate(detected_ids):
+                if marker_id not in self.marker_ids:
+                    continue
+                corner = corners[i][0]
+                center_x = np.mean(corner[:, 0])
+                center_y = np.mean(corner[:, 1])
+                image_points.append([center_x, center_y])
+                object_points.append(self.marker_positions_mm[marker_id])
+            
+            if len(image_points) < 3:
+                return None, None
+            
+            image_points = np.array(image_points, dtype=np.float32)
+            object_points = np.array(object_points, dtype=np.float32)
+            
+            H, _ = cv2.findHomography(object_points, image_points, cv2.RANSAC, 5.0)
+            if H is None:
+                return None, None
+            
+            # 求逆矩阵，将图像坐标转换到物理坐标
+            H_inv = np.linalg.inv(H)
+            
+            laser_img = np.array([[laser_point[0], laser_point[1]]], dtype=np.float32)
+            laser_img = laser_img.reshape(-1, 1, 2)
+            
+            laser_mm = cv2.perspectiveTransform(laser_img, H_inv)
+            laser_x_mm = laser_mm[0][0][0]
+            laser_y_mm = laser_mm[0][0][1]
+            
+            # 计算相对于靶心的偏移
+            # 注意：Y轴方向可能需要翻转（图像Y向下，物理Y通常向上）
+            dx_mm = laser_x_mm - self.target_center_offset_mm[0]
+            dy_mm = -(laser_y_mm - self.target_center_offset_mm[1])  # 翻转Y轴
+            
+            if self.logger:
+                self.logger.debug(f"[ARUCO] 激光点转换: 图像({laser_point[0]:.1f}, {laser_point[1]:.1f}) -> "
+                                f"物理({laser_x_mm:.1f}, {laser_y_mm:.1f}) -> "
+                                f"偏移({dx_mm:.1f}, {dy_mm:.1f})mm")
+            
+            return dx_mm, dy_mm
+            
+        except Exception as e:
+            if self.logger:
+                self.logger.error(f"[ARUCO] 激光点转换失败: {e}")
+            return None, None
+    
+    def draw_debug_info(self, frame, corners, ids, target_center=None, laser_point=None):
+        """
+        在图像上绘制调试信息
+        
+        Args:
+            frame: MaixPy图像帧
+            corners: 标记角点
+            ids: 标记ID
+            target_center: 计算的靶心位置
+            laser_point: 激光点位置
+            
+        Returns:
+            绘制后的图像
+        """
+        try:
+            from maix import image
+            img_cv = image.image2cv(frame, False, False).copy()
+            
+            # 绘制检测到的标记
+            if ids is not None:
+                cv2.aruco.drawDetectedMarkers(img_cv, corners, ids)
+                
+                # 绘制标记ID和中心
+                for i, marker_id in enumerate(ids.flatten()):
+                    corner = corners[i][0]
+                    center_x = int(np.mean(corner[:, 0]))
+                    center_y = int(np.mean(corner[:, 1]))
+                    
+                    # 绘制中心点
+                    cv2.circle(img_cv, (center_x, center_y), 5, (0, 255, 0), -1)
+                    
+                    # 绘制ID
+                    cv2.putText(img_cv, f"ID:{marker_id}", 
+                               (center_x + 10, center_y - 10),
+                               cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2)
+            
+            # 绘制靶心
+            if target_center:
+                cv2.circle(img_cv, target_center, 8, (255, 0, 0), -1)
+                cv2.circle(img_cv, target_center, 50, (255, 0, 0), 2)
+                cv2.putText(img_cv, "TARGET", (target_center[0] + 15, target_center[1] - 15),
+                           cv2.FONT_HERSHEY_SIMPLEX, 0.7, (255, 0, 0), 2)
+            
+            # 绘制激光点
+            if laser_point:
+                cv2.circle(img_cv, (int(laser_point[0]), int(laser_point[1])), 6, (0, 0, 255), -1)
+                cv2.putText(img_cv, "LASER", (int(laser_point[0]) + 10, int(laser_point[1]) - 10),
+                           cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2)
+            
+            # 转换回MaixPy图像
+            return image.cv2image(img_cv, False, False)
+            
+        except Exception as e:
+            if self.logger:
+                self.logger.error(f"[ARUCO] 绘制调试信息失败: {e}")
+            return frame
+
+
+# 创建全局单例实例
+aruco_detector = ArUcoDetector()
+
+
+def detect_target_with_aruco(frame, laser_point=None):
+    """
+    使用ArUco标记检测靶心的便捷函数
+    
+    Args:
+        frame: MaixPy图像帧
+        laser_point: 激光点坐标（可选）
+        
+    Returns:
+        (result_img, center, radius, method, best_radius1, ellipse_params)
+        与detect_circle_v3保持相同的返回格式
+    """
+    detector = aruco_detector
+    
+    # 检测ArUco标记
+    corners, ids, rejected = detector.detect_markers(frame)
+    
+    # 计算靶心
+    center, radius, method, ellipse_params = detector.get_target_center_from_markers(corners, ids)
+    
+    # 绘制调试信息
+    result_img = detector.draw_debug_info(frame, corners, ids, center, laser_point)
+    
+    # 返回与detect_circle_v3相同的格式
+    # best_radius1用于距离估算，这里用radius代替
+    return result_img, center, radius, method, radius, ellipse_params
+
+
+def compute_laser_offset_aruco(laser_point, corners, ids):
+    """
+    使用ArUco计算激光点相对于靶心的偏移（毫米）
+    
+    Args:
+        laser_point: (x, y) 激光点图像坐标
+        corners: ArUco标记角点
+        ids: ArUco标记ID
+        
+    Returns:
+        (dx_mm, dy_mm) 偏移量（毫米），或 (None, None)
+    """
+    return aruco_detector.transform_laser_point(laser_point, corners, ids)