pref: laser find center point

app.yaml

@@ -1,6 +1,6 @@
 id: t11
 name: t11
-version: 2.14.1
+version: 2.15.1
 author: t11
 icon: ''
 desc: t11
@@ -14,6 +14,7 @@ files:
   - cameraParameters.xml
   - config.py
   - hardware.py
+  - laser_detector.py
   - laser_manager.py
   - logger_manager.py
   - main.py

config.py

@@ -134,7 +134,7 @@ IMAGE_CENTER_Y = 240  # 图像中心 Y 坐标
 # ==================== 三角形四角标记：单应性偏移 + PnP 估距 ====================
 # 依赖 cameraParameters.xml（相机内参）与 triangle_positions.json（四角物方坐标，厘米或毫米见 JSON 约定）。
 # 部署时请把这两个文件放到 APP_DIR（与 main 同应用目录），或改下面路径为设备上的实际绝对路径。
-USE_TRIANGLE_OFFSET = True  # False 时仅走黄心圆/椭圆 + 半径估距，不使用三角形路径
+USE_TRIANGLE_OFFSET = False  # False 时仅走黄心圆/椭圆 + 半径估距，不使用三角形路径
 CAMERA_CALIB_XML = APP_DIR + "/cameraParameters.xml"
 TRIANGLE_POSITIONS_JSON = APP_DIR + "/triangle_positions.json"
 # 检测到的三角形边长在图像中的像素范围，分辨率或靶纸占比变化时可微调
@@ -309,7 +309,7 @@ LASER_THICKNESS = 1
 LASER_LENGTH = 2
 
 # ==================== 图像保存配置 ====================
-SAVE_IMAGE_ENABLED = True  # 是否保存图像（True=保存，False=不保存）
+SAVE_IMAGE_ENABLED = False  # 是否保存图像（True=保存，False=不保存）
 PHOTO_DIR = "/root/phot"    # 照片存储目录
 MAX_IMAGES = 1000
 # Stage2 调试目录（默认 PHOTO_DIR/stage2_roi）内 JPEG 最多保留张数；None 表示与 MAX_IMAGES 相同

laser_detector.py

@@ -14,9 +14,34 @@ except ImportError:
 WIDTH = 640
 HEIGHT = 480
 THRESHOLD = 100
-RED_RATIO = 1.3
+RED_RATIO = 1
 SEARCH_RADIUS = 60
-STABLE_COUNT = 5
+STABLE_COUNT = 2
+
+# Temporal smoothing
+_EMA_ALPHA = 0.4
+_GATE_PX = 10
+_FRAME_INTERVAL_MS = 50
+
+_prev_smoothed = None
+
+
+def _red_weighted_centroid(r_ch, g_ch, b_ch, mask, x0, y0):
+    y_ids, x_ids = np.where(mask)
+    if len(y_ids) == 0:
+        return None
+    r_vals = r_ch[y_ids, x_ids].astype(np.float64)
+    g_vals = g_ch[y_ids, x_ids].astype(np.float64)
+    b_vals = b_ch[y_ids, x_ids].astype(np.float64)
+    w = r_vals - np.maximum(g_vals, b_vals)
+    w = np.clip(w, 0, None)
+    w = w * w
+    total_w = w.sum()
+    if total_w < 1e-6:
+        return None
+    cx = (x_ids.astype(np.float64) * w).sum() / total_w + x0
+    cy = (y_ids.astype(np.float64) * w).sum() / total_w + y0
+    return (float(cx), float(cy))
 
 
 def find_ellipse(img_cv, cx, cy, roi_r, th, ratio):
@@ -43,27 +68,15 @@ def find_ellipse(img_cv, cx, cy, roi_r, th, ratio):
     for pt in cnt:
         pt[0][0] += x1
         pt[0][1] += y1
-    if len(cnt) >= 5:
+    ellipse_valid = len(cnt) >= 5
+    if ellipse_valid:
         (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(
+        return _red_weighted_centroid(
-            np.int32)
+            img_cv[:, :, 0], img_cv[:, :, 1], img_cv[:, :, 2],
-        masked = np.where(mask_ellipse > 0, brightness, 0)
+            mask_ellipse > 0, 0, 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"]))
@@ -76,55 +89,71 @@ def find_brightest_bytes(frame, cx, cy, roi_r, th, ratio):
     y1 = max(0, cy - roi_r)
     y2 = min(HEIGHT, cy + roi_r)
     data = frame.to_bytes()
-    best_score = 0
+    rs, gs, bs = [], [], []
-    best_pos = None
+    xs, ys = [], []
-    for y in range(y1, y2, 2):
+    step = 2
-        for x in range(x1, x2, 2):
+    for y in range(y1, y2, step):
+        for x in range(x1, x2, step):
             idx = (y * WIDTH + x) * 3
-            r = data[idx];
+            r = data[idx]
-            g = data[idx + 1];
+            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
+                rs.append(r)
-                dx = x - cx;
+                gs.append(g)
-                dy = y - cy
+                bs.append(b)
-                score *= max(0.5, 1.0 - ((dx * dx + dy * dy) ** 0.5 / roi_r) * 0.5)
+                xs.append(x)
-                if score > best_score:
+                ys.append(y)
-                    best_score = score
+    if not rs:
-                    best_pos = (x, y)
-    if best_pos is None:
         return None
-    fx, fy = best_pos
+    rs = np.array(rs, dtype=np.float64)
-    x1f = max(0, fx - 3);
+    gs = np.array(gs, dtype=np.float64)
-    x2f = min(WIDTH, fx + 4)
+    bs = np.array(bs, dtype=np.float64)
-    y1f = max(0, fy - 3);
+    xs = np.array(xs, dtype=np.float64)
-    y2f = min(HEIGHT, fy + 4)
+    ys = np.array(ys, dtype=np.float64)
-    best_bright = 0
+    w = rs - np.maximum(gs, bs)
-    final_pos = best_pos
+    w = np.clip(w, 0, None)
-    for y in range(y1f, y2f):
+    w = w * w
-        for x in range(x1f, x2f):
+    total_w = w.sum()
-            idx = (y * WIDTH + x) * 3
+    if total_w < 1e-6:
-            r = data[idx];
+        return None
-            g = data[idx + 1];
+    cx_f = (xs * w).sum() / total_w
-            b = data[idx + 2]
+    cy_f = (ys * w).sum() / total_w
-            if (r > th and r > g * ratio and r > b * ratio) or \
+    return (float(cx_f), float(cy_f))
-                    (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:
+def _ema_filter(pos, alpha=_EMA_ALPHA):
-                    best_bright = rgb_sum
+    global _prev_smoothed
-                    final_pos = (float(x), float(y))
+    if _prev_smoothed is None:
-    return final_pos
+        _prev_smoothed = pos
+        return pos
+    sx = alpha * pos[0] + (1 - alpha) * _prev_smoothed[0]
+    sy = alpha * pos[1] + (1 - alpha) * _prev_smoothed[1]
+    _prev_smoothed = (sx, sy)
+    return _prev_smoothed
+
+
+def _gated(pos, gate_px=_GATE_PX):
+    global _prev_smoothed
+    if _prev_smoothed is None:
+        return True
+    dx = pos[0] - _prev_smoothed[0]
+    dy = pos[1] - _prev_smoothed[1]
+    return (dx * dx + dy * dy) <= gate_px * gate_px
 
 
 def get_stable_laser_point(timeout_ms=15000, stable_count=STABLE_COUNT):
+    global _prev_smoothed
+    _prev_smoothed = None
     try:
-        last_pos = None
+        last_raw = None
         stable = 0
         start = time.ticks_ms()
         cx, cy = WIDTH // 2, HEIGHT // 2
         while True:
             if abs(time.ticks_diff(time.ticks_ms(), start)) > timeout_ms:
+                _prev_smoothed = None
                 return None
             frame = camera_manager.read_frame()
             if frame is None:
@@ -132,21 +161,34 @@ def get_stable_laser_point(timeout_ms=15000, stable_count=STABLE_COUNT):
                 continue
             pos_bright = find_brightest_bytes(frame, cx, cy, SEARCH_RADIUS, THRESHOLD, RED_RATIO)
             pos = pos_bright
-            if logger_manager.logger:
-                logger_manager.logger.info(f"pos:{pos},stable:{stable}")
             if _USE_CV:
                 img_cv = image.image2cv(frame, False, False)
                 pos_ellipse = find_ellipse(img_cv, cx, cy, SEARCH_RADIUS, THRESHOLD, RED_RATIO)
                 if pos_ellipse is not None:
                     pos = pos_ellipse
             if pos is not None:
-                if last_pos and abs(pos[0] - last_pos[0]) < 1 and abs(pos[1] - last_pos[1]) < 1:
+                if not _gated(pos):
-                    stable += 1
+                    if logger_manager.logger:
+                        logger_manager.logger.info(f"pos:{pos} gated,stable:{stable}")
+                    time.sleep_ms(_FRAME_INTERVAL_MS)
+                    continue
+                filtered = _ema_filter(pos)
+                if last_raw is not None:
+                    dx = abs(filtered[0] - last_raw[0])
+                    dy = abs(filtered[1] - last_raw[1])
+                    if dx <= 2 and dy <= 2:
+                        stable += 1
+                    else:
+                        stable = 1
                 else:
                     stable = 1
-                last_pos = pos
+                last_raw = filtered
+                if logger_manager.logger:
+                    logger_manager.logger.info(f"pos:{pos},filtered:{filtered},stable:{stable}")
                 if stable >= stable_count:
-                    return (int(pos[0]), int(pos[1]))
+                    result = (int(filtered[0]), int(filtered[1]))
-            time.sleep_ms(500)
+                    _prev_smoothed = None
+                    return result
+            time.sleep_ms(_FRAME_INTERVAL_MS)
     finally:
-        pass
+        _prev_smoothed = None

laser_manager.py

@@ -54,8 +54,8 @@ class LaserManager:
     @property
     def laser_point(self):
         """当前激光点（如果启用硬编码，则返回硬编码值）"""
-        if config.HARDCODE_LASER_POINT:
+        # if config.HARDCODE_LASER_POINT:
-            return config.HARDCODE_LASER_POINT_VALUE
+        #     return config.HARDCODE_LASER_POINT_VALUE
         return self._laser_point
     
     def get_last_frame_with_ellipse(self):