1572 lines
79 KiB
Python
1572 lines
79 KiB
Python
# -*- coding: utf-8 -*-
|
||
"""
|
||
YOLOv8 单类分割 + MediaPipe Hand Landmarker + 光流追踪（多边形）
|
||
更新点（本版重点）：
|
||
- 左下角第二个进度条"距离(≈1)" 已完全替换为：ratio = 物体面积 / 手面积 的"接近 1 程度"可视化
|
||
-> range_score = 1 - clamp(|ratio - 1| / RATIO_TOL, 0..1)
|
||
-> 画面同时显示 ratio 数值；ratio<1 提示"向前靠近"，ratio>1 提示"后退"，在 [1±RATIO_TOL] 内为"保持"
|
||
其他特性：
|
||
- Enter 锁定：在分割掩码"内收 5px"的内边界上取光流点
|
||
- TRACK 期间：监控当前多边形外扩 40px 周边区域的分割，命中即重锁
|
||
- 成功判定：放宽"握持(Grasp)"启发式（拿瓶子无需特别紧）
|
||
- 手骨架单色渲染；测距箭头（端点定位线 + 箭头 + 像素值）
|
||
- 中文绘制优先 Pillow + 系统中文字体（避免问号）
|
||
"""
|
||
|
||
import os
|
||
import time
|
||
import threading
|
||
import math
|
||
import cv2
|
||
import numpy as np
|
||
import mediapipe as mp
|
||
from mediapipe.framework.formats import landmark_pb2
|
||
from ultralytics import YOLO
|
||
from ultralytics.utils.plotting import Colors
|
||
import bridge_io
|
||
|
||
# Day 26: 抑制 pygame 社区欢迎信息
|
||
import os
|
||
os.environ['PYGAME_HIDE_SUPPORT_PROMPT'] = "1"
|
||
import pygame # 用于播放本地音频文件
|
||
|
||
from audio_player import play_audio_threadsafe
|
||
PERF_DEBUG = False # 打印调试信息（False 关闭）
|
||
HAND_DOWNSCALE = 0.8 # HandLandmarker 的输入缩放 0.5=长宽各减半（≈1/4 像素量）
|
||
HAND_FPS_DIV = 1 # 人手每 2 帧跑一次（1=每帧；2=隔帧；3=每3帧）
|
||
|
||
|
||
# === 前端风格配色（BGR） + UI叠加管理（左下角按行堆叠） ===
|
||
FRONTEND_COLORS = {
|
||
"text": (230, 237, 243), # --text: #e6edf3
|
||
"muted": (159, 176, 195), # --muted: #9fb0c3
|
||
"ok": (126, 231, 135), # --ok: #7ee787
|
||
"err": (128, 128, 255), # --err: #ff8080 (BGR)
|
||
"accent": (251, 218, 97), # #61dafb 近似的强调色（BGR 取近似亮色）
|
||
}
|
||
|
||
# 底部指令按钮文本
|
||
CURRENT_COMMAND_TEXT = "—"
|
||
|
||
_UI_LINE = 0
|
||
_UI_H = 0
|
||
_UI_TR_LINE = 0 # 右上角逐行叠放计数
|
||
_UI_TOP_MARGIN = 12
|
||
_UI_RIGHT_MARGIN = 12
|
||
UNIFIED_FONT_PX = 12 # 统一字号
|
||
|
||
|
||
def ui_reset_overlay(img_h: int):
|
||
"""每帧调用一次，重置叠加行计数（改为右上角布局）。"""
|
||
global _UI_LINE, _UI_H, _UI_TR_LINE
|
||
_UI_LINE = 0
|
||
_UI_TR_LINE = 0
|
||
_UI_H = int(img_h)
|
||
|
||
|
||
def _ui_next_y_top(font_size: int) -> int:
|
||
"""返回右上角下一行的y(顶部对齐)，并推进行计数。"""
|
||
global _UI_TR_LINE
|
||
line_gap = max(4, int(font_size * 0.25))
|
||
y_top = _UI_TOP_MARGIN + (_UI_TR_LINE * (font_size + line_gap))
|
||
_UI_TR_LINE += 1
|
||
return y_top
|
||
|
||
|
||
def set_current_command(text: str):
|
||
global CURRENT_COMMAND_TEXT
|
||
try:
|
||
CURRENT_COMMAND_TEXT = str(text) if text else "—"
|
||
except Exception:
|
||
CURRENT_COMMAND_TEXT = "—"
|
||
|
||
|
||
def draw_command_pill(img_bgr: np.ndarray, label: str):
|
||
"""统一改为右上角白色文案。不再绘制底部圆角按钮。"""
|
||
text_prefix = "当前指令："
|
||
full_text = f"{text_prefix}{label if label else '—'}"
|
||
# 直接用统一文本渲染
|
||
draw_text_cn(img_bgr, full_text, (0, 0), font_size=UNIFIED_FONT_PX, color=(255,255,255), ui_hint=True)
|
||
|
||
try:
|
||
from yoloe_backend import YoloEBackend
|
||
_YOLOE_READY = True
|
||
except Exception as e:
|
||
_YOLOE_READY = False
|
||
print(f"[DETECTOR] YOLOE backend not ready: {e}", flush=True)
|
||
|
||
# ========= 路径参数（按需修改）=========
|
||
YOLO_MODEL_PATH = 'model/shoppingbest5.pt'
|
||
HAND_TASK_PATH = 'model/hand_landmarker.task'
|
||
|
||
# ========= 摄像头 =========
|
||
CAM_INDEX = 0
|
||
INPUT_W, INPUT_H = 600, 480
|
||
|
||
# ========= 分割显示 =========
|
||
STROKE_WIDTH = 5 # 增加描边宽度，让黄框和绿框更粗
|
||
MASK_ALPHA = 0.45
|
||
CONF_THRESHOLD = 0.20
|
||
|
||
# —— 单 prompt 识别（只显示一个类）——
|
||
PROMPT_NAME = "AD_milk"
|
||
PROMPT_STRICT = True
|
||
|
||
# ========= 对齐条参数 =========
|
||
ALIGN_LOOSE_PCT = 0.12 # 归一化距离阈（相对画面对角线）
|
||
|
||
# ========= 距离条参数（本版采用"ratio≈1"为目标）=========
|
||
RATIO_IDEAL = 1.0 # 理想值：物体面积/手面积 ≈ 1
|
||
RATIO_TOL = 0.25 # 容许偏离：±25% 内认为距离合适
|
||
|
||
# ========= 语音播报 =========
|
||
TTS_INTERVAL_SEC = 1.0
|
||
ENABLE_TTS = True
|
||
|
||
# ========= 光流（LK）与特征点 =========
|
||
LK_PARAMS = dict(winSize=(21, 21),
|
||
maxLevel=3,
|
||
criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 12, 0.03))
|
||
FEATURE_PARAMS = dict(maxCorners=600,
|
||
qualityLevel=0.001,
|
||
minDistance=5,
|
||
blockSize=7)
|
||
|
||
# ========= 关键参数：内收与周边监控 =========
|
||
INNER_OFFSET_PX_LOCK = 5 # Enter 锁定：掩码腐蚀像素，保证点在物体内部
|
||
EDGE_DILATE_PX = 2 # 取内边界后小膨胀，利于提点
|
||
PERI_MONITOR_PX = 40 # TRACK：监控多边形外扩 40px 的周边带
|
||
PERI_CHECK_EVERY = 5 # 每隔 N 帧做一次周边分割检查，改为每帧
|
||
|
||
# ========= 轮廓精度参数 =========
|
||
CONTOUR_EPSILON_FACTOR = 0.002 # Douglas-Peucker算法的精度因子，越小越精细
|
||
TRACK_EPSILON_FACTOR = 0.003 # 追踪模式下的轮廓精度因子
|
||
|
||
# ========= YOLO实时矫正参数 =========
|
||
YOLO_CORRECTION_IOU_THRESHOLD = 0.2 # IoU阈值，越低越积极矫正
|
||
YOLO_CORRECTION_CONF_THRESHOLD = 0.15 # 置信度阈值，越低检测越敏感
|
||
|
||
# ========= 方向引导音频路径 =========
|
||
AUDIO_DIR = os.path.join(os.path.dirname(os.path.abspath(__file__)), "music") # 相对路径
|
||
AUDIO_FILES = {
|
||
"向上": os.path.join(AUDIO_DIR, "向上.wav"),
|
||
"向下": os.path.join(AUDIO_DIR, "向下.wav"),
|
||
"向左": os.path.join(AUDIO_DIR, "向左.wav"),
|
||
"向右": os.path.join(AUDIO_DIR, "向右.wav"),
|
||
"向前": os.path.join(AUDIO_DIR, "向前.wav"),
|
||
"后退": os.path.join(AUDIO_DIR, "向后.wav"),
|
||
"OK": os.path.join(AUDIO_DIR, "已对中.wav"),
|
||
}
|
||
GUIDANCE_INTERVAL_SEC = 1.5 # 引导播报间隔
|
||
|
||
# 初始化pygame音频
|
||
pygame.mixer.init()
|
||
|
||
# ========= 窗口 =========
|
||
WINDOW = "YOLO Seg + Flow Polygon (Peri-Relock) (Grab Guidance)"
|
||
|
||
# ======== MediaPipe 别名 ========
|
||
BaseOptions = mp.tasks.BaseOptions
|
||
VisionRunningMode = mp.tasks.vision.RunningMode
|
||
HandLandmarker = mp.tasks.vision.HandLandmarker
|
||
HandLandmarkerOptions = mp.tasks.vision.HandLandmarkerOptions
|
||
HAND_CONNECTIONS = mp.solutions.hands.HAND_CONNECTIONS
|
||
|
||
# ======== HandLandmarker 回调缓存 ========
|
||
_last_result = None # (result, timestamp_ms)
|
||
|
||
def on_result(result: mp.tasks.vision.HandLandmarkerResult,
|
||
output_image: mp.Image, timestamp_ms: int):
|
||
global _last_result
|
||
_last_result = (result, timestamp_ms)
|
||
|
||
def _to_proto(hand_lms) -> landmark_pb2.NormalizedLandmarkList:
|
||
proto = landmark_pb2.NormalizedLandmarkList()
|
||
proto.landmark.extend([
|
||
landmark_pb2.NormalizedLandmark(x=p.x, y=p.y, z=p.z) for p in hand_lms
|
||
])
|
||
return proto
|
||
|
||
# —— 手骨架单色渲染 —— #
|
||
def draw_hands_mono(img_bgr, hand_lms, color=(0, 255, 255), r=2, t=2):
|
||
mp_drawing = mp.solutions.drawing_utils
|
||
landmark_spec = mp_drawing.DrawingSpec(color=color, thickness=-1, circle_radius=r)
|
||
connection_spec = mp_drawing.DrawingSpec(color=color, thickness=t, circle_radius=r)
|
||
if hasattr(hand_lms, "landmark"):
|
||
proto = hand_lms
|
||
else:
|
||
proto = _to_proto(hand_lms)
|
||
mp_drawing.draw_landmarks(
|
||
img_bgr,
|
||
landmark_list=proto,
|
||
connections=HAND_CONNECTIONS,
|
||
landmark_drawing_spec=landmark_spec,
|
||
connection_drawing_spec=connection_spec,
|
||
)
|
||
|
||
def norm_name(s: str) -> str:
|
||
return "".join(str(s).lower().split())
|
||
|
||
# ======== TTS（pyttsx3）========
|
||
class Speaker:
|
||
def __init__(self, enable=True):
|
||
self.enable = enable
|
||
self._engine = None
|
||
self._lock = threading.Lock()
|
||
if enable:
|
||
try:
|
||
import pyttsx3
|
||
self._engine = pyttsx3.init()
|
||
self._engine.setProperty('rate', 190)
|
||
self._engine.setProperty('volume', 1.0)
|
||
except Exception:
|
||
self._engine = None
|
||
self.enable = False
|
||
|
||
def say_async(self, text: str):
|
||
if not self.enable or not text:
|
||
return
|
||
def _run():
|
||
try:
|
||
with self._lock:
|
||
self._engine.stop()
|
||
self._engine.say(text)
|
||
self._engine.iterate()
|
||
t0 = time.time()
|
||
while self._engine.isBusy() and (time.time() - t0) < 1.2:
|
||
self._engine.iterate()
|
||
time.sleep(0.01)
|
||
except Exception:
|
||
pass
|
||
threading.Thread(target=_run, daemon=True).start()
|
||
|
||
# ======== 中文文本绘制（优先 Pillow）========
|
||
_PIL_OK = False
|
||
_FONT_PATH = None
|
||
def _init_font():
|
||
global _PIL_OK, _FONT_PATH
|
||
try:
|
||
from PIL import ImageFont # noqa
|
||
_PIL_OK = True
|
||
except Exception:
|
||
_PIL_OK = False
|
||
return
|
||
candidates = [
|
||
# Linux 中文字体路径 (Ubuntu/Debian)
|
||
"/usr/share/fonts/truetype/wqy/wqy-zenhei.ttc",
|
||
"/usr/share/fonts/truetype/wqy/wqy-microhei.ttc",
|
||
"/usr/share/fonts/opentype/noto/NotoSansCJK-Regular.ttc",
|
||
"/usr/share/fonts/truetype/noto/NotoSansCJK-Regular.ttc",
|
||
"/usr/share/fonts/truetype/droid/DroidSansFallbackFull.ttf",
|
||
]
|
||
for p in candidates:
|
||
if os.path.exists(p):
|
||
_FONT_PATH = p
|
||
return
|
||
_PIL_OK = False
|
||
_init_font()
|
||
|
||
def draw_text_cn(img_bgr, text, xy, font_size=20, color=(255,255,255), stroke=None, ui_hint=True):
|
||
"""
|
||
统一的文本绘制：
|
||
- 默认采用前端风格：小字体、左下角按行堆叠(ui_hint=True)。
|
||
- 若 ui_hint=False 则按传入 xy 精确定位（用于贴近目标的小标注）。
|
||
"""
|
||
# 统一样式：微软雅黑 + 固定字号 + 纯白
|
||
color = (255, 255, 255)
|
||
font_size = int(UNIFIED_FONT_PX)
|
||
|
||
H, W = img_bgr.shape[:2]
|
||
# 右上角堆叠布局：计算y顶边，并按文本宽度右对齐
|
||
y_top = _ui_next_y_top(font_size) if ui_hint else _ui_next_y_top(font_size)
|
||
# 先估算文本尺寸
|
||
tw = th = 0
|
||
font_obj = None
|
||
|
||
if _PIL_OK and _FONT_PATH:
|
||
try:
|
||
from PIL import Image, ImageDraw, ImageFont
|
||
font_obj = ImageFont.truetype(_FONT_PATH, font_size)
|
||
# 计算文本尺寸
|
||
bbox = ImageDraw.Draw(Image.new('RGB', (1,1))).textbbox((0,0), text, font=font_obj)
|
||
tw = max(1, bbox[2] - bbox[0])
|
||
th = max(1, bbox[3] - bbox[1])
|
||
except Exception:
|
||
pass
|
||
if _PIL_OK and _FONT_PATH and font_obj is not None:
|
||
try:
|
||
from PIL import Image, ImageDraw
|
||
img_rgb = cv2.cvtColor(img_bgr, cv2.COLOR_BGR2RGB)
|
||
pil_img = Image.fromarray(img_rgb)
|
||
draw = ImageDraw.Draw(pil_img)
|
||
x = max(8, W - _UI_RIGHT_MARGIN - tw)
|
||
y = y_top
|
||
draw.text((x, y), text, fill=(255,255,255), font=font_obj)
|
||
img_bgr[:] = cv2.cvtColor(np.asarray(pil_img), cv2.COLOR_RGB2BGR)
|
||
return
|
||
except Exception:
|
||
pass
|
||
# OpenCV 回退：估算尺寸并右对齐
|
||
if tw <= 0 or th <= 0:
|
||
scale = font_size/24.0
|
||
(tw, th), _ = cv2.getTextSize(text, cv2.FONT_HERSHEY_SIMPLEX, scale, 2)
|
||
x = max(8, W - _UI_RIGHT_MARGIN - int(tw))
|
||
y_baseline = int(y_top + th)
|
||
cv2.putText(img_bgr, text, (x, y_baseline), cv2.FONT_HERSHEY_SIMPLEX, font_size/24.0, color, 2, cv2.LINE_AA)
|
||
|
||
# ======== 工具函数 ========
|
||
def clamp01(x): return max(0.0, min(1.0, x))
|
||
|
||
def draw_progress_bars(vis, align_score, range_score):
|
||
"""第一条=对齐，第二条=距离(≈1)，对应 ratio 与 1 的接近程度"""
|
||
H, W = vis.shape[:2]
|
||
bar_w = int(W * 0.28)
|
||
bar_h = 12
|
||
gap = 8
|
||
x0 = 12
|
||
y0 = H - 2*bar_h - gap - 12
|
||
# 背景
|
||
cv2.rectangle(vis, (x0, y0), (x0 + bar_w, y0 + bar_h), (50, 50, 50), -1)
|
||
cv2.rectangle(vis, (x0, y0 + bar_h + gap), (x0 + bar_w, y0 + 2*bar_h + gap), (50, 50, 50), -1)
|
||
# 填充
|
||
cv2.rectangle(vis, (x0, y0), (x0 + int(bar_w * clamp01(align_score)), y0 + bar_h), (0, 220, 0), -1)
|
||
cv2.rectangle(vis, (x0, y0 + bar_h + gap), (x0 + int(bar_w * clamp01(range_score)), y0 + 2*bar_h + gap), (0, 180, 255), -1)
|
||
draw_text_cn(vis, "对齐", (x0, y0 - 18), font_size=18, color=(180,180,180))
|
||
draw_text_cn(vis, "距离(≈1)", (x0, y0 + bar_h + gap - 18), font_size=18, color=(180,180,180))
|
||
|
||
def polygon_center_and_area(poly):
|
||
if poly is None or len(poly) < 3:
|
||
return None, 0.0
|
||
poly = np.array(poly, dtype=np.float32)
|
||
M = cv2.moments(poly)
|
||
if abs(M["m00"]) < 1e-6:
|
||
c = np.mean(poly, axis=0)
|
||
return (float(c[0]), float(c[1])), 0.0
|
||
cx = float(M["m10"] / M["m00"])
|
||
cy = float(M["m01"] / M["m00"])
|
||
area = float(cv2.contourArea(poly.astype(np.int32)))
|
||
return (cx, cy), area
|
||
|
||
def hand_bbox_and_area(lms, W, H):
|
||
xs = [int(p.x * W) for p in lms]
|
||
ys = [int(p.y * H) for p in lms]
|
||
if not xs or not ys:
|
||
return None, 0.0
|
||
x0, y0, x1, y1 = min(xs), min(ys), max(xs), max(ys)
|
||
w = max(1, x1 - x0)
|
||
h = max(1, y1 - y0)
|
||
area = float(w * h)
|
||
return (x0, y0, w, h), area
|
||
|
||
# ======== 手势：握持(Grasp) 识别（放宽版启发式）========
|
||
THUMB_INDEX_CLOSE = 0.34 # 放宽
|
||
FINGERTIP_NEAR = 0.44 # 放宽
|
||
MIN_CURLED_COUNT = 1 # 放宽
|
||
|
||
def detect_grasp(hand_lms, W, H):
|
||
box, _ = hand_bbox_and_area(hand_lms, W, H)
|
||
if not box:
|
||
return False, 0.0
|
||
x0, y0, w0, h0 = box
|
||
hand_diag = float(np.hypot(w0, h0)) + 1e-6
|
||
palm_idx = [0, 5, 9, 13, 17]
|
||
px = np.mean([hand_lms[i].x * W for i in palm_idx])
|
||
py = np.mean([hand_lms[i].y * H for i in palm_idx])
|
||
palm = np.array([px, py], dtype=np.float32)
|
||
t4 = np.array([hand_lms[4].x * W, hand_lms[4].y * H], dtype=np.float32)
|
||
t8 = np.array([hand_lms[8].x * W, hand_lms[8].y * H], dtype=np.float32)
|
||
thumb_index_dist = float(np.linalg.norm(t4 - t8)) / hand_diag
|
||
tips = [12, 16, 20]
|
||
dists = []
|
||
for i in tips:
|
||
ti = np.array([hand_lms[i].x * W, hand_lms[i].y * H], dtype=np.float32)
|
||
dists.append(float(np.linalg.norm(ti - palm)) / hand_diag)
|
||
curled_cnt = sum(1 for d in dists if d < FINGERTIP_NEAR)
|
||
cond1 = (thumb_index_dist < THUMB_INDEX_CLOSE)
|
||
cond2 = (curled_cnt >= MIN_CURLED_COUNT)
|
||
score = 0.5 * (1.0 - min(thumb_index_dist / THUMB_INDEX_CLOSE, 1.0)) + \
|
||
0.5 * min(curled_cnt / 3.0, 1.0)
|
||
return (cond1 and cond2), score
|
||
|
||
# ======== 内收后的边界提点 ========
|
||
def inner_offset_edge(mask_bin, offset_px=5, edge_dilate_px=2):
|
||
if offset_px > 0:
|
||
k = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (2*offset_px+1, 2*offset_px+1))
|
||
eroded = cv2.erode(mask_bin.astype(np.uint8), k, iterations=1)
|
||
else:
|
||
eroded = mask_bin.astype(np.uint8)
|
||
edges = cv2.Canny(eroded*255, 50, 150)
|
||
if edge_dilate_px > 0:
|
||
k2 = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (2*edge_dilate_px+1, 2*edge_dilate_px+1))
|
||
edges = cv2.dilate(edges, k2, iterations=1)
|
||
return edges # uint8 0/255
|
||
|
||
# ======== YOLO 分割：全帧或 ROI 内选择最佳 mask ========
|
||
def find_best_mask(frame_bgr, yolo, W, H, target_cls_id, conf_thr=0.10, roi_rect=None):
|
||
results = yolo(frame_bgr, verbose=False)
|
||
best_mask = None
|
||
best_score = 0.0
|
||
if results and results[0].masks is not None:
|
||
r0 = results[0]
|
||
for mask_t, conf_t, cls_t in zip(r0.masks.data, r0.boxes.conf, r0.boxes.cls):
|
||
cls_id = int(cls_t.item())
|
||
conf_value = float(conf_t.item())
|
||
if target_cls_id is not None and cls_id != target_cls_id:
|
||
continue
|
||
if conf_value < conf_thr:
|
||
continue
|
||
mask_np = mask_t.detach().cpu().numpy()
|
||
mask_rz = cv2.resize(mask_np, (W, H), interpolation=cv2.INTER_LINEAR)
|
||
mask_bin = (mask_rz > 0.5).astype(np.uint8)
|
||
|
||
if roi_rect is not None:
|
||
x0, y0, x1, y1 = roi_rect
|
||
x0, y0 = max(0, x0), max(0, y0)
|
||
x1, y1 = min(W-1, x1), min(H-1, y1)
|
||
roi = np.zeros_like(mask_bin, dtype=np.uint8)
|
||
roi[y0:y1+1, x0:x1+1] = 1
|
||
overlap = (mask_bin & roi).sum()
|
||
score = float(overlap)
|
||
else:
|
||
score = float(mask_bin.sum())
|
||
|
||
if score > best_score:
|
||
best_score = score
|
||
best_mask = mask_bin
|
||
return best_mask
|
||
|
||
# ======== 工程化：测距箭头（端点定位线 + 箭头 + 像素值）========
|
||
def draw_measure_arrow(img, p1, p2, txt=None):
|
||
p1 = (int(p1[0]), int(p1[1]))
|
||
p2 = (int(p2[0]), int(p2[1]))
|
||
# 端点定位线
|
||
def end_cap(pt, size=8, color=(255,255,255), t=1):
|
||
x, y = pt
|
||
cv2.line(img, (x - size, y), (x + size, y), color, t, cv2.LINE_AA)
|
||
cv2.line(img, (x, y - size), (x, y + size), color, t, cv2.LINE_AA)
|
||
end_cap(p1, size=7, color=(255,255,255), t=1)
|
||
end_cap(p2, size=7, color=(255,255,255), t=1)
|
||
# 箭头
|
||
cv2.arrowedLine(img, p1, p2, (255,255,255), 2, cv2.LINE_AA, tipLength=0.18)
|
||
# 文本
|
||
if txt is None:
|
||
d = int(np.hypot(p2[0]-p1[0], p2[1]-p1[1]))
|
||
txt = f"{d}px"
|
||
mid = ((p1[0]+p2[0])//2, (p1[1]+p2[1])//2)
|
||
font = cv2.FONT_HERSHEY_SIMPLEX
|
||
fs, th = 0.6, 2
|
||
(tw, th_text), _ = cv2.getTextSize(txt, font, fs, th)
|
||
pad = 4
|
||
x0 = mid[0] - tw//2 - pad
|
||
y0 = mid[1] - th_text - 6
|
||
x1 = mid[0] + tw//2 + pad
|
||
y1 = mid[1] + 6
|
||
cv2.rectangle(img, (x0, y0), (x1, y1), (32,32,32), -1)
|
||
cv2.putText(img, txt, (x0+pad, y1-6), font, fs, (255,255,255), th, cv2.LINE_AA)
|
||
|
||
# 添加绘制虚线的函数
|
||
def draw_dashed_line(img, pt1, pt2, color=(255, 255, 255), thickness=2, dash_length=10, gap_length=5):
|
||
"""绘制虚线"""
|
||
pt1 = np.array(pt1, dtype=np.float32)
|
||
pt2 = np.array(pt2, dtype=np.float32)
|
||
line_vec = pt2 - pt1
|
||
line_len = np.linalg.norm(line_vec)
|
||
if line_len < 1:
|
||
return
|
||
|
||
line_vec = line_vec / line_len # 单位向量
|
||
|
||
# 绘制虚线段
|
||
current_pos = 0
|
||
while current_pos < line_len:
|
||
start_pos = current_pos
|
||
end_pos = min(current_pos + dash_length, line_len)
|
||
|
||
start_pt = pt1 + line_vec * start_pos
|
||
end_pt = pt1 + line_vec * end_pos
|
||
|
||
cv2.line(img, tuple(start_pt.astype(int)), tuple(end_pt.astype(int)), color, thickness)
|
||
|
||
current_pos += dash_length + gap_length
|
||
|
||
# 添加绘制手部轮廓的函数
|
||
def draw_hand_contour(img, hand_lms, W, H, color=(255, 255, 255), thickness=1):
|
||
"""绘制手部landmarks的凸包轮廓"""
|
||
# 获取所有手部关键点
|
||
points = []
|
||
for lm in hand_lms:
|
||
x = int(lm.x * W)
|
||
y = int(lm.y * H)
|
||
points.append([x, y])
|
||
|
||
if len(points) > 3:
|
||
points = np.array(points, dtype=np.int32)
|
||
# 计算凸包
|
||
hull = cv2.convexHull(points)
|
||
# 绘制凸包轮廓
|
||
cv2.polylines(img, [hull], True, color, thickness)
|
||
|
||
# 检测手和物体是否接触
|
||
def check_hand_object_contact(hand_box, poly, overlap_threshold=0.15):
|
||
"""
|
||
检测手的边界框和物体多边形是否有重叠
|
||
返回: (是否接触, 重叠比例)
|
||
"""
|
||
if hand_box is None or poly is None or len(poly) < 3:
|
||
return False, 0.0
|
||
|
||
# 获取手的边界框
|
||
hx, hy, hw, hh = hand_box
|
||
hand_rect = np.array([
|
||
[hx, hy],
|
||
[hx + hw, hy],
|
||
[hx + hw, hy + hh],
|
||
[hx, hy + hh]
|
||
], dtype=np.int32)
|
||
|
||
# 创建掩码来计算重叠
|
||
H = int(max(hy + hh, np.max(poly[:, 1])) + 10)
|
||
W = int(max(hx + hw, np.max(poly[:, 0])) + 10)
|
||
|
||
hand_mask = np.zeros((H, W), dtype=np.uint8)
|
||
cv2.fillPoly(hand_mask, [hand_rect], 1)
|
||
|
||
obj_mask = np.zeros((H, W), dtype=np.uint8)
|
||
cv2.fillPoly(obj_mask, [poly.astype(np.int32)], 1)
|
||
|
||
# 计算重叠
|
||
intersection = np.logical_and(hand_mask, obj_mask).sum()
|
||
hand_area = hand_mask.sum()
|
||
|
||
# 重叠比例（相对于手的面积）
|
||
overlap_ratio = intersection / max(1.0, hand_area)
|
||
|
||
return overlap_ratio > overlap_threshold, overlap_ratio
|
||
|
||
# 添加方向判断函数
|
||
def get_guidance_direction(hand_center, object_center, hand_area, object_area, hand_box=None, poly=None):
|
||
"""
|
||
根据手心和物体中心位置，以及面积比，返回引导方向
|
||
返回: (方向文字, 是否需要前后调整)
|
||
"""
|
||
if hand_center is None or object_center is None:
|
||
return None, None
|
||
|
||
# 首先检查手和物体是否接触
|
||
is_touching = False
|
||
overlap_ratio = 0.0
|
||
if hand_box is not None and poly is not None:
|
||
is_touching, overlap_ratio = check_hand_object_contact(hand_box, poly, overlap_threshold=0.1)
|
||
|
||
hx, hy = hand_center
|
||
ox, oy = object_center
|
||
|
||
# 计算水平和垂直偏差
|
||
dx = ox - hx # 正数表示物体在右边
|
||
dy = oy - hy # 正数表示物体在下边
|
||
|
||
# 如果手和物体已经接触，直接返回"向前"
|
||
if is_touching:
|
||
return "向前", f"接触度: {overlap_ratio:.1%}"
|
||
|
||
# 如果没有接触，引导上下左右
|
||
# 判断主要方向
|
||
h_threshold = 30 # 水平偏差阈值（像素）
|
||
v_threshold = 30 # 垂直偏差阈值（像素）
|
||
|
||
h_dir = None
|
||
v_dir = None
|
||
|
||
# 水平方向
|
||
if abs(dx) > h_threshold:
|
||
h_dir = "向右" if dx > 0 else "向左"
|
||
|
||
# 垂直方向
|
||
if abs(dy) > v_threshold:
|
||
v_dir = "向下" if dy > 0 else "向上"
|
||
|
||
# 选择偏移最大的方向
|
||
if abs(dx) > abs(dy) and h_dir:
|
||
# 水平偏移更大
|
||
return h_dir, v_dir
|
||
elif v_dir:
|
||
# 垂直偏移更大或相等
|
||
return v_dir, h_dir
|
||
else:
|
||
# 已经在中心附近但还没接触，提示靠近
|
||
distance = np.sqrt(dx**2 + dy**2)
|
||
if distance < 50: # 很近但还没接触
|
||
return "向前", "请缓慢靠近"
|
||
else:
|
||
return "保持", None
|
||
|
||
# 播放音频的函数
|
||
def play_guidance_audio(direction):
|
||
"""播放方向引导音频"""
|
||
# 直接调用新的音频播放函数
|
||
play_audio_threadsafe(direction)
|
||
# 同步更新底部按钮的指令文本
|
||
try:
|
||
if isinstance(direction, str) and direction.strip():
|
||
set_current_command(direction.strip())
|
||
except Exception:
|
||
pass
|
||
|
||
# 添加居中判断函数
|
||
def get_center_guidance(object_center, frame_center, threshold=30):
|
||
"""
|
||
判断物体是否在画面中心，返回引导方向
|
||
返回: (方向文字, 是否已居中)
|
||
"""
|
||
if object_center is None:
|
||
return None, False
|
||
|
||
ox, oy = object_center
|
||
cx, cy = frame_center
|
||
|
||
dx = cx - ox # 正数表示需要向右移动
|
||
dy = cy - oy # 正数表示需要向下移动
|
||
|
||
# 判断是否已经居中
|
||
distance = np.sqrt(dx**2 + dy**2)
|
||
if distance < threshold:
|
||
return "已居中", True
|
||
|
||
# 判断主要方向（对调左右和上下）
|
||
if abs(dx) > abs(dy):
|
||
return "向左" if dx > 0 else "向右", False # 对调了
|
||
else:
|
||
return "向上" if dy > 0 else "向下", False # 对调了
|
||
|
||
def main(headless: bool = False, prompt_name: str = None, stop_event=None):
|
||
|
||
# OpenCV 优化
|
||
try:
|
||
import cv2
|
||
cv2.setUseOptimized(True)
|
||
cv2.setNumThreads(2) # 视 CPU 核心数而定；树莓派类设备可设 1
|
||
except Exception:
|
||
pass
|
||
|
||
|
||
|
||
|
||
# 如果传入了 prompt_name，使用它替换全局的 PROMPT_NAME
|
||
global PROMPT_NAME
|
||
if prompt_name:
|
||
PROMPT_NAME = prompt_name
|
||
print(f"[YOLOMEDIA] Using dynamic prompt: {PROMPT_NAME}")
|
||
|
||
speaker = Speaker(ENABLE_TTS)
|
||
last_tts_ts = 0.0
|
||
MODE = "SEGMENT" # 模式：SEGMENT -> FLASH -> CENTER_GUIDE -> TRACK
|
||
colors = Colors()
|
||
|
||
FRAME_IDX = 0
|
||
last_mask = None # 上一帧"目标掩膜"（用于 IoU 降噪）
|
||
flow_mask = None # 光流外推得到的掩膜（你现有代码里会更新它）
|
||
flow_grace = 0 # YOLOE 丢检后，允许光流顶住的计数
|
||
last_seen_ts = 0.0 # 最近一次 YOLOE 成功检测的时间戳
|
||
locked_id = None # （可选）若你在 tracker 里记录了 id，可在下面选择相同 id
|
||
# 刷新/容错参数（可按需微调）
|
||
REDETECT_EVERY = 5 # 每 5 帧强制"信任 YOLOE 一次"
|
||
FLOW_GRACE_MAX = 8 # YOLOE 连续丢检时，光流最多顶 8 帧
|
||
IOU_MIN_KEEP = 0.20 # 新/旧掩膜 IoU 太低时，用平滑合成，避免闪烁
|
||
|
||
|
||
|
||
print("[INIT] 加载 YOLO 模型...")
|
||
# NOTE: shoppingbest 不再用于找东西流程；如其他模式仍需，可保留 yolo = YOLO(...) 但不在本流程使用
|
||
# yolo = YOLO(YOLO_MODEL_PATH)
|
||
|
||
# —— 直接启用 YOLOE 文本提示后端（不再先查 shoppingbest）——
|
||
use_yoloe = False
|
||
yoloe_backend = None
|
||
if _YOLOE_READY:
|
||
try:
|
||
yoloe_backend = YoloEBackend() # 可用 YOLOE_MODEL_PATH 环境变量指定模型
|
||
yoloe_backend.set_text_classes([PROMPT_NAME]) # 文本类别
|
||
use_yoloe = True
|
||
print(f"[DETECTOR] YOLOE text-prompt backend enabled for: {PROMPT_NAME}", flush=True)
|
||
except Exception as e:
|
||
print(f"[DETECTOR] YOLOE init failed: {e}", flush=True)
|
||
else:
|
||
print("[DETECTOR] YOLOE backend not ready (import failed)", flush=True)
|
||
|
||
# 类名映射（YOLOE 模式下简化）
|
||
if use_yoloe:
|
||
# YOLOE 模式下，只有一个目标类
|
||
id_to_name = {0: PROMPT_NAME}
|
||
name_to_id = {norm_name(PROMPT_NAME): 0}
|
||
target_cls_id = 0
|
||
else:
|
||
# 如果将来需要支持传统 YOLO，可以在这里初始化
|
||
id_to_name = {}
|
||
name_to_id = {}
|
||
target_cls_id = None
|
||
|
||
# 目标类已在上面的 YOLOE 模式中设置
|
||
|
||
print(f"[CLASS] target id={target_cls_id}, name={id_to_name.get(target_cls_id, 'N/A')}")
|
||
print(f"[阈值] conf >= {CONF_THRESHOLD:.2f}")
|
||
|
||
# Hand Landmarker
|
||
print("[INIT] 初始化 Hand Landmarker...")
|
||
base = BaseOptions(model_asset_path=HAND_TASK_PATH)
|
||
hand_options = HandLandmarkerOptions(
|
||
base_options=base,
|
||
running_mode=VisionRunningMode.LIVE_STREAM,
|
||
num_hands=1,
|
||
min_hand_detection_confidence=0.40,
|
||
min_hand_presence_confidence=0.50,
|
||
min_tracking_confidence=0.70,
|
||
result_callback=on_result
|
||
)
|
||
landmarker = HandLandmarker.create_from_options(hand_options)
|
||
|
||
W = None
|
||
H = None
|
||
print("[Bridge] 等待 ESP32 画面 ...")
|
||
|
||
# [headless] 仅在非 headless 时创建窗口（原逻辑保留，外层加判断）
|
||
if not headless:
|
||
cv2.namedWindow(WINDOW, cv2.WINDOW_NORMAL)
|
||
|
||
# 光流缓存
|
||
old_gray = None
|
||
p0 = None
|
||
lock_edge_debug = None # 调试可视化：内边界
|
||
track_frame_count = 0 # 控制周边监控频率
|
||
last_poly_box = None # 当前多边形外接矩形
|
||
|
||
fps_hist = []
|
||
|
||
# 添加自动锁定相关变量
|
||
auto_lock_start_time = None # 开始检测到物体的时间
|
||
auto_lock_delay = 1.0 # 1秒后自动锁定
|
||
last_detected_mask = None # 最后检测到的mask
|
||
|
||
# 添加闪烁动画相关变量
|
||
flash_start_time = None # 闪烁开始时间
|
||
flash_duration = 1.0 # 闪烁持续时间（秒）
|
||
flash_frequency = 1 # 闪烁频率（Hz） - 只闪一次
|
||
flash_mask = None # 用于闪烁的mask
|
||
flash_color = (0, 255, 255) # 闪烁颜色（黄色）
|
||
|
||
# 添加引导相关变量
|
||
last_guidance_time = 0
|
||
last_guidance_direction = None
|
||
|
||
# 添加居中引导相关变量
|
||
center_guide_mask = None # 用于居中引导的mask
|
||
center_guide_start = None # 居中引导开始时间
|
||
center_threshold = 30 # 居中判定阈值（像素）
|
||
last_center_guide_time = 0 # 上次居中引导语音时间
|
||
center_reached = False # 是否已经到达中心
|
||
|
||
# 添加抓取跟踪相关变量
|
||
grasp_tracking_frames = [] # 存储最近的手和物体位置
|
||
grasp_tracking_duration = 1.0 # 需要持续1秒
|
||
grasp_movement_threshold = 10 # 最小移动像素阈值（提高阈值）
|
||
grasp_detected = False # 是否已经检测到抓取
|
||
grasp_start_time = None # 开始检测到协同移动的时间
|
||
|
||
# 背景参考点（用于检测相机移动） - 移到这里初始化
|
||
background_points = None
|
||
old_background_gray = None
|
||
|
||
try:
|
||
while True:
|
||
# 检查停止事件
|
||
if stop_event and stop_event.is_set():
|
||
print("[YOLOMEDIA] Stop event detected, exiting...")
|
||
break
|
||
|
||
frame = bridge_io.wait_raw_bgr(timeout_sec=0.5)
|
||
if frame is None:
|
||
# 没取到帧就继续等（ESP32还没连上或暂时无新帧）
|
||
# [headless] 给出 1ms 让出调度，避免空转
|
||
if headless:
|
||
cv2.waitKey(1)
|
||
continue
|
||
|
||
# 每帧重置 UI 文字叠加到左下角
|
||
H, W = frame.shape[:2]
|
||
ui_reset_overlay(H)
|
||
|
||
vis = frame.copy()
|
||
t_now = time.time()
|
||
|
||
# 抽帧 + 降采样（人手识别）
|
||
if FRAME_IDX % HAND_FPS_DIV == 0:
|
||
rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
|
||
if HAND_DOWNSCALE and HAND_DOWNSCALE != 1.0:
|
||
small = cv2.resize(rgb, None, fx=HAND_DOWNSCALE, fy=HAND_DOWNSCALE, interpolation=cv2.INTER_AREA)
|
||
mp_image = mp.Image(image_format=mp.ImageFormat.SRGB, data=small)
|
||
else:
|
||
mp_image = mp.Image(image_format=mp.ImageFormat.SRGB, data=rgb)
|
||
landmarker.detect_async(mp_image, int(t_now * 1000))
|
||
# 否则跳过，复用上一次 _last_result；Landmarker 会自己做 tracking
|
||
|
||
|
||
# 取手心、手框、握持（放宽版）
|
||
hand_center = None
|
||
hand_area = None
|
||
hand_box = None
|
||
grasp_now = False
|
||
grasp_score = 0.0
|
||
if _last_result is not None:
|
||
res, _ = _last_result
|
||
if res.hand_landmarks and len(res.hand_landmarks) > 0:
|
||
l0 = res.hand_landmarks[0]
|
||
|
||
# 绘制手部骨骼
|
||
draw_hands_mono(vis, l0, color=(0, 255, 255), r=2, t=2)
|
||
|
||
# 绘制手部轮廓（替代矩形框）
|
||
draw_hand_contour(vis, l0, W, H, color=(255, 255, 255), thickness=1)
|
||
|
||
xs = [p.x * W for p in l0]
|
||
ys = [p.y * H for p in l0]
|
||
hand_center = (float(sum(xs)/len(xs)), float(sum(ys)/len(ys)))
|
||
hand_box, hand_area = hand_bbox_and_area(l0, W, H)
|
||
# 注释掉矩形框绘制
|
||
# if hand_box:
|
||
# x0, y0, w0, h0 = hand_box
|
||
# cv2.rectangle(vis, (x0, y0), (x0+w0, y0+h0), (0,255,255), 1)
|
||
grasp_now, grasp_score = detect_grasp(l0, W, H)
|
||
draw_text_cn(vis, f"握持评分: {grasp_score:.2f}", (10, 70), font_size=18, color=(0, 180, 255))
|
||
|
||
|
||
if MODE == "SEGMENT":
|
||
# —— 仅 YOLOE：每帧文本提示分割 + 取最大目标（删掉 shoppingbest 与重复 YOLOE 段）——
|
||
FRAME_IDX += 1
|
||
candidate_masks = []
|
||
detected_object = False
|
||
|
||
if use_yoloe and yoloe_backend is not None:
|
||
# 每帧都跑；persist=True 便于维持目标 ID
|
||
det = yoloe_backend.segment(frame, conf=0.20, iou=0.45, persist=True)
|
||
H, W = frame.shape[:2]
|
||
|
||
# 选一个掩膜：优先与 locked_id 相同；否则面积最大
|
||
chosen_idx = None
|
||
if det["masks"]:
|
||
if locked_id is not None and det["ids"] and (locked_id in det["ids"]):
|
||
chosen_idx = det["ids"].index(locked_id)
|
||
else:
|
||
areas = [int(m.sum()) for m in det["masks"]]
|
||
chosen_idx = int(np.argmax(areas))
|
||
|
||
if chosen_idx is not None:
|
||
m = det["masks"][chosen_idx]
|
||
if m.shape[:2] != (H, W):
|
||
m = cv2.resize(m, (W, H), interpolation=cv2.INTER_NEAREST)
|
||
|
||
mask_bin = (m > 0).astype(np.uint8)
|
||
candidate_masks.append({
|
||
"mask": mask_bin,
|
||
"area": int(mask_bin.sum()),
|
||
"name": PROMPT_NAME,
|
||
"cls_id": 0,
|
||
"conf": 0.99,
|
||
})
|
||
detected_object = True
|
||
|
||
# 简单可视化（半透明叠层 + 轮廓），不影响你后面的逻辑
|
||
colored = np.zeros_like(frame, dtype=np.uint8)
|
||
colored[mask_bin == 1] = (0, 255, 255)
|
||
vis = cv2.addWeighted(vis, 1.0, colored, MASK_ALPHA, 0)
|
||
contours, _ = cv2.findContours(mask_bin, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
|
||
if contours:
|
||
# 选择最大轮廓并进行适度平滑
|
||
largest_contour = max(contours, key=cv2.contourArea)
|
||
# 使用Douglas-Peucker算法适度简化，保持更多细节
|
||
epsilon = CONTOUR_EPSILON_FACTOR * cv2.arcLength(largest_contour, True) # 更小的epsilon保留更多细节
|
||
smoothed_contour = cv2.approxPolyDP(largest_contour, epsilon, True)
|
||
cv2.drawContours(vis, [smoothed_contour], -1, (0, 255, 255), STROKE_WIDTH)
|
||
|
||
# 记录 id，减少目标跳变
|
||
if det["ids"] and len(det["ids"]) > chosen_idx and det["ids"][chosen_idx] is not None:
|
||
locked_id = int(det["ids"][chosen_idx])
|
||
|
||
else:
|
||
# YOLOE 未就绪：提示并保持原画面（不阻塞前端）
|
||
draw_text_cn(vis, "YOLOE 未就绪，显示原始画面", (10, 100), font_size=22, color=(0, 215, 255))
|
||
|
||
# 选择面积最大的mask ←—— 这一行下面开始保留你的原代码
|
||
|
||
# 选择面积最大的mask
|
||
if candidate_masks:
|
||
# 按面积降序排序
|
||
candidate_masks.sort(key=lambda x: x['area'], reverse=True)
|
||
largest_mask_info = candidate_masks[0]
|
||
last_detected_mask = largest_mask_info['mask']
|
||
|
||
# 可选：在最大的物体上添加特殊标记
|
||
contours, _ = cv2.findContours(last_detected_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
|
||
if contours:
|
||
# 找到最大轮廓的中心
|
||
M = cv2.moments(contours[0])
|
||
if M["m00"] != 0:
|
||
cx = int(M["m10"] / M["m00"])
|
||
cy = int(M["m01"] / M["m00"])
|
||
# 在最大物体中心画一个圆圈标记
|
||
cv2.circle(vis, (cx, cy), 8, (0, 255, 0), 2)
|
||
cv2.circle(vis, (cx, cy), 12, (0, 255, 0), 1)
|
||
# 目标标签：保持就地标注
|
||
draw_text_cn(vis, "目标", (cx + 15, cy - 5), font_size=16, color=FRONTEND_COLORS["ok"], ui_hint=False)
|
||
|
||
# 显示检测信息
|
||
if len(candidate_masks) > 1:
|
||
draw_text_cn(vis, f"检测到{len(candidate_masks)}个物体，选择最大的（面积: {largest_mask_info['area']}）",
|
||
(10, H - 30), font_size=16, color=(255, 255, 0))
|
||
|
||
# 自动锁定逻辑
|
||
if detected_object and last_detected_mask is not None:
|
||
if auto_lock_start_time is None:
|
||
auto_lock_start_time = t_now
|
||
print(f"[AUTO] 检测到物体，选择最大的（面积: {np.sum(last_detected_mask)}），开始倒计时...")
|
||
#play_guidance_audio("检测到物体") # 添加这行
|
||
|
||
elapsed = t_now - auto_lock_start_time
|
||
remaining = auto_lock_delay - elapsed
|
||
|
||
if remaining > 0:
|
||
# 显示倒计时（移动到左下角，前端风格）
|
||
draw_text_cn(vis, f"检测到物体，{remaining:.1f}秒后自动锁定", (10, 100), font_size=16, color=FRONTEND_COLORS["text"], stroke=(0,0,0))
|
||
|
||
# 绘制锁定框 - 使用虚线框表示正在准备锁定
|
||
if last_detected_mask is not None:
|
||
contours, _ = cv2.findContours(last_detected_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
|
||
if contours:
|
||
# 找到最大轮廓
|
||
largest_contour = max(contours, key=cv2.contourArea)
|
||
# 简化轮廓
|
||
epsilon = CONTOUR_EPSILON_FACTOR * cv2.arcLength(largest_contour, True)
|
||
smoothed_contour = cv2.approxPolyDP(largest_contour, epsilon, True)
|
||
|
||
# 根据倒计时进度改变颜色亮度
|
||
progress = 1.0 - (remaining / auto_lock_delay)
|
||
color_intensity = int(100 + 155 * progress) # 从100到255
|
||
lock_color = (0, color_intensity, color_intensity) # 黄色渐亮
|
||
|
||
# 绘制虚线轮廓
|
||
pts = smoothed_contour.reshape(-1, 2)
|
||
for i in range(len(pts)):
|
||
pt1 = tuple(pts[i])
|
||
pt2 = tuple(pts[(i + 1) % len(pts)])
|
||
# 使用虚线效果（通过绘制短线段）
|
||
draw_dashed_line(vis, pt1, pt2, color=lock_color, thickness=3,
|
||
dash_length=15, gap_length=8)
|
||
else:
|
||
# 进入闪烁模式
|
||
print("[AUTO] 进入闪烁动画模式")
|
||
MODE = "FLASH"
|
||
flash_start_time = t_now
|
||
flash_mask = last_detected_mask.copy()
|
||
auto_lock_start_time = None
|
||
play_guidance_audio("检测到物体")
|
||
else:
|
||
# 没有检测到物体，重置计时器
|
||
if auto_lock_start_time is not None:
|
||
print("[AUTO] 物体丢失，重置倒计时")
|
||
auto_lock_start_time = None
|
||
last_detected_mask = None
|
||
draw_text_cn(vis, "分割中... 等待检测到物体", (10, 100), font_size=16, color=FRONTEND_COLORS["muted"])
|
||
|
||
elif MODE == "FLASH":
|
||
# 闪烁动画模式
|
||
if flash_start_time is not None and flash_mask is not None:
|
||
elapsed = t_now - flash_start_time
|
||
|
||
if elapsed < flash_duration:
|
||
# 计算渐入渐出效果
|
||
# 前0.3秒渐入，中间0.4秒保持，后0.3秒渐出
|
||
if elapsed < 0.3:
|
||
# 渐入阶段
|
||
alpha = elapsed / 0.3 * 0.8 # 0到0.8
|
||
elif elapsed < 0.7:
|
||
# 保持阶段
|
||
alpha = 0.8
|
||
else:
|
||
# 渐出阶段
|
||
alpha = (1.0 - elapsed) / 0.3 * 0.8 # 0.8到0
|
||
|
||
# 绘制闪烁的mask
|
||
colored = np.zeros_like(frame, dtype=np.uint8)
|
||
colored[flash_mask == 1] = flash_color
|
||
vis = cv2.addWeighted(vis, 1.0 - alpha, colored, alpha, 0)
|
||
|
||
# 绘制轮廓（固定粗细，颜色渐变）
|
||
contours, _ = cv2.findContours(flash_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
|
||
if contours:
|
||
# 轮廓颜色也跟随alpha变化
|
||
contour_color = tuple(int(c * (0.5 + alpha * 0.5)) for c in flash_color)
|
||
cv2.drawContours(vis, contours, -1, contour_color, STROKE_WIDTH + 1)
|
||
|
||
# 显示提示文字（左下角）
|
||
draw_text_cn(vis, "正在锁定目标...", (10, 100), font_size=18, color=FRONTEND_COLORS["accent"])
|
||
else:
|
||
# 闪烁结束，初始化光流追踪并进入居中引导模式
|
||
print("[AUTO] 闪烁结束，初始化光流追踪")
|
||
edge_mask = inner_offset_edge(flash_mask, offset_px=INNER_OFFSET_PX_LOCK, edge_dilate_px=EDGE_DILATE_PX)
|
||
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
|
||
pts = cv2.goodFeaturesToTrack(gray, mask=edge_mask, **FEATURE_PARAMS)
|
||
|
||
if pts is not None and len(pts) >= 8:
|
||
p0 = pts
|
||
old_gray = gray
|
||
MODE = "CENTER_GUIDE"
|
||
lock_edge_debug = edge_mask.copy()
|
||
track_frame_count = 0
|
||
center_guide_start = t_now
|
||
center_reached = False
|
||
flash_start_time = None
|
||
flash_mask = None
|
||
last_detected_mask = None
|
||
print(f"[LOCK] 内边界特征点数={len(p0)} → CENTER_GUIDE")
|
||
else:
|
||
print("[LOCK] 内边界特征点不足，返回检测模式")
|
||
MODE = "SEGMENT"
|
||
flash_start_time = None
|
||
flash_mask = None
|
||
last_detected_mask = None
|
||
|
||
elif MODE == "CENTER_GUIDE":
|
||
# 居中引导模式（使用光流追踪）
|
||
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
|
||
poly_center = None
|
||
poly_area = 0.0
|
||
|
||
if old_gray is not None and p0 is not None and len(p0) >= 5:
|
||
# 光流追踪
|
||
p1, st, err = cv2.calcOpticalFlowPyrLK(old_gray, gray, p0, None, **LK_PARAMS)
|
||
if p1 is not None and st is not None:
|
||
good_new = p1[st == 1]
|
||
if len(good_new) >= 5:
|
||
p0 = good_new.reshape(-1, 1, 2)
|
||
hull = cv2.convexHull(good_new.reshape(-1,1,2))
|
||
poly = hull.reshape(-1, 2)
|
||
|
||
if len(poly) >= 3:
|
||
H, W = frame.shape[:2]
|
||
|
||
# 把当前光流多边形 rasterize 成掩膜（便于与 YOLOE 掩膜做 IoU）
|
||
poly_mask = np.zeros((H, W), dtype=np.uint8)
|
||
cv2.fillPoly(poly_mask, [poly.astype(np.int32)], 1)
|
||
|
||
# 降频：每3帧用 YOLOE 重新检测，其余帧依赖光流维持
|
||
need_reseed = False
|
||
new_det_mask = None
|
||
|
||
if use_yoloe and yoloe_backend is not None and (FRAME_IDX % 3 == 0):
|
||
# 添加调试信息
|
||
if FRAME_IDX % 30 == 0: # 每30帧打印一次
|
||
print(f"[YOLOE] 实时检测第 {FRAME_IDX} 帧")
|
||
det = yoloe_backend.segment(frame, conf=0.20, iou=0.45, persist=True)
|
||
if det["masks"]:
|
||
# 取面积最大的那个
|
||
areas = [int(m.sum()) for m in det["masks"]]
|
||
j = int(np.argmax(areas))
|
||
m = det["masks"][j]
|
||
if m.shape[:2] != (H, W):
|
||
m = cv2.resize(m, (W, H), interpolation=cv2.INTER_NEAREST)
|
||
new_det_mask = (m > 0).astype(np.uint8)
|
||
|
||
# 和当前光流多边形的 IoU
|
||
inter = np.logical_and(new_det_mask, poly_mask).sum()
|
||
union = np.logical_or(new_det_mask, poly_mask).sum() + 1e-6
|
||
iou = inter / union
|
||
|
||
# IoU 太低，说明漂了：用 YOLOE 的掩膜重播种光流
|
||
# 降低阈值，让 YOLOE 更容易更新光流
|
||
if iou < 0.5: # 从 IOU_MIN_KEEP (0.20) 提高到 0.5
|
||
need_reseed = True
|
||
# 用新掩膜的「内边界特征点」播种
|
||
edge_mask = inner_offset_edge(new_det_mask, offset_px=INNER_OFFSET_PX_LOCK, edge_dilate_px=EDGE_DILATE_PX)
|
||
gray2 = gray # 本帧灰度图已在上面算过
|
||
pts = cv2.goodFeaturesToTrack(gray2, mask=edge_mask, **FEATURE_PARAMS)
|
||
if pts is not None and len(pts) >= 8:
|
||
p0 = pts
|
||
old_gray = gray2
|
||
# 更新 last_mask，便于下游逻辑一致
|
||
last_mask = new_det_mask.copy()
|
||
last_seen_ts = time.time()
|
||
flow_grace = 0
|
||
print("[RESEED] YOLOE 低 IoU 触发重播种（已更新光流特征点）")
|
||
|
||
# 如果这帧没重播种，但 YOLOE 有结果且与 poly 很接近，可以做一次"平滑融合"，抑制抖动
|
||
if (not need_reseed) and (new_det_mask is not None):
|
||
inter = np.logical_and(new_det_mask, poly_mask).sum()
|
||
union = np.logical_or(new_det_mask, poly_mask).sum() + 1e-6
|
||
iou = inter / union
|
||
# 降低融合阈值，让 YOLOE 结果更容易被采用
|
||
if iou < 0.95: # 从 0.90 提高到 0.95
|
||
# 增加 YOLOE 的权重，让实时检测更明显
|
||
poly_mask = ((0.8 * new_det_mask + 0.2 * poly_mask) > 0.5).astype(np.uint8)
|
||
# 用更新后的 poly_mask 回写到可视化与引导的后续变量（如果你下游用的是 last_detected_mask/last_mask）
|
||
last_mask = poly_mask.copy()
|
||
# 更新多边形轮廓，让可视化实时更新
|
||
contours, _ = cv2.findContours(poly_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
|
||
if contours:
|
||
# 找到最大轮廓
|
||
largest_contour = max(contours, key=cv2.contourArea)
|
||
# 使用精细的轮廓处理，保留更多细节
|
||
epsilon = TRACK_EPSILON_FACTOR * cv2.arcLength(largest_contour, True)
|
||
poly = cv2.approxPolyDP(largest_contour, epsilon, True).reshape(-1, 2)
|
||
# 注释掉凸包处理，保留原始轮廓细节
|
||
# hull = cv2.convexHull(poly.reshape(-1,1,2))
|
||
# poly = hull.reshape(-1, 2)
|
||
# 重新计算特征点
|
||
edge_mask = inner_offset_edge(poly_mask, offset_px=INNER_OFFSET_PX_LOCK, edge_dilate_px=EDGE_DILATE_PX)
|
||
pts = cv2.goodFeaturesToTrack(gray, mask=edge_mask, **FEATURE_PARAMS)
|
||
if pts is not None and len(pts) >= 5:
|
||
p0 = pts
|
||
|
||
# 绘制追踪的多边形 - 使用更粗的线条
|
||
cv2.polylines(vis, [poly.astype(np.int32)], isClosed=True, color=(0,255,255), thickness=STROKE_WIDTH)
|
||
|
||
# 计算多边形中心
|
||
poly_center, poly_area = polygon_center_and_area(poly)
|
||
|
||
if poly_center:
|
||
object_center = (int(poly_center[0]), int(poly_center[1]))
|
||
|
||
# 画面中心
|
||
frame_center = (W // 2, H // 2)
|
||
|
||
# 绘制物品中心点
|
||
cv2.circle(vis, object_center, 8, (0, 255, 0), -1)
|
||
cv2.circle(vis, object_center, 12, (0, 255, 0), 2)
|
||
|
||
# 绘制画面中心十字
|
||
cv2.line(vis, (frame_center[0] - 20, frame_center[1]),
|
||
(frame_center[0] + 20, frame_center[1]), (255, 255, 255), 2)
|
||
cv2.line(vis, (frame_center[0], frame_center[1] - 20),
|
||
(frame_center[0], frame_center[1] + 20), (255, 255, 255), 2)
|
||
|
||
# 绘制引导虚线
|
||
draw_dashed_line(vis, object_center, frame_center,
|
||
color=(255, 255, 0), thickness=2,
|
||
dash_length=10, gap_length=5)
|
||
|
||
# 获取引导方向
|
||
direction, is_centered = get_center_guidance(object_center, frame_center, center_threshold)
|
||
|
||
if not center_reached:
|
||
if is_centered:
|
||
# 到达中心，播放OK音效
|
||
center_reached = True
|
||
last_center_guide_time = t_now
|
||
play_guidance_audio("OK")
|
||
try:
|
||
bridge_io.send_ui_final("✓ 物品已居中！")
|
||
except Exception:
|
||
pass
|
||
draw_text_cn(vis, "✓ 物品已居中！", (10, 60), font_size=18, color=FRONTEND_COLORS["ok"])
|
||
else:
|
||
# 显示引导文字
|
||
msg = f"请将物品移到画面中心: {direction}"
|
||
try:
|
||
# 节流：每次语音播报也推一次final
|
||
if t_now - last_center_guide_time > GUIDANCE_INTERVAL_SEC:
|
||
bridge_io.send_ui_final(msg)
|
||
except Exception:
|
||
pass
|
||
draw_text_cn(vis, msg,
|
||
(10, 40), font_size=18, color=FRONTEND_COLORS["text"])
|
||
|
||
# 显示距离信息
|
||
dx = frame_center[0] - object_center[0]
|
||
dy = frame_center[1] - object_center[1]
|
||
distance = int(np.sqrt(dx**2 + dy**2))
|
||
draw_text_cn(vis, f"距离: {distance}px",
|
||
(10, 60), font_size=16, color=FRONTEND_COLORS["muted"])
|
||
|
||
# 播放语音引导
|
||
if t_now - last_center_guide_time > GUIDANCE_INTERVAL_SEC:
|
||
play_guidance_audio(direction)
|
||
last_center_guide_time = t_now
|
||
else:
|
||
# 已经居中，显示成功信息
|
||
try:
|
||
bridge_io.send_ui_final("✓ 物品已成功移到中心！")
|
||
except Exception:
|
||
pass
|
||
draw_text_cn(vis, "✓ 物品已成功移到中心！",
|
||
(10, 60), font_size=18, color=FRONTEND_COLORS["ok"])
|
||
|
||
# 等待1秒后进入手部追踪模式
|
||
if t_now - last_center_guide_time > 1.0:
|
||
print("[CENTER] 进入手部追踪模式")
|
||
try:
|
||
bridge_io.send_ui_final("进入手部追踪模式")
|
||
except Exception:
|
||
pass
|
||
MODE = "TRACK"
|
||
# 保持当前的光流追踪状态
|
||
else:
|
||
# 多边形中心计算失败，显示警告
|
||
draw_text_cn(vis, "正在追踪物体...", (10, 100), font_size=20, color=(255, 255, 0))
|
||
else:
|
||
# 光流点数不足，尝试重新检测
|
||
MODE = "SEGMENT"
|
||
old_gray = None
|
||
p0 = None
|
||
print("[CENTER] 光流追踪失败，返回检测模式")
|
||
|
||
old_gray = gray
|
||
|
||
else: # MODE == "TRACK"
|
||
# 手部追踪模式（原有逻辑保持不变）
|
||
align_score = 0.0
|
||
range_score = 0.0
|
||
ratio = None
|
||
|
||
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
|
||
track_frame_count += 1
|
||
|
||
relock_done = False
|
||
poly_center = None
|
||
poly_area = 0.0
|
||
|
||
# 初始化camera_movement为默认值
|
||
camera_movement = np.array([0.0, 0.0])
|
||
|
||
# 初始化或更新背景参考点（在物体多边形外部取点）
|
||
if background_points is None or track_frame_count % 30 == 0:
|
||
# 在画面四角取一些背景特征点
|
||
mask_for_bg = np.ones((H, W), dtype=np.uint8) * 255
|
||
if last_poly_box:
|
||
x, y, w, h = last_poly_box
|
||
# 扩大区域，排除物体和手
|
||
expand = 100
|
||
x1 = max(0, x - expand)
|
||
y1 = max(0, y - expand)
|
||
x2 = min(W, x + w + expand)
|
||
y2 = min(H, y + h + expand)
|
||
mask_for_bg[y1:y2, x1:x2] = 0
|
||
|
||
# 在背景区域提取特征点
|
||
try:
|
||
bg_pts = cv2.goodFeaturesToTrack(gray, maxCorners=20,
|
||
qualityLevel=0.1,
|
||
minDistance=30,
|
||
mask=mask_for_bg)
|
||
if bg_pts is not None and len(bg_pts) >= 5:
|
||
background_points = bg_pts
|
||
old_background_gray = gray.copy()
|
||
except Exception as e:
|
||
#print(f"[TRACK] 背景特征点提取失败: {e}")
|
||
background_points = None
|
||
|
||
# 计算背景移动（相机移动）
|
||
if old_background_gray is not None and background_points is not None and len(background_points) > 0:
|
||
try:
|
||
bg_p1, bg_st, _ = cv2.calcOpticalFlowPyrLK(
|
||
old_background_gray, gray, background_points, None, **LK_PARAMS
|
||
)
|
||
if bg_p1 is not None and bg_st is not None:
|
||
good_bg_old = background_points[bg_st == 1]
|
||
good_bg_new = bg_p1[bg_st == 1]
|
||
if len(good_bg_new) >= 3 and len(good_bg_old) >= 3:
|
||
# 计算背景的平均移动
|
||
bg_movement = np.mean(good_bg_new - good_bg_old, axis=0)
|
||
camera_movement = bg_movement.reshape(2)
|
||
background_points = good_bg_new.reshape(-1, 1, 2)
|
||
old_background_gray = gray.copy()
|
||
except Exception as e:
|
||
print(f"[TRACK] 背景光流计算失败: {e}")
|
||
camera_movement = np.array([0.0, 0.0])
|
||
|
||
if old_gray is not None and p0 is not None and len(p0) >= 5:
|
||
p1, st, err = cv2.calcOpticalFlowPyrLK(old_gray, gray, p0, None, **LK_PARAMS)
|
||
if p1 is not None and st is not None:
|
||
good_new = p1[st == 1]
|
||
if len(good_new) >= 5:
|
||
p0 = good_new.reshape(-1, 1, 2)
|
||
hull = cv2.convexHull(good_new.reshape(-1,1,2))
|
||
poly = hull.reshape(-1, 2)
|
||
|
||
if len(poly) >= 3:
|
||
# 统一的 YOLOE 实时检测和校正（每帧）
|
||
latest_det_mask = None
|
||
if use_yoloe and yoloe_backend is not None:
|
||
# 添加调试信息
|
||
if track_frame_count % 30 == 0: # 每30帧打印一次
|
||
print(f"[YOLOE] TRACK模式实时检测第 {track_frame_count} 帧")
|
||
|
||
# YOLOE 实时检测（统一调用，避免重复）
|
||
det = yoloe_backend.segment(frame, conf=YOLO_CORRECTION_CONF_THRESHOLD, iou=0.45, persist=True)
|
||
if det["masks"]:
|
||
# 取面积最大的那个
|
||
areas = [int(m.sum()) for m in det["masks"]]
|
||
j = int(np.argmax(areas))
|
||
m = det["masks"][j]
|
||
if m.shape[:2] != (H, W):
|
||
m = cv2.resize(m, (W, H), interpolation=cv2.INTER_NEAREST)
|
||
latest_det_mask = (m > 0).astype(np.uint8)
|
||
|
||
# 和当前光流多边形的 IoU
|
||
poly_mask = np.zeros((H, W), dtype=np.uint8)
|
||
cv2.fillPoly(poly_mask, [poly.astype(np.int32)], 1)
|
||
inter = np.logical_and(latest_det_mask, poly_mask).sum()
|
||
union = np.logical_or(latest_det_mask, poly_mask).sum() + 1e-6
|
||
iou = inter / union
|
||
|
||
# 降低IoU阈值，更积极地校正
|
||
if iou > YOLO_CORRECTION_IOU_THRESHOLD: # 使用可配置阈值
|
||
# 用 YOLOE 结果更新多边形
|
||
contours, _ = cv2.findContours(latest_det_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
|
||
if contours:
|
||
largest_contour = max(contours, key=cv2.contourArea)
|
||
# 使用更精细的轮廓处理，减少过度简化
|
||
epsilon = TRACK_EPSILON_FACTOR * cv2.arcLength(largest_contour, True)
|
||
poly = cv2.approxPolyDP(largest_contour, epsilon, True).reshape(-1, 2)
|
||
|
||
# 更新光流特征点
|
||
edge_mask = inner_offset_edge(latest_det_mask, offset_px=INNER_OFFSET_PX_LOCK, edge_dilate_px=EDGE_DILATE_PX)
|
||
pts = cv2.goodFeaturesToTrack(gray, mask=edge_mask, **FEATURE_PARAMS)
|
||
if pts is not None and len(pts) >= 5:
|
||
p0 = pts
|
||
#print(f"[TRACK] YOLOE 实时校正，IoU: {iou:.3f}")
|
||
|
||
# 检查是否接触，决定轮廓颜色
|
||
is_touching = False
|
||
overlap_ratio = 0.0
|
||
if hand_box is not None and poly is not None:
|
||
is_touching, overlap_ratio = check_hand_object_contact(hand_box, poly, overlap_threshold=0.1)
|
||
|
||
# 绘制多边形（可能已被 YOLOE 更新）- 使用更粗的线条
|
||
if is_touching:
|
||
# 接触时用亮绿色，并添加发光效果
|
||
poly_color = (0, 255, 127)
|
||
# 绘制一个更粗的外层轮廓作为发光效果
|
||
cv2.polylines(vis, [poly.astype(np.int32)], isClosed=True,
|
||
color=(127, 255, 127), thickness=STROKE_WIDTH + 4)
|
||
# 添加半透明的填充效果
|
||
overlay = vis.copy()
|
||
cv2.fillPoly(overlay, [poly.astype(np.int32)], (0, 255, 0))
|
||
cv2.addWeighted(overlay, 0.15, vis, 0.85, 0, vis)
|
||
else:
|
||
# 未接触时用普通绿色
|
||
poly_color = (0, 255, 0)
|
||
cv2.polylines(vis, [poly.astype(np.int32)], isClosed=True, color=poly_color, thickness=STROKE_WIDTH)
|
||
# 多边形质心与面积
|
||
poly_center, poly_area = polygon_center_and_area(poly)
|
||
if poly_center:
|
||
pc = (int(poly_center[0]), int(poly_center[1]))
|
||
cv2.circle(vis, pc, 6, (0,255,0), -1)
|
||
|
||
# 多边形外接矩形（用于周边监控）
|
||
x, y, w, h = cv2.boundingRect(poly.astype(np.int32))
|
||
last_poly_box = (x, y, w, h)
|
||
|
||
# ====== 对齐分数（第一条）======
|
||
if hand_center and poly_center:
|
||
hc = np.array(hand_center, dtype=np.float32)
|
||
oc = np.array(poly_center, dtype=np.float32)
|
||
dist = float(np.linalg.norm(oc - hc))
|
||
diag = float(np.linalg.norm([W, H]))
|
||
align_score = 1.0 - min(dist/(ALIGN_LOOSE_PCT*diag + 1e-6), 1.0)
|
||
|
||
# 绘制虚线引导（替代原来的实线箭头）
|
||
draw_dashed_line(vis, (hc[0], hc[1]), (oc[0], oc[1]),
|
||
color=(255, 255, 0), thickness=2,
|
||
dash_length=15, gap_length=10)
|
||
|
||
# 方向引导
|
||
direction, secondary = get_guidance_direction(
|
||
hand_center, poly_center, hand_area, poly_area,
|
||
hand_box, poly
|
||
)
|
||
|
||
if direction and direction != "保持":
|
||
# 根据是否接触显示不同颜色
|
||
if direction == "向前":
|
||
# 手已经接触物体，用绿色显示
|
||
guide_color = (0, 255, 0) # 绿色
|
||
draw_text_cn(vis, f"引导: {direction} - 伸手抓取", (W//2 - 80, 40),
|
||
font_size=24, color=guide_color, stroke=(0, 0, 0))
|
||
else:
|
||
# 还未接触，用黄色显示
|
||
guide_color = (0, 255, 255) # 黄色
|
||
draw_text_cn(vis, f"引导: {direction}", (W//2 - 60, 40),
|
||
font_size=24, color=guide_color, stroke=(0, 0, 0))
|
||
|
||
# 显示次要信息（接触度或其他方向）
|
||
if secondary:
|
||
if isinstance(secondary, str):
|
||
# 接触度信息
|
||
draw_text_cn(vis, secondary, (W//2 - 60, 70),
|
||
font_size=18, color=(0, 255, 0))
|
||
else:
|
||
# 其他方向信息
|
||
draw_text_cn(vis, f"（或 {secondary}）", (W//2 - 60, 70),
|
||
font_size=18, color=(200, 200, 200))
|
||
|
||
# 播放语音引导 - 确保每个方向都会播放
|
||
if t_now - last_guidance_time > GUIDANCE_INTERVAL_SEC:
|
||
# 检查方向是否改变，或者时间间隔足够
|
||
if direction != last_guidance_direction or t_now - last_guidance_time > GUIDANCE_INTERVAL_SEC * 2:
|
||
play_guidance_audio(direction)
|
||
last_guidance_direction = direction
|
||
last_guidance_time = t_now
|
||
print(f"[GUIDE] 播放引导音频: {direction}")
|
||
else:
|
||
align_score = 0.0
|
||
|
||
# 显示接触状态
|
||
is_touching, overlap_ratio = check_hand_object_contact(hand_box, poly, overlap_threshold=0.1)
|
||
if is_touching:
|
||
draw_text_cn(vis, f"状态: 已接触 ({overlap_ratio:.1%})", (10, 95),
|
||
font_size=16, color=(0, 255, 0))
|
||
else:
|
||
# 计算手和物体的距离
|
||
if hand_center and poly_center:
|
||
distance = np.sqrt((hand_center[0] - poly_center[0])**2 +
|
||
(hand_center[1] - poly_center[1])**2)
|
||
draw_text_cn(vis, f"距离: {distance:.0f}px", (10, 95),
|
||
font_size=16, color=FRONTEND_COLORS["muted"])
|
||
|
||
# 成功条件：握持（放宽）
|
||
if (_last_result and _last_result[0].hand_landmarks and len(_last_result[0].hand_landmarks) > 0):
|
||
l0 = _last_result[0].hand_landmarks[0]
|
||
grasp_now, grasp_score = detect_grasp(l0, W, H)
|
||
else:
|
||
grasp_now, grasp_score = False, 0.0
|
||
|
||
# guidance_msg 相关代码已经集成到上面的引导逻辑中
|
||
|
||
# ===== 周边监控 & 重新锁定（复用YOLO结果）=====
|
||
if (track_frame_count % PERI_CHECK_EVERY == 0) and (last_poly_box is not None) and (latest_det_mask is not None):
|
||
# 直接使用刚才的YOLO检测结果，避免重复调用
|
||
px, py, pw, ph = last_poly_box
|
||
x0 = max(0, px - PERI_MONITOR_PX)
|
||
y0 = max(0, py - PERI_MONITOR_PX)
|
||
x1 = min(W - 1, px + pw + PERI_MONITOR_PX)
|
||
y1 = min(H - 1, py + ph + PERI_MONITOR_PX)
|
||
|
||
# 检查周边区域是否有更好的检测结果
|
||
peri_area = latest_det_mask[y0:y1, x0:x1].sum()
|
||
total_area = latest_det_mask.sum()
|
||
|
||
# 如果周边区域有显著检测结果，重新锁定
|
||
if peri_area > total_area * 0.1: # 周边有10%以上的检测面积
|
||
edge_mask = inner_offset_edge(latest_det_mask, offset_px=INNER_OFFSET_PX_LOCK, edge_dilate_px=EDGE_DILATE_PX)
|
||
pts = cv2.goodFeaturesToTrack(gray, mask=edge_mask, **FEATURE_PARAMS)
|
||
if pts is not None and len(pts) >= 8:
|
||
p0 = pts
|
||
old_gray = gray
|
||
lock_edge_debug = edge_mask.copy()
|
||
#print(f"[PERI] 周边重锁定，特征点数={len(p0)}")
|
||
else:
|
||
MODE = "SEGMENT"; old_gray = None; p0 = None; lock_edge_debug = None
|
||
else:
|
||
MODE = "SEGMENT"; old_gray = None; p0 = None; lock_edge_debug = None
|
||
else:
|
||
MODE = "SEGMENT"; old_gray = None; p0 = None; lock_edge_debug = None
|
||
else:
|
||
MODE = "SEGMENT"; old_gray = None; p0 = None; lock_edge_debug = None
|
||
|
||
|
||
|
||
if MODE == "SEGMENT":
|
||
draw_text_cn(vis, "追踪丢失 → 正在重新识别。按 Enter 重新锁定", (10, 100), font_size=22, color=(0,0,255))
|
||
|
||
old_gray = gray
|
||
|
||
# FPS（移动到左下角样式）
|
||
if 'fps_hist' not in locals():
|
||
fps_hist = []
|
||
fps_hist.append(t_now)
|
||
if len(fps_hist) > 30:
|
||
fps_hist.pop(0)
|
||
fps = 0.0 if len(fps_hist) < 2 else (len(fps_hist)-1)/(fps_hist[-1]-fps_hist[0])
|
||
draw_text_cn(vis, f"FPS: {fps:.1f}", (10, 40), font_size=16, color=FRONTEND_COLORS["ok"])
|
||
|
||
# 右下角显示"内边界/最近一次锁定"的调试图
|
||
if lock_edge_debug is not None:
|
||
# 极小缩放并放在右下角
|
||
small = cv2.resize(lock_edge_debug, (0,0), fx=0.22, fy=0.22, interpolation=cv2.INTER_NEAREST)
|
||
sh, sw = small.shape[:2]
|
||
small_bgr = cv2.cvtColor(small, cv2.COLOR_GRAY2BGR)
|
||
# 右下角位置，留 10-12px 边距
|
||
x1 = max(8, W - sw - 12)
|
||
y1 = max(8, H - sh - 12)
|
||
y2 = y1 + sh
|
||
x2 = x1 + sw
|
||
vis[y1:y2, x1:x2] = small_bgr
|
||
# 标签置于图上方紧贴，使用更小字号
|
||
#draw_text_cn(vis, "内边界", (x1, y1 - 8), font_size=12, color=FRONTEND_COLORS["muted"], ui_hint=False)
|
||
|
||
# 底部中间的"当前指令"按钮（始终绘制，文案随音频同步）
|
||
draw_command_pill(vis, CURRENT_COMMAND_TEXT)
|
||
|
||
# 展示（无论 headless 与否，都会推给前端）
|
||
bridge_io.send_vis_bgr(vis)
|
||
|
||
# [headless] 只有非 headless 时才弹窗与键盘交互；headless 下用 waitKey(1) 让出调度
|
||
if not headless:
|
||
cv2.imshow(WINDOW, vis)
|
||
key = cv2.waitKey(1) & 0xFF
|
||
if key in (27, ord('q')):
|
||
break
|
||
elif key == ord('r'):
|
||
MODE = "SEGMENT"; old_gray = None; p0 = None; lock_edge_debug = None
|
||
elif key == 13: # Enter：从 SEGMENT 锁定并开始 TRACK（内收 5px）
|
||
if MODE == "SEGMENT":
|
||
# 使用 YOLOE 进行手动锁定
|
||
if use_yoloe and yoloe_backend is not None:
|
||
det = yoloe_backend.segment(frame, conf=CONF_THRESHOLD, iou=0.45, persist=True)
|
||
if det["masks"]:
|
||
# 取面积最大的那个
|
||
areas = [int(m.sum()) for m in det["masks"]]
|
||
j = int(np.argmax(areas))
|
||
m = det["masks"][j]
|
||
if m.shape[:2] != (H, W):
|
||
m = cv2.resize(m, (W, H), interpolation=cv2.INTER_NEAREST)
|
||
best_mask = (m > 0.5).astype(np.uint8)
|
||
else:
|
||
best_mask = None
|
||
else:
|
||
best_mask = None
|
||
if best_mask is not None:
|
||
edge_mask = inner_offset_edge(best_mask, offset_px=INNER_OFFSET_PX_LOCK, edge_dilate_px=EDGE_DILATE_PX)
|
||
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
|
||
pts = cv2.goodFeaturesToTrack(gray, mask=edge_mask, **FEATURE_PARAMS)
|
||
if pts is not None and len(pts) >= 8:
|
||
p0 = pts
|
||
old_gray = gray
|
||
MODE = "TRACK"
|
||
lock_edge_debug = edge_mask.copy()
|
||
track_frame_count = 0
|
||
print(f"[LOCK] 内边界特征点数={len(p0)} → TRACK")
|
||
else:
|
||
print("[LOCK] 内边界特征点不足，请调整画面后重试。")
|
||
else:
|
||
print("[LOCK] 当前帧未找到有效分割，请重试。")
|
||
else:
|
||
# headless 下也调用一次 waitKey(1)，让 OpenCV 的计时器/回调得到机会，且避免 CPU 忙等
|
||
cv2.waitKey(1)
|
||
|
||
# 在 headless 模式下检查停止事件
|
||
if stop_event and stop_event.is_set():
|
||
print("[YOLOMEDIA] Received stop signal in headless mode")
|
||
break
|
||
|
||
finally:
|
||
try:
|
||
landmarker.close()
|
||
except Exception:
|
||
pass
|
||
#cap.release()
|
||
# [headless] 仅在非 headless 时销毁窗口
|
||
if not headless:
|
||
cv2.destroyAllWindows()
|
||
|
||
|
||
if __name__ == "__main__":
|
||
main()
|