update laser cabration

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)