diff --git a/app.yaml b/app.yaml index 21b2c21..c8e5878 100644 --- a/app.yaml +++ b/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 diff --git a/config.py b/config.py index f65e0f4..9dfb6b3 100644 --- a/config.py +++ b/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 相同 diff --git a/laser_detector.py b/laser_detector.py index 1a392f2..8e9a6db 100644 --- a/laser_detector.py +++ b/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( - 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)) + return _red_weighted_centroid( + img_cv[:, :, 0], img_cv[:, :, 1], img_cv[:, :, 2], + mask_ellipse > 0, 0, 0 + ) 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 - best_pos = None - for y in range(y1, y2, 2): - for x in range(x1, x2, 2): + rs, gs, bs = [], [], [] + xs, ys = [], [] + step = 2 + for y in range(y1, y2, step): + for x in range(x1, x2, step): idx = (y * WIDTH + x) * 3 - r = data[idx]; - g = data[idx + 1]; + 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: + rs.append(r) + gs.append(g) + bs.append(b) + xs.append(x) + ys.append(y) + if not rs: return None - 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 + rs = np.array(rs, dtype=np.float64) + gs = np.array(gs, dtype=np.float64) + bs = np.array(bs, dtype=np.float64) + xs = np.array(xs, dtype=np.float64) + ys = np.array(ys, dtype=np.float64) + w = rs - np.maximum(gs, bs) + w = np.clip(w, 0, None) + w = w * w + total_w = w.sum() + if total_w < 1e-6: + return None + cx_f = (xs * w).sum() / total_w + cy_f = (ys * w).sum() / total_w + return (float(cx_f), float(cy_f)) + + +def _ema_filter(pos, alpha=_EMA_ALPHA): + global _prev_smoothed + if _prev_smoothed is None: + _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: - stable += 1 + if not _gated(pos): + 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])) - time.sleep_ms(500) + result = (int(filtered[0]), int(filtered[1])) + _prev_smoothed = None + return result + time.sleep_ms(_FRAME_INTERVAL_MS) finally: - pass + _prev_smoothed = None diff --git a/laser_manager.py b/laser_manager.py index 594e0ea..2d5d148 100644 --- a/laser_manager.py +++ b/laser_manager.py @@ -54,8 +54,8 @@ class LaserManager: @property def laser_point(self): """当前激光点（如果启用硬编码，则返回硬编码值）""" - if config.HARDCODE_LASER_POINT: - return config.HARDCODE_LASER_POINT_VALUE + # if config.HARDCODE_LASER_POINT: + # return config.HARDCODE_LASER_POINT_VALUE return self._laser_point def get_last_frame_with_ellipse(self):